扩展卡尔曼滤波EKF

1、 实用标准文案第三章 扩展卡尔曼滤波 ekf3.1 扩展 kalman 滤波原理kalman 滤波能够在线性高斯模型的条件下，可以对目标的状态做出最优的估计，得到较好的跟踪效果。对非线性滤波问题常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题。因此，可以利用非线性函数的局部性特性，将非线性模型局部化，再利用 kalman 滤波算法完成滤波跟踪。扩展 kalman滤波就是基于这样的思想，将系统的非线性函数做一阶 taylor 展开，得到线性化的系统方程从而完成对目标的滤波估计等处理。非线性系统离散动态方程可以表示为x (k+1) = fk,x(k) + g(k) w(k)z(k)

2、= hk,x(k) + v(k)（3-1-1）（3-1-2）这里为了便于数学处理，假定没有控制量的输入，并假定过程噪声是均值为零的高斯白噪声，且噪声分布矩阵 g(k) 是已知的。其中，观测噪声v(k) 也是加性均值为零的高斯白噪声。假定过程噪声和观测噪声序列是彼此独立的，并且有初始状态估计 x (0 | 0) 和协方差矩阵 p(0 | 0)。和线性系统的情况一样，我们可以得到扩展 kalman 滤波算法如下x (k | k+1) = f(x(k | k)（3-1-3）（3-1-4）p(k+1| k) = f(k+1| k) p(k | k)f (k+1| k) + q(k+1)（3-1-5）k

3、(k+1) = p(k+1| k)h (k+1)h(k+1)p(k+1| k) h (k+1)+ r(k+1)-1晻x (k+1| k+1) = x(k+1| k) + k(k+1)z(k+1)- h(x(k+1| k)（3-1-6）（3-1-7）p(k+1) = i- k(k+1) h(k+1)p(k+1| k)精彩文档 实用标准文案这里需要重要说明的是，状态转移 f(k+1| )和量测矩阵 (k+1) 是由 和khf的雅克比矩阵代替的。其雅克比矩阵的求法如下：h假如状态变量有 n 维，即x = x x . x ，则对状态方程对各维求偏导，12nf f f ff（3-1-8）（3-1-9）f

4、(k+1) =+.+x x x xx123nh h h hhh(k+1) =+.+x x x xx123n3.2 扩展卡尔曼在一维非线性系统中的应用3.2.1 状态方程和观测方程都为非线性的通用系统所谓的非线性方程，就是因变量和自变量的关系不是线性的，这类方程很多，例如平方关系，对数关系，指数关系，三角函数关系等等。这类方程可分为两类，一类是多项式方程，一种是非多项式方程。为了便于说明非线性卡尔曼滤波扩展 kalman 滤波的原理，我们选用一下系统，系统状态为 x (k)，它仅包含一维变量，即 x (k) = x(k)，系统状态方程为2.5x (k-1)（3-2-1）x (k) = 0.5x(

5、k -1)+8cos(1.2k) + w(k)1+ x (k-1)2观测方程为x (k)2y(k) =+ v(k)（3-2-2）20其中，式（3-1-1）是包含分式，平方，三角函数在内的严重非线性的方程，w(k)为过程噪声，其均值为 0，方差为q ，观测方程中，观测信号y(k)与状态 x (k)的关系也是非线性的， v(k) 也是均值为 0，方差为 r 的高斯白噪声。因此关于（3-1-1）和（3-2-2）是一个状态和观测都为非线性的一维系统。以此为通用的非线性方程的代表，接下来讲述如何用扩展 kalman 滤波来处理噪声问题。精彩文档 实用标准文案第一步：初始化状态 (0),y(0) ，协方差

6、矩阵x。p0第二步：状态预测2.5x (k-1)（3-2-3）（3-2-4）x (k | k-1) = 0.5x(k -1)+8cos(1.2k)1+ x (k-1)2第三步：观测预测x (k | k-1)2y(k | k-1) =20第四步：一阶线性化状态方程，求解状态转移矩阵f(k)f2.51- x (k | k-1)2（3-2-5）f(k) = 0.5+x1+ x (k | k-1)22第五步：一阶线性化观测方程，求解观测矩阵 (k)hh x (k | k-1)（3-2-6）（3-2-7）h(k) =x10第六步：求协方差矩阵预测 (k | k-1)pp(k | k-1) = f(k)

7、p(k-1| k-1)f (k) + gqg晻这里需要说明的是，当噪声驱动矩阵不存在的时候，或系统状态方程中，在w(k) 前没有任何驱动矩阵，这时候， q 必然和状态的维数一样的方阵，可将式（3-2-7）直接写为 p(k | k-1) = f(k) p(k-1| k-1)f (k) + q。第七步：求 kalman 增益（3-2-8）（3-2-9）k(k) = p(k | k-1) h (k)(h(k) p(k | k-1) h (k) + r)晻第八步：求状态更新x (k) = x(k | k-1)+ k(y(k) - y(k | k-1)第九步：协方差更新p(k) = (i - k(k) h(k) p(k | k-1)（3-2-10）n以上九步为扩展卡尔曼年滤波的一个计算周