Kalman滤波理论及其在导航系统中的应用式（2.62a)和式(2.62b）又称为Kalman滤波器方程，由此两式可得到Kalman滤波器方框图，如图2.1所示。在图2.1中，滤波器的输入是系统状态的观测值，输出是系统状态的估计值，7X单位滞后O+1OtxX图2.1随机线性离散系统Kalman滤波器结构图式（2.62)的滤波算法可用方框图表示，如图2.2所示。从图2.2中可以明显看出，Kalman滤波具有两个计算回路：增益计算回路和滤波计算回路。其中增益计算回路是独立计算的，滤波计算回路依赖于增益计算回路。Pk=1Xrk=10.Pu -ox-Pr-,ph ,+r++iOr,--R-RK-PHHPL,HTRIk1X-X++K[2,-1,X-]P-{-K,H,)P-I图2.2Kalman滤波算法方框图2.2.3随机线性离散系统Kalman滤波方程的直观推导假设在k时刻我们得到了#次观测值Z1，，Z，，且找到了X的一个最优线性估计×-1.即x-1是Zi，"，Zk-1的线性函数，由状态方程（2.60)可见，W-1是白噪声，一个简单而直观的想法是用Xk,-1 = 0k是-1Xb-1(2.63)作为X的预测估计，由于V也为白噪声，考虑到EV=0,所以对于时刻系统的观测值2的预测估计为2x,x-1 = H,Xx,k-1(2.64)
第2章随机线性系统Kalman滤波基本方程23当我们在表时刻获得观测值，它与预测估计之-之间有一误差Zkk-I = Zh - 2k.k-1 = Zk -HXk.k 1(2.65)造成这·-误差的原因是预测估计×-L与观测值Z都可能有误差，为了得到k时刻X，的滤波值，白然会想到利用预测误差乙-来修正原来的状态预测估计X，-1，于是有X= Xxk-I + K(Ze-HXe,-1)(2.66)式中K，为待定的滤波增益矩阵。记文表1≤X-X1(2.67)-X(2.68)它们的含义分别为获得观测值Z前后对X，的估计误差。现在的问题是如何按照月标函数J =E[Xx,XT](2.69)最小的要求来确定最优滤波增益矩阵K。根据式（2.68）、式(2.63)和式（2.66)有X= Xe -XA.-1 - K[Z -HXA.k-1]= XA.RI - K[HX + V, - HiXA.-1](2.70)- Xk.k-I -K[HXE,a-I + Vi]= [I -KH]X.1- KV?由于文，-1是Z，，…，2，的线性函数，故有E[X VI] = 0, E[VXI- = 0(2.71)X,XT=[I-KHX.-I-KV·[I-KHX.-I-KVT-EI-KHIXAR-IXT-I-KH,JT-KVXI-I-KH JT- [I - KH,JX.-+ VTKT + KVVIK](2.72)
Kalman滤波理论及其在导航系统中的应用24:于是，滤波误差方差矩阵为PA=FEX,XTI(2.73)= [- K,H]P-[I- KH,JT+ K,PK]将上式展开，并同时加上和减去Pe, 1H,[HPet-IHI+ R]-'HPk,-1这项，再把有关K，的项合并到一起，即PA=Pk,-I-Pe.-IHT[HPk.k-IHT+RJ-"HPa,-1+[K,-Pe.kIHI[HPe.k--HI+ R,]-]]+[HPk-IHT+RJ[K-Pe,-HT[HPa,H +R]-] (2.74)在式(2.74)中前两项不含K因子，因此，为使滤波误差方差阵P.极小，只要选择K-Pk-IHT[H,Pe.-IHT +R,]-I = 0(2.75)于是得到K = Pr.t-IHT[H,Pe.&-IHI + R.]-1(2.76)而此时误差方差阵P为PE- Ph,R-I-PAR-IHTEHiP'e.E-IHT+ R.J-"HPa,k-!(2.77)=[I-K,HIPA,.-1式中P，1为一步预测误差方差阵，即Pe.-1 = E[x..-IXT.a-](2.78)进一步有Xk.k-1= X-X.k-I= Φ.k-IX&-1 + Fe-I W,-1 - @, 1x-I(2.79)= @k,k 1xk-1 + Fe,-I Wk-1Xkk-IXT. =[@.-1x1+ Ixa-} W1][@k-1x-1+ Ih.k-IWe-I]T=@rk-IX.-1XT-,OT..-I + IsR-1We-+ WT-FI.-1+Ie,k- W iXT-IP,-1 + @x,k-IX&-I WT-IFT,-1(2.80)内为E[@k.k-IX+-- WT-,T.-1] - O, E[Fh..-IWe-IX-I@T..-1] = 0(2.81)
第2章随机线性系统Kalman滤波基本方程?25，于是,有Pk.k 1 = F[X.k-IXT,-I] = Da.k-IPe-1T,,-I + Fe,k-1Q&-IFT,t-1(2.82)至此，我们得到了随机线性离散系统Kalman滤波基本方程。Kaiman滤波算法具有如下特点：（1）出于Kalman滤波算法将被估计的信号看作在白噪声作用下-~个随机线性系统的输出，并且其输人输出关系是出状态方程和输出方程在时间域内给出的，闪此这种滤波方法不仅适用于平稳序列的滤波，而且特别适用于非平稳或平稳马尔可夫序列或高斯-马尔可夫序列的滤波，因此其应用范围是十分广泛的。（2）由于Kalman滤波的基本程是时间域内的递推形式，其计算过程是-个不断地“预测-修正”过程，在求解时不要求存储大量数据，并且一旦观测到了新的数据，随时可以算得新的滤波值，因此这种滤波方法非常便于实时处理，计算机实现（3）由于滤波器的增益矩阵与观测无关，因此它可预先离线算出、从而可以减少实时在线计算；在求滤波器增益矩阵K，时，要求一个矩阵的逆，即要计算（HPk，-H+R），它的阶数只取决于观测方程的维数m，而m通常是很小的，这样，上面的求逆运算是比较方便的；另外，在求解滤波器增益的过程中，随时可以算得滤波器的精度指标P，其对角线上的元就是滤波误差向量各分量的方差。（4）增益矩阵K与初始方差阵Po.系统噪声方差阵Q以及观测噪声方差阵R之间具有如下关系：：由Kalman滤波的基本方程（2.62c）、（2.62d)可以看出：Po，Q1和R（k=1，2，）同乘一个相同的标量时，K值不变。，由滤波的基本方程（2.62c）可见，当R，增大时，K，就变小，这在直观上是很容易理解的，因为如果观测噪声增大，那么滤波增益就应取小一些（因为这时的新信息里的误差比较大），以减弱观测噪声对滤波值的影响。*如果Pc变小，Q-1变小，或两者都变小，则由滤波的基本方程（2.62d)可以看山，这时Pk，1变小，而从滤波的基本方程（2.62e)可以看出，这时的P也变小，从而K，变小。这也是很自然的。因为P。变小，表示初始估计较好，Q-1变小，表示系统噪声变小，于是增益矩阵也应小些以便给于较小的修正。综上所述，可以简单地说，增益矩阵K与Q-1成正比，而与R，成反比。2.2.4随机线性离散系统Kalman滤波方程的投影法推导上一节用直观推导方法得出了Kalman滤波基本方程，尽管有些地方在数学
?26·Kalman滤波理论及其在导航系统中的应用上不够严密，但却反映了滤波的物理过程。考虑到投影法在数学上的严密性，并且也是R.E.Kalman在其发表的Kalman滤波论文中使用的方法，因此，有必要介绍用投影法推导Kalman滤波方程，设系统数学模型及条件同式（2.60）和式(2.61)。1、寻找一步最优线性预报估计根据向量正交投影的结论1，设基于k一1次观测向量集合-1=［Zi，Z2.，Z-门得到的线性最小方差估计为X-1=E[X-1/z](2.83)那么，基于Z-1估计X而得到的--步最优线性预报为Xt-1=E[X./k-1](2.84)而由式(2.84)和向量正交投影的结论2,得Xk,-1= E[(x,k-1Xh-1 + Fk 1Wk-1)/Z "](2.85)= k 1X-1 +Pk-1E[W-1/Z-1]由于假设W-与Z1,Z2，Zk-不相关，即W,-1与2-1正交（因为z1可由与W-1不相关的Xo，Wi，W2，"，W,-2，Vi，V2，，V-线性表示），且有EW,-1=0,故由向量正交投影的结论1知，E［W1/Z-1]=0,则式（2.85)变成X,k-1 = Dk.k-IX1(2.86)记×-1=X-X，·1，则×-1与-1正交。2.寻找一步最优线性预报观测值基于Z-1对Z，所做的一步最优线性预报应为Zk.k-1=E[Z/Zk-1]=E[(HX,+V)/Zi-1]= H,E[X,/Z-1] +E[V/Z]由于V与Xo，W.，W2,""，W-2,Vi，V2，，V-1不相关，即V与-1正交，且有V=0,则有E[V,/Z-1]=0,于是2k,k-1 =HXk, 1 = H@a.k-Ix-1(2.87)3.寻找Z与Z-1的正交分量新息由于2-1为Z在-1上的正交投影，因此Zk.k-1 = Zh - 2h,k-1 = Zk - H@k,-1X元-1(2.88)