扩展卡尔曼滤波EKF.docx

第三章扩展卡尔曼滤波EKF3.1扩展Kalman滤波原理Kalman滤波能够性高斯模型的条件下，可以对目标的状态做出最优的估计，得到较好的跟踪效果对非线性滤波问题常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题因此，可以利用非线性函数的局部性特性，将非线性模型局部化，再利用Kalman滤波算法完成滤波跟踪扩展Kalman滤波就是基于这样的思想，将系统的非线性函数做一阶Taylor展开，得到线性化的系统方程从而完成对目标的滤波估计等处理非线性系统离散动态方程可以表示为X(k1)f[k,X(k)]G(k)W(k)(3-1-1)Z(k)h[k,X(k)]V(k)(3-1-2)这里为了便于数学处理，假定没有控制量的输入，并假定过程噪声是均值为零的高斯白噪声，且噪声分布矩阵G(k)是已知的其中，观测噪声V(k)也是加性均值为零的高斯白噪声假定过程噪声和观测噪声序列是彼此独立的，并且有初始状态估计乂(0|0)和协方差矩阵P(0|0)和线性系统的情况一样，我们可以得到扩展Kalman滤波算法如下乂(k|k1)f(X(k|k))(3-1-3)?-，一-一、P(k1|k)(k1|k)P(k|k)?(k1|k)Q(k1)(3-1-4)1K(k1)P(k1|k)H(k1)[H(k1)P(k1|k)H(k1)R(k1)](3-1-5)乂(K1|k1)X(K1|k)K(k1)[Z(k1)h(X(K1|k))](3-1-6)P(k1)[IK(k1)H(k1)]P(k1|k)(3-1-7)这里需要重要说明的是，状态转移(k1|k)和量测矩阵H(k1)是由f和h的雅克比矩阵代替的。

其雅克比矩阵的求法如下:假如状态变量有n维，即X[X1X2...Xn],则对状态方程对各维求偏导,(k1)ff—-XX1x2x3fXn(3-1-8)HE)X1X2X3hXn(3-1-9)3.2扩展卡尔曼在一维非线性系统中的应用3.2.1状态方程和观测方程都为非线性的通用系统所谓的非线性方程，就是因变量和自变量的关系不是线性的，这类方程很多,例如平方关系，对数关系，指数关系，三角函数关系等等这类方程可分为两类,一类是多项式方程，一种是非多项式方程为了便丁说明非线性卡尔曼滤波一—扩展Kalman滤波的原理，我们选用一下系统,系统状态为X(k)，它仅包含一维变量，即X(k)[x(k)],系统状态方程为X(k)0.5X(k(3-2-1)2.5X(k1)1)8cos(1.2k)w(k)观测方程为Y(k)X2(k)——v(k)20(3-2-2)其中，式(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)，协方差矩阵P第二步：状态预测X(k|k1)2.5X(k1)0.5X(k1)28cos(1.2k)1X(k1)(3-2-3)第三步：观测预测Y(k|k1)X2(k|k1)20(3-2-4)第四步：一阶线性化状态方程，求解状态转移矩阵(k)-_2(k)f0.52.5[1X«|，1]X[1X(k|k1)](3-2-5)第五步：一阶线性化观测方程，求解观测矩阵H(k)H—光F)(3-2-6)第六步：求协方差矩阵预测P(k|k1)P(k|k1)(k)P(k1|k1)(k)Q(3-2-7)这里需要说明的是,当噪声驱动矩阵不存在的时候,或系统状态方程中，在w(k)前没有任何驱动矩阵，这时候，Q必然和状态的维数一样的方阵，可将式(3-2-7)直接写为P(k|k1)(k)P(k1|k1)?(k)Q第七步：求Kalman增益K(k)P(k|k1)H(k)(H(k)P(k|k1)H(k)R)(3-2-8)第八步：求状态更新X(k)X(k|k1)K(Y(k)Y(k|k1))(3-2-9)第九步：协方差更新P(k)(InK(k)H(k))P(k|k1)(3-2-10)以上九步为扩展卡尔曼年滤波的一个计算周期，如此循环下去就是各个时刻EKF对非线性系统的处理过程。

其他参数设置请查看源程序，方阵以上系统得到状态滤波结果，如图3-2-1所示滤波后的状态与真值之间的偏差如图图3-2-2所示。