4.1 扩展的卡尔曼滤波方程前面讲的Kalman滤波要求系统状态方程和观测方程都是线性的。

然而，许多工程系统往往不能用简单的线性系统来描述。

例如，导弹控制问题，测轨问题和惯性导航问题的系统状态方程往往不是线性的。

因此，有必要研究非线性滤波问题。

对于非线性模型的滤波问题，理论上还没有严格的滤波公式。

一般情况下，都是将非线性方程线性化，而后，利用线性系统Kalman滤波基本方程。

这一节我们就给出非线性系统的Kalman滤波问题的处理方法。

为了方便描述，下面仅限于讨论下列情况的非线性模型kkxkk=k+(3.2.8.1)Φ+Γx+x),1(])w[()(k()1),(kkv=+kk+z(3.2.8.2)hx),1]1()1([+(+)1+式中，1)(⨯∈n R k x 是状态向量，1)(⨯∈m R k z 是观测向量，)(k w 和)(k v 是噪声；1⨯∈Φn R 是k k x ),(的非线性函数，具有一阶连续导数；1⨯∈m R h 是1),1(++k k x 的非线性函数，具有一阶连续导数。

)(k w 和)(k v 都是均值为零的白噪声序列，其统计特性如下{}{}0)(,0)(==k v E k w E ，{}kj T k Q j w k w E δ)()()(=，{}kj T k R j v k v E δ)()()(=另外，已知初始条件，即)0(x 的统计特性。

下面仅介绍推广的Kalman 滤波方法，即围绕滤波值)|(ˆk k x的线性化滤波方法，这种方法是先将非线性模型线性化，而后应用线性系统的Kalman 滤波基本公式。

由系统状态方程(3.2.8.1)可得)(]),|(ˆ[)]|(ˆ)([)),|(ˆ()1()|(ˆ)(k w k k k x k k xk x xk k k xk x k k xk x Γ+-∂Φ∂+Φ≈+= (2.3.8.3)),1()|(ˆ)(k k xk k xk x +Φ=∂Φ∂= (2.3.8.4))()|(ˆ]),|(ˆ[)|(ˆ)(k f k k xxk k k xk k xk x =∂Φ∂-Φ= (2.3.8.5) 则状态方程为)()(]),|(ˆ[)(),1()1(k f k w k k k xk x k k k x +Γ++Φ=+ (3.2.8.6) 初始值为{}000ˆm x E x==。

同基本Kalman 滤波模型相比，在已知求得前一步滤波值)|(ˆk k x 的条件下，状态方程(3.2.8.6)中增加了非随机的外作用项)(k f 。

把观测方程的][∙h 围绕)|1(ˆk k x+进行泰勒展开，略去二次以上项，可得 )1()]|1(ˆ)1([]1),|1(ˆ[)1()|1(ˆ)1(+++-+∂∂+++=++=+k v k k xk x xhk k k xh k z k k xk x (3.2.8.7) 令)1()|1(ˆ)1(+=∂∂+=+k C xhk k xk x)1(]1),|1(ˆ[)|1(ˆ)|1(ˆ)1(+=++++∂∂-+=+k y k k k x h k k xxh k k xk x 则观测方程为)1()1()1()1()1(++++++=+k v k y k x k C k z (3.2.8.8)应用Kalman 滤波基本方程可得)]|1(ˆ)1()1()1()[1()|1(ˆ)1|1(ˆk k x k C k y k z k K k k x k k x++-+-++++=++ (3.2.8.9) 即]}1),|1(ˆ[)1(){[1()|1(ˆ)1|1(ˆ++-++++=++k k k x h k z k K k k x k k x(3.2.8.10) 式中)()|(ˆ),1()|1(ˆk f k k x k k A k k x++=+ (3.2.8.11) 即]),|(ˆ[)|1(ˆk k k x k k xΦ=+1)]1()1()|1()1()[1()|1()1(-+++++++=+k R k C k k P k C k C k k P k K T T (3.2.8.12)方程(3.2.8.13)称为估计误差方差阵的递推方程),1()(),1(),1()|(),1()|1(k k k Q k k k k A k k P k k A k k P T T +Γ+Γ+++=+ (3.2.8.13))|1()]1()1([)1|1(k k P k C k K I k k P +++-=++ (3.2.8.14)式中滤波值和滤波误差方差阵的初始值为{}00)0(ˆm x E x==， {}T m x m x E P ])0(][)0([000--= (3.2.8.15) 推广的Kalman 滤波的优点是不必预先计算标称轨道。

注意推广的Kalman 滤波只要在滤波误差)|(ˆ)()|(~k k x k x k k x -=及一步预测误差)|1(ˆ)1()|1(~k k xk x k k x +-+=+较小时才适用。

4.2 强跟踪滤波基本理论本小节引入自1990年以来发展起来的一个有强跟踪滤波器理论[2-3]。

考虑如下一大类非线性系统的状态估计问题)()())(),(,()1(k w k k k k k Γ+=+x u f x (3.2.9.1))1())1(,1()1(++++=+k v k k k x h z (3.2.9.2)其中，整数k ≥0为离散时间变量，1)(⨯∈n R k x 为状态向量，1)(⨯∈l R k u 为输入向量，1)(⨯∈p R k z 为输出向量。

非线性函数111:⨯⨯⨯→⨯n n l R R R f , 11:⨯⨯→p n R R h 具有关于状态的一阶连续偏导数。

q n R ⨯∈Γ为已知的矩阵。

系统噪声1)(⨯∈p R k w 和测量噪声1)(⨯∈p R k v 匀是高斯白噪声，并具有如下的统计特性0)}({,0)}({==k v E k w E (3.2.9.3) j k T k j w k w E ,)()}()({δQ = (3.2.9.4)j k T k j v k v E ,)()}()({δR = (3.2.9.5) 0)}()({=j v k E T w (3.2.9.6) 其中, Q ()k 为对称的非负定阵, R ()k 为对称正定阵。

初始状态x ()0为高斯分布的随机向量，且满足统计特性0)}0({x x =E (3.2.9.7)000}])0(][)0({[P x x x x =--T E (3.2.9.8) 并且有x ()0与)(),(k v k w 统计独立。

系统(3.2.9.1)-(3.2.9.2)的状态估计问题可以首先选择在3.2.7节引入的扩展Kalman滤波器(Extended Kalman Filter —— EKF)进行解决) 1 +k ( ) 1 +k ( + )k | 1 +k ( ˆ= ) 1 +k | 1 +k ( ˆγK x x(3.2.9.9) 其中，) 1 ( ˆ | k k + x为状态的一步预报值。

)) ( ˆ, ) ( , ( = )k | 1 +k ( ˆk | k k u k f x x(3.2.9.10) 增益阵1)]1())|1(ˆ,1()|1())|1(ˆ,1())[|1(ˆ,1()|1()1(-+++++⨯+++++=+k k k k k k k k k k k k k k k TT R xH P x H xH P K (3.2.9.11)预报误差协方差阵)()()())|(ˆ),(,()|())|(ˆ),(,()|1(k k k k k k k k k k k k k k k T T ΓΓ+=+Q x u F P xu F P (3.2.9.12) 状态估计误差协方差阵)|1())]|1(ˆ,1()1([)1|1(k k k k k k I k k ++++-=++P xH K P (3.2.9.13) 残差序列))|1(ˆ,1()1()1(ˆ)1()1(k k k k k yk y k ++-+=+-+=+γx h y (3.2.9.14) 式(3.2.9.11)中)|1(ˆ)1())1(,1())|1(ˆ,1(k k k k k k k k H +=+∂++∂=++x x xx h x(3.2.9.15)式(3.2.9.12)中)|(ˆ)())(),(,())|(ˆ),(,(k k k k k k k k k u k F x x xx u f x==∂∂ (3.2.9.16) 式(3.2.9.9)—(3.2.9.16)就是著名的扩展Kalman 滤波器的递推公式。

此时输出残差序列的协方差阵为)1())|1(ˆ,1()|1())|1(ˆ,1()]1()1([)1(0++++⨯+++≈++=+k k k k k k k k k k k E k TT R xH P xH V γγ (3.2.9.17)当系统模型(3.2.9.1)-(3.2.9.2)具有足够的精度，并且滤波器的初始值 (|),(|)x 0000P 选择得当时，上述的EKF 可以给出比较准确的状态估计值 (|)xk k ++11。

然而，通常的情况是，系统模型(3.2.9.1)-(3.2.9.2)具有模型不确定性，即此模型与其所描述的非线性系统不能完全匹配，造成模型不确定性的主要原因有：1)模型简化。

对于比较复杂的系统，若要精确描述其行为，通常需要较高维数的状态变量，甚至无穷维的变量。

这对系统状态的重构造成了极大不便。

因此，通常人们都要使用模型简化的办法，使用较少的状态变量来描述系统的主要特征，忽略掉实际系统某些较不重要的因素。

也就是存在所谓的未建模动态。

这些未建模动态在某些特殊条件下有可能被激发起来，造成模型与实际系统之间较大的不匹配[2-3]。

2) 噪声统计特性不准确。

即所建模型的噪声统计特性与实际过程噪声的统计特性有较大差异。

所建模型噪声的统计特性一般过于理想。

实际系统在运行过程中，可能会受到强电磁干扰等随机因素的影响，造成实际系统的统计特性发生较大的变动。

3) 对实际系统初始状态的统计特性建模不准。

4) 实际系统的参数发生变动。

由于实际系统部件老化、损坏等原因，使得系统的参数发生变动(缓变或突变)，造成原模型与实际系统不匹配。

一个很遗憾的事实是，EKF 关于模型不确定性的鲁棒性很差，造成EKF 会出现状态估计不准，甚至发散等现象[2-3]。

此外，EKF 在系统达到平稳状态时，将丧失对突变状态的跟踪能力。

这是EKF 类滤波器(包括卡尔曼滤波器在内)的另一大缺陷。

造成这种情况的主要原因是，当系统达平稳状态时，EKF 的增益阵K k ()+1将趋于极小值。