2010_第四章_非线性系统的Kalman滤波

1、z(k+1)=hx(k+1),k+1+v(k+1)(3.2.8.2)z(k+1)=hx(k+1),k+1+v(k+1)(3.2.8.2)4.1扩展的卡尔曼滤波方程前面讲的Kalman滤波要求系统状态方程和观测方程都是线性的。然而，许多工程系统往往不能用简单的线性系统来描述。例如，导弹控制问题，测轨问题和惯性导航问题的系统状态方程往往不是线性的。因此，有必要研究非线性滤波问题。对于非线性模型的滤波问题，理论上还没有严格的滤波公式。一般情况下，都是将非线性方程线性化，而后，利用线性系统Kalman滤波基本方程。这一节我们就给出非线性系统的Kalman滤波问题的处理方法。为了方便描述，下面仅限于讨论

2、下列情况的非线性模型x(k+1)二(x(k),k)+口x(k+1),kw(k)(3.2.8.1)x(k+1)=x(k+1Ik)式中x(k)eRnxi是状态向量，z(k)eRmn是观测向量，w(k)和v(k)是噪声；wRi是x(k),k的非线性函数，具有一阶连续导数；heRm“是x(k+1),k+1的非线性函数，具有一阶连续导数。w(k)和v(k)都是均值为零的白噪声序列，其统计特性如下E(v(k)=0,E.(k)=0，E(v(k)wt(j)=Q(k)5，E(k)vt(j)=R(k)5kjkj另外，已知初始条件，即x(0)的统计特性。下面仅介绍推广的Kalman滤波方法，即围绕滤波值X(kIk)

3、的线性化滤波方法，这种方法是先将非线性模型线性化，而后应用线性系统的Kalman滤波基本公式。由系统状态方程(3.2.8.1)可得x(k+1)0(x(kIk),k)+x(k)一x(kIk)+TX(kIk),kw(k)(2-3-8-3)dxx(k)=x(kIk)竺=0(k+1,k)(2.3.8.4)x(k)=x(kIk)dxOx(kIk),k-dxx(k)=X(kIk)X(kIk)=f(k)(2.3.8.5)则状态方程为x(k+1)=(k+1,k)x(k)+TX(kIk),kw(k)+f(k)(3.2.8.6)初始值为x=ex=m。000同基本Kalman滤波模型相比，在已知求得前一步滤波值x(

4、kIk)的条件下，状态方程(328.6)中增加了非随机的外作用项f(k)。把观测方程的h围绕x(k+1Ik)进行泰勒展开，略去二次以上项，可得z(k+1)=hx(k+1Ik),k+1+竺dxdhdxx(k+1)-x(k+1Ik)+v(k+1)(3.2.8.7)x(k+1)=x(k+1Ik)=C(k+1)dhdxx(k+1)=x(k+llk)x(k+1lk)+hx(k+1lk),k+1=y(k+1)则观测方程为z(k+1)=C(k+1)x(k+1)+y(k+1)+v(k+1)应用Kalman滤波基本方程可得x(k+11k+1)=x(k+11k)+K(k+1)z(k+1)-y(k+1)-C(k+1

5、)x(k+11k)即x(k+11k+1)=x(k+11k)+K(k+1)z(k+1)-hx(k+11k),k+1式中x(k+11k)=A(k+1,k)x(klk)+f(k)(3.2.8.8)(3.2.8.9)(3.2.8.10)(3.2.8.11)x(k+11k)=Ox(klk),kK(k+1)二P(k+11k)Ct(k+l)C(k+1)P(k+11k)Ct(k+1)+R(k+1)-i(3.2.8.12)K(k+1)二P(k+11k)Ct(k+l)C(k+1)P(k+11k)Ct(k+1)+R(k+1)-i(3.2.8.12)方程(3.2.8.13)称为估计误差方差阵的递推方程P(k+1Ik)

6、=A(k+1,k)P(kIk)At(k+1,k)+r(k+1,k)Q(k)rt(k+1,k)(3.2.8.13)P(k+1Ik+1)=I-K(k+1)C(k+1)P(k+1Ik)(3.2.8.14)式中滤波值和滤波误差方差阵的初始值为x=E(x(O)=m，P=ex(O)-mx(0)-mt00000(3.2.8.15)推广的Kalman滤波的优点是不必预先计算标称轨道。注意推广的Kalman滤波只要在滤波误差(kIk)=x(k)-x(kIk)及一步预测误差(k+11k)=x(k+1)-x(k+11k)较小时才适用。4.2强跟踪滤波基本理论本小节引入自1990年以来发展起来的一个有强跟踪滤波器理论

7、2-3。考虑如下一大类非线性系统的状态估计问题x(k+1)二f(k,u(k),x(k)+r(k)w(k)(3.2.9.1)z(k+1)二h(k+1,x(k+1)+v(k+1)(329.2)其中，整数k0为离散时间变量，x(k)eRnxi为状态向量，u(k)eRm为输入向量，z(k)eRpx1为输出向量。非线性函数f:Rlx1xRnx1Rnx1,h:Rnx1Rpx1具有关于状态的一阶连续偏导数。reRnxq为已知的矩阵。系统噪声w(k)eRpxi和测量噪声v(k)eRpxi匀是高斯白噪声，并具有如下的统计特性Ew(k)二0,Ev(k)二0(3.2.9.3)Ew(k)wt(j)=Q(k)8(3.2

8、.9.4)k,jEv(k)vT(j)=R(k)5k,j(3.2.9.5)Ew(k)VT(j)=0(3.2.9.6)其中，Q(k)为对称的非负定阵，R(k)为对称正定阵。初始状态x(0)为高斯分布的随机向量，且满足统计特性Ex(0)二x0(3.2.9.7)Ex(0)-xx(0)-xt=P000(3.2.9.8)并且有X(0)与w(k),v(k)统计独立。系统(329.1)-(329.2)的状态估计问题可以首先选择在3.2.7节引入的扩展Kalman(3.2.9.9)滤波器(ExtendedKalmanFilterEKF)进行解决X(k+1lk+1)=X(k+1lk)+K(k+1)Y(k+1)其中

9、，貳(k+1|k)为状态的一步预报值。(3.2.9.10)x(k+llk)=f(k,u(k),x(kIk)增益阵(3.2.9.ll)K(k+1)二P(k+llk)Ht(k+1,X(k+llk)H(k+1,X(k+llk)xP(k+llk)Ht(k+1,X(k+llk)+R(k+1)-i(3.2.9.12)预报误差协方差阵P(k+llk)二F(k,u(k),X(klk)P(klk)Ft(k,u(k),X(klk)+r(k)Q(k)rt(k)(3.2.9.13)状态估计误差协方差阵P(k+llk+1)二I-K(k+1)H(k+1,X(k+llk)P(k+llk)(3.2.9.14)残差序列Y(k+

10、1)二y(k+1)y(k+1)二y(k+1)h(k+1,X(k+llk)式(3.2.9.11)中H(k+1,X(k+11k)二ah(k+1,x(k+1)(3.2.9.15)Qxx(k+1)二X(k+11k)式(3.2.9.12)中F(k,u(k),X(kIk)二Qf(k,u(k),x(k)axx(k)二x(kIk)(3.2.9.16)式(3.2.9.9)(3.2.9.16)就是著名的扩展Kalman滤波器的递推公式。此时输出残差序列的协方差阵为V(k+1)二Ey(k+1)yT(k+1)沁H(k+1,x(k+1Ik)P(k+1Ik)0 xHt(k+1,x(k+1Ik)+R(k+1)(3.2.9.

11、17)当系统模型(329.1)-(329.2)具有足够的精度，并且滤波器的初始值x(00),P(00)选择得当时，上述的EKF可以给出比较准确的状态估计值x(k+1Ik+1)。然而，通常的情况是，系统模型(3.2.9.1)-(3.2.9.2)具有模型不确定性，即此模型与其所描述的非线性系统不能完全匹配，造成模型不确定性的主要原因有：模型简化。对于比较复杂的系统，若要精确描述其行为，通常需要较高维数的状态变量，甚至无穷维的变量。这对系统状态的重构造成了极大不便。因此，通常人们都要使用模型简化的办法，使用较少的状态变量来描述系统的主要特征，忽略掉实际系统某些较不重要的因素。也就是存在所谓的未建模动

12、态。这些未建模动态在某些特殊条件下有可能被激发起来，造成模型与实际系统之间较大的不匹配2-3。噪声统计特性不准。即所建模型的噪声统计特性与实际过程噪声的统计特性有较大差异。所建模型噪声的统计特性一般过于理想。实际系统在运行过程中，可能会受到强电磁干扰等随机因素的影响，造成实际系统的统计特性发生较大的变动对实际系统初始状态的统计特性建模不。4)实际系统的参数发生变。由于实际系统部件老化、损坏等原因，使得系统的参数发生变动(缓变或突变)，造成原模型与实际系统不匹配。一个很遗憾的事实是，EKF关于模型不确定性的鲁棒性很差，造成EKF会出现状态估计不准，甚至发散等现象2-3。此外，EKF在系统达到平稳

13、状态时，将丧失对突变状态的跟踪能力。这是EKF类滤波器(包括卡尔曼滤波器在内)的另一大缺陷。造成这种情况的主要原因是，当系统达平稳状态时，EKF的增益阵k(k+1)将趋于极小值。这时，若系统状态发生突变,预报残差丫(k+1)将随之增大。然而，此时的增益阵k(k+1)仍将保持为极小值，K(k+1)不会随丫(k+1)的增大而相应地增大。因此，由式(329.9)X(k+llk+l)=X(k+llk)+K(k+l)Y(k+1)(329.9)得知，EKF将丧失对突变状态的跟踪能力。从这个意义上可以认为，EKF类滤波器是一种开环滤波器，因为这类滤波器的增益阵K(k+1)不会随滤波效果自适应地进行调整，以始

14、终保持对系统状态的准确跟踪能力这一现象对定常线性随机系统将更加直观一些。此时，只需用通常的Kalman滤波器(KF)进行状态估计。而KF的增益阵可以根据线性系统的参数（A,B,C）离线计算出来，然后存储在计算机中在线应用。这时，如果系统状态发生突变，滤波器的增益阵当然不会随之变动，因此，KF也就丧失了对突变状态的跟综能力。所以说,KF也是一种开环滤波器。为了克服EKF存在的上述缺陷，迫切需要有一种性能更加优越的滤波器。为此,人们提出如下强跟踪滤波器的新概念。定义3.2.91称一个滤波器为强跟踪滤波器（StrongTrackingFilter，STF），若它与通常的滤波器相比，具有以下优良的特性

15、：1）较强的关于模型不确定性的鲁棒性。2）极强的关于突变状态的跟踪能力。甚至在系统达平稳状态时，仍保持对缓变状态与突变状态的跟踪能力。3）适中的计算复杂性。显然，特性1）和2）就是为了克服EKF的上述两大缺陷而提出来的。特性3）是为了使得STF便于实时应用。关于系统(329.1)-(329.2)的一类强跟踪滤波器应具有如下的一般结构x(k+11k+1)=x(k+11k)+K(k+1)y(k+1)其中X(k+1Ik)=f(k,u(k),X(kl)kY(k+1)二y(k+1)-h(k+1,X(k+11k)(3.2.9.18)(3.2.9.19)(3.2.9.20)现在面临的难点就是要在线确定时变增

16、益阵K(k+1)，使得此滤波器具有强跟踪滤波器的所有特性。为此，提出一个正交性原理。正交性原理使得滤波器(329.18)为强跟踪滤波器的一个充分条件是在线选择一个适当的时变增益阵k(k+1)，使得1)minE(k+1+j)yt(k+1)L0,k=0,1,2,;j=1,2,2)(3.2.9.21)(3.2.9.22)其中，条件2)要求不同时刻的残差序列处处保持相互正交，这也是正交性原理这一名称的由来条件1)实际上就是原来的EKF的性能指标说明此正交性原理的一个浅显的例子是，早已证明，当模型与实际系统完全匹配时，Kalman滤波器(KF)的输出残差序列是不自相关的高斯白噪声序列，因此，式(3.2.

17、9.22)是满足的。而式(3.2.9.21)就是KF的性能指标，因此当然也是满足的。当由于模型不确定性的影响，造成滤波器的状态估计值偏离系统的状态时，必然会在输出残差序列的均值与幅值上表现出来。这时，若我们在线调整增益阵K(k+1),强迫(329.22)式仍然成立，使得残差序列仍然保持相互正交，则可以强迫强跟踪滤波器保持对实际系统状态的跟踪。这也是”强跟踪滤波器”一词的由来。此正交性原理具有很强的物理意义。它说明当存在模型的不确定性时，应在线调整增益阵K(k+1)，使得输出残差始终具有类似高斯白噪声的性质。这也表明已经将输出残差中的一切有效信息提取出来。当不存在模型的不确定性时，强跟踪滤波器正

18、常运行，式(3.2.9.22)已自然满足，不起调节作用。此时的强跟踪滤波器就退化为通常的基于性能指标(329.21)的EKF。注释3.2.91此正交性原理的核心是式(3.2.9.22)2)e(k+1+j)yt(k+1)L0,k=0,1,2,;j二1,2,(3.2.9.22)当用其它的性能指标取代式(329.21)后，就可以得到另外一些变形的类似的正交性原理因此，当在原有的滤波器上附加上条件(3.2.9.22)后，就可以改造原来的滤波器，使其具有强跟踪滤波器的性质注释3292对非线性系统，实际应用此正交性原理时，式(329.21)与(3.2.9.22)很难精确满足。这时,只需使其近似满足即可，以

19、减少计算量，保持强跟踪滤波器具有良好的实时性3.2.9.3一种带次优渐消因子的扩展卡尔曼滤波器(SFEKF)为了使得滤波器具有强跟踪滤波器的优良性能，一个自然的想法是采用时变的渐消因子对过去的数据渐消，减弱老数据对当前滤波值的影响。这可以通过实时调整状态预报误差的协方差阵以及相应的增益阵来达到。为此，我们修改上面的EKF中的公式(3.2.9.2.12)为：P(k+11k)二九(k+1)F(k,u(k),X(klk)P(klk)Ft(k,u(k),X(klk)+(32931)r(k)Q(k)rt(k),其中，九(k+1)1为时变的渐消因子。(3.2.9.2.9)(3.2.9.2.11)式,(3.

20、2.9.3.1)式,(3.2.9.2.13)(3.2.9.2.17)式构成了一种带次优渐消因子的扩展卡尔曼滤波器，简记为：SFEKF(SuboptimalFadingExtendedKalmanFilter),参见17,18。我们之所以称其为次优渐消因子，是因为我们通常采用次优的算法来求取九(k+1),以提高算法的实时性。3.2.9.1一个有用的定理现在的目标是应用上一节的正交性原理来确定时变次优渐消因子九(k+1),进而也就确定了时变增益阵K(k+1)。首先，我们给出一个有用的定理。定理3.2.9.1令：(k)Ax(k)X(kl切，其中，(kIk)为由上面的SFEKF得到的状态估计值。若O(

21、k)|2O(k)成立，对j二1,2,，就有下式成立:E(k+1+j)Yt(k+1)丿qH(k+1+j,X(k+1+jIk+j)-F(k+j,u(k+j),x(k+j1k+j)1-K(k+j)H(k+j,x(k+j1k+j-1)F(k+2,u(k+2),X(k+21k+2)I-K(k+2)H(k+2,X(k+21k+1)-F(k+1,u(k+1),X(k+1IIk+1)P(k+1Ik)Ht(k+1,X(k+1Ik)-K(k+1)V(k+1)0(3.2.9.3.2)=H(k+1+j,X(k+1+j1k+j)-rfF(k+1,u(k+1),X(k+11k+1)I-K(k+1)H(k+1,X(k+11

22、k+1-1)-1=jF(k+1,u(k+1),X(k+1IIk+1)P(k+1Ik)Ht(k+1,X(k+1Ik)-K(k+1)V(k+1)0证明：略因此，有E(k+1+j)YT(k+1)丿=0P(k+1Ik)Ht(k+1,X(k+1Ik)-K(k+1)V(k+1)=003292次优渐消因子的确定根据正交性原理，为了使SFEKF具有强跟踪滤波器的性质，在每一采样时刻，应在线确定其增益阵K(k+1)，强迫使式(3.2.9.2.22)保持成立17,18,即E(k+1+j)t(k+1)L0(3.2.9.3.13)由定理3.2.9.1与(3.2.9.3.13)式得知，当在线选择适当的时变增益阵K(k+

23、1),使得P(k+11k)Ht(k+1,X(k+11k)-K(k+1)V(k+1)三0(3.2.9.3.14)0时，则正交性原理的(3.2.9.2.22)式必然成立。为此，可以令W(k+1)AP(k+11k)Ht(k+1,X(k+11k)-K(k+1W(k+1)(3.2.9.3.15)0并定义g(x(k+1)二込W2(k+1),(3.2.9.3.16)iji=1j=1其中，w(k+1)=G(k+1)。ij由此可知，(3.2.9.3.14)的符合程度可以通过求解下面的性能指标来衡量：mign九(k+1)(3.2.9.3.17)Nk+1)由性能指标(3.2.9.3.17)求解九(k+1)可采用任何

24、一元无约束非线性规划方法，在这里我们首先给出一种梯度方法。算法3291梯度方法次优渐消因子可由下面的迭代公式得到19：h1+1)(k+1)亠1)(k+1)-Jg:)(Mk+D),心01,2,；k二1,2,(3.2.9.3.18)ok(i)(k+1)初始值：加0)(1)二1,加0)(k+1)二九(k).式(3.2.9.3.18)中，申0为迭代步长，此参数的选择需要一定的技巧，以使g(i)(九(k+1)快速衰减.其中，i为迭代步数.(3.2.9.3.18)式中的0g(1)(M(k+】)可由下0九i)(k+1)列公式得到：学刖丄迟2W(i)(k+叽5X(i)(k+1)耳i=1j=15W(/)(k+1)5X(i)(k+1)ij,其中W(i)(k+1)=P(i)(k+11k)Ht(k+1,X(k+11k)-K(i)(k+1)V(k+1),0P(i)(k+11k)=加i)(k+1)F(k,u(k),