(精品)扩展卡尔曼滤波EKF

第三章扩展卡尔曼滤波EKF3.1扩展Kalman滤波原理Kalman滤波能够在线性高斯模型的条件下，可以对目标的状态做出最优的估计，得到较好的跟踪效果。对非线性滤波问题常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题。因此，可以利用非线性函数的局部性特性，将非线性模型局部化，再利用Kalman滤波算法完成滤波跟踪。扩展Kalman滤波就是基于这样的思想，将系统的非线性函数做一阶Taylor展开，得到线性化的系统方程从而完成对目标的滤波估计等处理。非线性系统离散动态方程可以表示为(k1)f[k,X(k)]G(k)W(k)X（3-1-1）(k)h[k,X(k)]V(k)Z（3-1-2）这里为了便于数学处理，假定没有控制量的输入，并假定过程噪声是均值为零的高斯白噪声，且噪声分布矩阵(k)G是已知的。其中，观测噪声(k)V也是加性均值为零的高斯白噪声。假定过程噪声和观测噪声序列是彼此独立的，并且有初始状态估计ˆ(0|0)X和协方差矩阵(0|0)P。和线性系统的情况一样，我们可以得到扩展Kalman滤波算法如下ˆˆ(k|k1)f(X(k|k))X（3-1-3）(k1|k)(k1|k)P(k|k)(k1|k)Q(k1)P•（3-1-4）1(k1)P(k1|)H(k1)[H(k1)P(k1|k)H(k1)R(k1)]Kk晻（3-1-5）ˆˆˆ(K1|k1)X(K1|k)K(k1)[Z(k1)h(X(K1|k))]X（3-1-6）(k1)[IK(k1)H(k1)]P(k1|k)P（3-1-7）这里需要重要说明的是，状态转移(k1|)k和量测矩阵(k1)H是由f和h的雅克比矩阵代替的。其雅克比矩阵的求法如下：假如状态变量有n维，即12[xx...x]nX，则对状态方程对各维求偏导，123(k1)...nfffffxxxxx（3-1-8）123(k1)...nhhhhhHXxxxx（3-1-9）3.2扩展卡尔曼在一维非线性系统中的应用3.2.1状态方程和观测方程都为非线性的通用系统所谓的非线性方程，就是因变量和自变量的关系不是线性的，这类方程很多，例如平方关系，对数关系，指数关系，三角函数关系等等。这类方程可分为两类，一类是多项式方程，一种是非多项式方程。为了便于说明非线性卡尔曼滤波——扩展Kalman滤波的原理，我们选用一下系统，系统状态为(k)X，它仅包含一维变量，即(k)[x(k)]X，系统状态方程为22.5(k1)(k)0.5X(k1)8cos(1.2k)w(k)1(k1)XXX（3-2-1）观测方程为2(k)(k)(k)20XYv（3-2-2）其中，式（3-1-1）是包含分式，平方，三角函数在内的严重非线性的方程，(k)w为过程噪声，其均值为0，方差为Q，观测方程中，观测信号(k)Y与状态(k)X的关系也是非线性的，(k)v也是均值为0，方差为R的高斯白噪声。因此关于（3-1-1）和（3-2-2）是一个状态和观测都为非线性的一维系统。以此为通用的非线性方程的代表，接下来讲述如何用扩展Kalman滤波来处理噪声问题。第一步：初始化状态(0),Y(0)X，协方差矩阵0P。第二步：状态预测22.5(k1)(k|k1)0.5X(k1)8cos(1.2k)1(k1)XXX（3-2-3）第三步：观测预测2(k|k1)(k|k1)20XY（3-2-4）第四步：一阶线性化状态方程，求解状态转移矩阵(k)2222.5[1X(k|k1)](k)0.5[1X(k|k1)]fX（3-2-5）第五步：一阶线性化观测方程，求解观测矩阵(k)H(k|k1)(k)10hXHX（3-2-6）第六步：求协方差矩阵预测(k|k1)P(k|k1)(k)P(k1|k1)(k)PQ晻（3-2-7）这里需要说明的是，当噪声驱动矩阵不存在的时候，或系统状态方程中，在(k)w前没有任何驱动矩阵，这时候，Q必然和状态的维数一样的方阵，可将式（3-2-7）直接写为(k|k1)(k)P(k1|k1)(k)QP•。第七步：求Kalman增益(k)P(k|k1)H(k)(H(k)P(k|k1)H(k)R)K晻（3-2-8）第八步：求状态更新(k)X(k|k1)K(Y(k)Y(k|k1))X（3-2-9）第九步：协方差更新(k)(IK(k)H(k))P(k|k1)nP（3-2-10）以上九步为扩展卡尔曼年滤波的一个计算周期，如此循环下去就是各个时刻EKF对非线性系统的处理过程。其他参数设置请查看源程序，方阵以上系统得到状态滤波结果，如图3-2-1所示。滤波后的状态与真值之间的偏差如图图3-2-2所示。