《扩展卡尔曼滤波EKF》由会员分享，可在线阅读，更多相关《扩展卡尔曼滤波EKF（4页珍藏版）》请在金锄头文库上搜索。
1、第三章 扩展卡尔曼滤波 EKF3.1 扩展 Kalman 滤波原理Kalman 滤波能够在线性高斯模型的条件下，可以对目标的状态做出最优的 估计，得到较好的跟踪效果。对非线性滤波问题常用的处理方法是利用线性化技 巧将其转化为一个近似的线性滤波问题。因此，可以利用非线性函数的局部性特 性，将非线性模型局部化，再利用Kalman滤波算法完成滤波跟踪。扩展Kalman 滤波就是基于这样的思想，将系统的非线性函数做一阶Taylor展开，得到线性 化的系统方程从而完成对目标的滤波估计等处理。非线性系统离散动态方程可以表示为X (k +1) = fk,X(k) + G(k)W(k)3-1一1Z (k) =
2、 hk,X(k) + V(k) 3-1-2这里为了便于数学处理，假定没有控制量的输入，并假定过程噪声是均值为零的 高斯白噪声，且噪声分布矩阵G(k)是的。其中，观测噪声V(k)也是加性均值为 零的高斯白噪声。假定过程噪声和观测噪声序列是彼此独立的，并且有初始状态 估计X(01 0)和协方差矩阵P(010)。和线性系统的情况一样，我们可以得到扩展 Kalman 滤波算法如下X (klk +1) = f(X (k I k) 3-1-3P(k + 1Ik)二(k + 1I k)P(k I k)丁 (k + 1I k) + Q(k +1)3-1-4K(k +1)二 P(k + 1I k)H 丁 (k
3、+ 1)H(k+ 1)P(k + 1I k)H 丁 (k +1) + R(k +1)卜i3-1-5X (K + 1I k +1) = X(K + 1Ik) + K(k + 1)Z(k+1) - h(X(K+1I k)3-1一6P(k +1) = I- K(k + 1)H(k+1)P(k + 1I k)3-1-7这里需要重要说明的是，状态转移(k + 1I k)和量测矩阵H(k +1)是由f和h的雅克比矩阵代替的。其雅克比矩阵的求法如下：假设状态变量有n维，即X二x x . x ，如此对状态方程对各维求偏1 2 n导，(k+1) = f 旦 + f + f +. + f3-1-8dx dxdxd
4、xdx1 2 3 ndhdhdhdhdhH (k +1) =+. +3-1-9dXdxdxdxdx1 2 3 n3.2 扩展卡尔曼在一维非线性系统中的应用3.2.1 状态方程和观测方程都为非线性的通用系统所谓的非线性方程，就是因变量和自变量的关系不是线性的，这类方程很多 例如平方关系，对数关系，指数关系，三角函数关系等等。这类方程可分为两类 一类是多项式方程，一种是非多项式方程。为了便于说明非线性卡尔曼滤波 扩展 Kalman 滤波的原理，我们选用一下系统，系统状态为X (k)，它仅包含一维变量，即X(k)二x(k)，系统状态方程为X (k) = 0.5X(k -1)+2.5 X (k -1)
5、1 + X 2(k -1)+ 8cos( 1 . 2k) +w(k)3-2-1观测方程为3-2-2Y (k)=竽 + V(k)其中，式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。 第二步：状态预测X (klk -1)
6、= 0.5X(k -1) + 2.5 X(k -1 + 8cos(1.2k) 3-2-31+X2(k-1)第三步：观测预测Y (klk -1)=X2(k|k-1)3-2-4第四步：一阶线性化状态方程，求解状态转移矩阵(k)= 0.5* 2.51 -X2(k|k-1)ng6X1+ X2(klk -1)2第五步：一阶线性化观测方程，求解观测矩阵 H (k)H(k)旦=X(klk一 1)3-2-60X10第六步：求协方差矩阵预测P (klk -1)P(k l k-1)二(k)P(k - 1l k-1) 丁 (k) + 厂0厂丁 3-2-7这里需要说明的是，当噪声驱动矩阵不存在的时候，或系统状态方程中
7、，在w(k)前没有任何驱动矩阵，这时候，Q必然和状态的维数一样的方阵，可将式3-2-7 丨直接写为 P(klk-1)二(k)P(k - 1l k-1) 丁 (k) + Q。第七步：求 Kalman 增益K(k)二 P(k l k-l)Hk)(H(k)P(k l k-l)H,k) + R) 3-2-8第八步：求状态更新X (k) = X(klk -1) + K(Y(k) - Y(k lk -1) 3-2-9第九步：协方差更新P(k)二(I -K(k)H(k)P(k lk-1)3-2-10n以上九步为扩展卡尔曼年滤波的一个计算周期，如此循环下去就是各个时刻 EKF 对非线性系统的处理过程。其他参数设置请查看源程序，方阵以上系统得到状态 滤波结果，如图 3-2-1 所示。滤波后的状态与真值之间的偏差如图图 3-2-2 所示。
