扩展卡尔曼滤波EKF

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

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

3、1| k)H (k 1) R(k 1)(3-1-5)兄(K 1| k 1) =X(K 1| k) K(k 1)Z(k 1)-h(X(K 1| k)(3-1-6)P(k 1)=I-K(k 1)H(k1)P(k 1| k)( 3-1-7)这里需要重要说明的是，状态转移"(k 1|k)和量测矩阵H(k /)是由f和h的雅克比矩阵代替的。其雅克比矩阵的求法如下：假如状态变量有n维，即X = x1 x2 . xn ，则对状态方程对各维求偏导，精彩文档：(k 1)=dx(3-1-8 )f +亘+旦+.+旦:X .x2_:x3_:xnH(k 1).:h:X_:h jh jh：h t + +* _X

4、- X?_ X3_ Xn(3-1-9 )3.2扩展卡尔曼在一维非线性系统中的应用状态方程和观测方程都为非线性的通用系统所谓的非线性方程，就是因变量和自变量的关系不是线性的，这类方程很多, 例如平方关系，对数关系，指数关系，三角函数关系等等。这类方程可分为两类, 一类是多项式方程，一种是非多项式方程。为了便于说明非线性卡尔曼滤波一一 扩展Kalman滤波的原理，我们选用一下系统，系统状态为X(k)，它仅包含一维变量，即X(k)二x(k)，系统状态方程为2.5X(k1)/c “、X(k) =0.5X(k -1)28cos(1.2k) w(k) ( 3-2-1)1 +X2(k-1)观测方程为2Y(k

5、)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滤波来处理噪声问题。第一步：初始化状态X (0),Y(0)，协方差矩阵P0第二步：状态预测丄 2.5X(k1)丄/c c c、X(k|k 1) =0.5X(k -1)28cos(1.2k)(3-2-3

6、)1+X2(k-1)第三步：观测预测Y(k|k-1)二2X (k|k-1)20(3-2-4)第四步：一阶线性化状态方程，求解状态转移矩阵G(k)= 0.52.51_X2(k|k_1)221 X (k|k 1)(3-2-5)第五步：一阶线性化观测方程，求解观测矩阵 H(k)H(k).:hXX(k |k-1)10(3-2-6)第六步：求协方差矩阵预测P(k|k 1)晻P(k|k-1) = G(k)P(k-1|k-1)晻(k)丨 Q( 3-2-7)这里需要说明的是，当噪声驱动矩阵不存在的时候,或系统状态方程中，在w(k)前没有任何驱动矩阵，这时候，Q必然和状态的维数一样的方阵，可将式(3-2-7 )直接写为 P(k |k-1) =(k)P(k-1|k-1)：？(k) Q第七步：求Kalman增益K(k)二 P(k|k-1)H 晻(k)(H(k)P(k |k-1)H (k) R) (3-2-8)第八步：求状态更新X(k) =X(k |k-1) K(Y(k) -Y(k |k-1)(3-2-9)第九步：协方差更新P(k) =(ln-K(k)H(k)P(k |k-1)(3-2-10)以上九步为扩展卡