状态估计理论基础.pdf

《状态估计理论基础.pdf》由会员分享，可在线阅读，更多相关《状态估计理论基础.pdf（125页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、第3-4讲 状态估计理论基础4.1引 言在自动控制、航空航天、通讯、导航和工业生产等领域中，越来越多地遇到“估计”问题。所谓“估计”，简单地说，就是从观测数据中提取信息。例如，在做实验时，为了便于说明问题，常把实验结果用曲线的形式表示，需要根据观测数据来估计和描述该曲线的方程中的某些参数，这一过程叫做参数估计，这些被估计的参数都是随机变量。再举一个例子，在飞行器导航中，要从带有随机干扰的观测数据中，估计出飞行器的位置、速度、加速度等运动状态变量,这就遇到状态变量的估计问题，这些状态变量都是随机过程。因此，“估计”的任务就是从带有随机误差的观测数据中估计出某些参数或某些状态变量，这些被估参数或状

2、态变量统称为被估量。本讲主要讨论状态变量的估计问题，即状态估计。状态与系统相联系。而所谓状态估计，顾名思义，是对动态随机系统状态的估计。设有动态系统，已知其数学模型和有关随机向量的统计性质。系统的状态估计问题，就是根据选定的估计准则和获得的量测信号，对系统的状态进行估计。其中状态方程确定了被估计的随机状态的向量过程。估计准则确定了状态估计最优性的含义，通过测量方程得到的测量信息，提供了状态估计所必需的统计资料。随机过程的估计问题，是从2 0 世 纪 3 0 年代才积极开展起来的。主要成果为 1940年美国学者维纳(Wiener N)所提出的在频域中设计统计最优滤波器的 方 法，称 为 维 纳

3、滤 波。同 一 时 期，苏 联 学 者 哥 尔 莫 郭 洛 夫(A.H.KFJIMOrOPOB)提出并初次解决了离散平稳随机序列的预测和外推问题3支维纳滤波和哥尔莫郭洛夫滤波方法，局限于处理平稳随机过程，并只能提供稳态的最优估值。这一滤波方法在工程实践由于不具有实时性，实际应用受到很大限制！1960年,美国学者卡尔曼(Kalman RE)和 布 西(B ucyR S)提出最优递推滤波方法，称为Kalman滤波。这一滤波方法，考虑了被估量和观测值的统计特性，可用数字计算机来实现。Kalman滤波既适用于平稳随机过程，又适用于非平稳随机过程，因此，Kalman滤波方法得到广泛的应用！本讲共分九节，

4、第二节介绍了 Kalman滤波问题提出的背景；第三节是两个定理矩阵求逆引理和正交定理；第四节和第五节分解介绍了白噪声情况下Kalman最优预测和Kalman最优滤波的基本方程；第六节讨论了最优平滑问题;第七节讨论了有色噪声情况的Kalman滤波问题；第八节将线性的卡尔曼最优估计问题推广到非线性的情形，得到了推广的Kalman滤波方法。第九节是小结。4.2 滤波问题的提出4.2.1 卡尔曼滤波问题的提法在许多实际控制过程中，例如，飞机或导弹在运动过程中，往往受到随机干扰的作用。在这种情况下，线性控制过程可用下式来表示X(r)=A(r)XQ)+j?(r)t/(O+F(r)W(r)(4.2.1)式中

5、，x(r)为控制过程的n维状态向量；U为r维控制向量；W为均值为零的p维白噪声向量；A)为 矩 阵；BQ)为x r矩阵；/为xp矩阵。在许多实际问题中，往往不能直接得到形成最优控制规律所需的状态变量。如飞机或导弹的位置、速度等状态变量都是无法直接得到的，需要通过雷达或其他测量装置进行观测，根据观测得到的信号来确定飞机或者导弹的状态变量。在雷达或别的测量装置中都存在随机干扰的问题，因此，在观测得到的信号中往往夹杂有随机噪声。我们要从夹杂有随机噪声的观测信号中分离出飞机或导弹的运动状态变量。要想准确地得到所需状态变量是不可能的，只能根据观测信号来估计或预测这些状态变量。根据估计或预测得到的状态变量

6、来形成最优控制规律。一般情况下，观测系统可用下述观测 方 程(或测量方程)来表示z(r)=C(OX(O+r(O+V(r)(4.2.2)其中，z为m维观测值；C为?x观测矩阵；y为观测系统的系统误差(已知的非随机序列）；V。）为均值为零的白噪声在式（421）,（422）中假定W，V均为均值为零的白噪声向量。其统计性为E机 旷（7）=。（。即-7）.七 卜 口&氏 河-r）而 丫 第 即 一 切(4.2.3)其中，3（一 丁）是狄拉克函数，它具有性质08(t T)=00t Tt=T+8p(z-r)dr=l-0 0式（322.3）式中。是对称的非负定矩阵，R）是对称的正定矩阵。正定的物理意义是观测向

7、量各分量均附加有随机噪声。、R Q)可对/连续微分。我们的任务是在已知X(r)的初始状态X仇)的统计性，如：期望:仇X o)=恤协 方 差:尸伉)=E X 9)/%X伉)-m0T的条件下，从观测信号ZQ)中得到状态变量X。)的最优估计值。所谓最优估计，是指在某种准则下达最优，估计准则不同会导致不同的估计方法。我们这里采用线性最小方差估计。线性最小方差估计可阐述如下：假定线性控制过程如X(0=A(t)X(t)+即)U(。+(4.2.4)所示，观测方程如式Z(r)=C(/)X(r)+y(/)+V(/)(4.2.5)所示。从时刻t。开始进行观测，得观测值ZQ)；现 在 已 知 内 的 观 测 值Z

8、),要求找出X 4)的最优线性估计戈(乙，(这里，记号小表示利用t时刻以前的观测值Z9)来估计出0时刻的X 6)。最优线性估计包含以下几点意义：1)估计值文&是Z9)的线性函数；2)估计值是无偏的，即仇戈G H)=E X&)；3)要求估计误差九八|f)=X(G-戈(f j t)的方差阵为最小,即EX（tlt）XT（tlt）=min4）根据。和t的大小关系，估计问题可分成三类：（1）乙 ，称为预测（或外推）问题；（2）称为滤波（或估计）问题；（3）称为平滑（或内插）问题。比较起来，预测和滤波问题稍微简单些，平滑问题最为复杂。通常讲的K al m an滤波指的是预测和滤波。4.2.2连续系统的离散

9、化过程在用计算机进行控制时，需要把连续系统离散化，即把微分方程转化为差分方程。设连续系统方程为X =A)X +B(f)UQ)+尸 WQ)(4.2.4)初始条件为X(/)=X。则利用常微分方程基本理论,可得线性非齐次方程(4.2.4)满足初始条件X)=X。的解为XQ)=(乎o)X0+f 亿7 (7)U(7)dr+f O(r,r)F(r)W(r)2 9或T EX-aZ-b2)所以，如果力满足正交条件，则估计误差方差为最小。充分性证毕。证毕我们知道，线性最小方差估计是无偏估计。即讥 =0。由此，可以证明M 与戈正交。事实上E X X =E X(aZ +h)=aEXZ+hE X =0正交定理的几何意义

10、可解释如下随机变量x和z可看作向量空间的两个向量，向量Z,X和X-a Z-b表示如图3.1。图3.1方 差E X-a Z-仃 是x a Z-b长 度 的 平 方，若X-a Z-b与Z正 交，即EX-aZ-bZ0,那么这个长度是最短的。从图3.1可以看出A0。尸为直角三角形，则有E X-aZ-b2=EX2-E(aZ+Z?)2与上面的推导结论相同。如 果X和Z分 别 是n维 和q维 的 向 量，则X的 估 值A可用下式表示X=b+A Zo其中，bn维非随机常数向量，A 非随机常数矩阵。同样可证明正交定理。(略)4.4离散系统卡尔曼最优预测基本方程设系统的状态方程和观测方程为X(攵 +1)=A(k+

11、G(k+1,)。(攵)+r(k+(左)Z(k)=C(k)X(k)+Y(k)+V(k)(4.4.1)(4.4.2)式中u(k)是已知的非随机控制序列，在采样间隔内为常值。Y(k)为观测系统的系统误差项，是已知的非随机序列，在采样间隔内为常值。当不考虑控制信号的作用时，U(k)和Y(k)均为零。卬伏)和，(k)为白噪声序列，在采样间隔内为常值。其统计特性如(3.2.14)所示。当W 和v(k)相互独立时，Sk=0o状态向量的初始值X(0)的统计特性已知为f E X(O)=moR X(O)-m0 X(O)-mo T =Po 4 4 3)已 知 观 测 序 列Z(O),Z,Z(k),要 求 找 出X(

12、k +/)的 最 优 线 性 估 计戈(攵+11攵)，使得估计误差X(k+lk)=X(k+k)-X(k+lk)的方差阵为最小,即EX(k+11 +11 k)=min要求估值文(k +i|k)是z(o),z,z(k)的线性函数，并且估计是无偏的，即EX(k+lk)=EXk+l)下面利用正交定理导出卡尔曼预测的递推公式。4.4.1 状态的预测估计已知观测值Z(O),Z,Z(A-1),设求出了状态向量x(k)的一个最优线性预测-1)。在尚未获得Z(k)之前，对X(k +1)的预测只能借助于状态方程X（k+1）=A（k+T,k）X*）+G（k+1,攵）。（女）+（攵 +1,攵）%（攵）（4,4.1）X

13、(k+lk-l)=A(k+,k)X(kk-1)+G(k+,k)U(k)(4.4.4)为已知Z(O),Z,Z(攵-1)条件下，X(k+1)的预测估计。当戈(I-1)是X/)的最优线性预测时,可以证明，文(k +l|A-1)也是X(k+1)的最优线性预测。事实上，由(441)和(4.4.4)可得X(k+lk-l)=X(k+l)-X(k+lk-l)=A(k+1,k)X(k)+G(k+1,k)U(k)+“k +1,k)W(k)A(k+1,k)X(k 伙-1)+1,%)U(Z)(445)=A(k +,k)X(k)-X(kk-l)+r(k +i,6W(k)即(A+1 伙 -1)=4(4+l,k)X(kk-

14、r)+r(k+1W 伙)(4.4.6)由于北(Z|火-1)是X(%)的最优线性预测，根据正交定理，估计误差X(kk-1)必 须 正 交 于Z(O),Z(1),Z(I),因 此，A(k +1戈(左依-1)也 应 当 正 交 于Z(O),Z(1),Z(A 1);又由于卬伏)是和Z(O),Z,Z(k-1)互相独立的白噪声序歹I J,故W(外正交于Z(O),Z,Z(k-l)。因此，在Z尚未知时，戈伏+1|4一1)是X(O +1)的最优线性预测。4.4.2状态预测估计的修正下面研究在获取了新的观测值Z(k)后，怎样对预测估计值/(k +l|k-l)进行修正。通过观测方程可以确定观测值Z(k)的最优线性预

15、测Z(左)=C伏)X(A)+y/)+V (4.4.2)Z(kk-1)=C(k)X(k K 1)+Y(k)(4.4.7)可以证明,如果新的观测值Z(A)恰好等于Z(k)的预测值2dk-l),则新的观测值没有提供有用的信息。此时，戈伏+1|1)就是X伙+1)的最优线性预测。事实上，根据正交定理，戈(&|&-1)与戈(&|&-1)正交，即E X(k k-1)XT(k k-1)=0(4.4.8)因此，EX(k+lk-X)ZT(kk-Y)=EX(k+k-)C(k)X(k|k -1)+Y(k)Y=EA(k+,k)X(kk-l)+r(k+,k)W(k)XT(k k-r)CT +X =A(k+1,k)EX(k

16、 k-i)XT(kk-V)CT(A)+F(k+,k)EW(k)XT(kk-l)CT(k)+A(k+l,k)EX(k|k-l)YT(k)+r(k+1,k)EW(k)YT(k)=0(4.4.9)即我(k|k-1)与2伏|女-1)正交。故，若新的观测值z(z)恰好等于之(k|k-1)时，文(%+”&-1)就是X(k +1)的最优线性预测。关于这一结论我们可以这样来理解，当我们获得x(k +i)的估计值反(左+1.-1)时，已经假定z等于2(4 5-1),而事实上，若新的观测值Z(k)恰好等于之(&|k-1),则新的观测值z(k)到来时，当然没有提供新的信息，所以，就不必进行修正！但事实上，由于对k时

17、刻状态向量的预测估计有误差；并且观测方程中也存在白噪声V(k)的影响，所以，新的观测值Z(k)一般情况下并不等于2(k|k-i),这个时候，X(k +1)的最优估计值就不再是文伏+1|k-1)了，需要利用Z 6)对戈(女+1|k-1)进行修正,才能得至I J文(女+1|外,怎样利用Z(k)对戈(女+1|k-1)进行修正呢？由于估计值月(&+11氏-1)利用的信息有Z(O),Z(1),Z伙-1)和Z(kk-l);现在，当Z(k)到来的时候，求父(女+1阳利用的信息有Z(O),Z,Z(k),比较起来,新信息就是*k|L l)=Z -2(k|k-1);同时我们进行最优估计的准则是线性最小方差准则，即

18、要求文伍+1因 为Z(O),Z,Z 的线性函数;所以，经分析，自然可令X(k+lk)=A(k+l,k)X(kk-l)+G(k+1 U(%)+K(k)Z(kk-Y)(4.4.10)其中，K6)为待定的矩阵，称为最优增益阵。利用(347)式，上式可改写为X(k+k)=A(k+l,k)X(k k-)+G(k+l,k)U(k)+K(k)Z(k)-Z(kk-1)=A(k+l,k)X(kk-)+G(k+,k)U(k)+K(k)Z(k)-C(k)X(kk-1)-r(J t)(4.4.11)4.4.3最优增益阵下面利用正交定理来确定最优增益阵K(左)。由(4.4.1)式可知k+1时刻的状态方程为X(k+1)=

19、A(k+l,k)X(k)+G(k+l,k)U(k)+r(k+l,k)W 伙)(4.4.1)X(k+k)=A(k+l,k)X(k|k-1)+G(k+l,k)U(k)+K(k)Z(k)-Z(kk-1)=A(k+,k)X(k k-1)+G(k+l,k)U(k)+K(k)Z(k)C(k)X(k 陕1)F(&)(4.4.11)(4.4.1)与(3.4.11)作差得文 伙+I|&)=X(A+I)-3(&+I|&)=A(k+1,k)X(k)+G()t+1,k)U(k)+r(k+1,k)W(k)(4 4 戛)A(k+1,k)X(k I fc-1)+G(/:+1,k)U(k)+K(k)Z(k)-C(k)X(k|

20、/:-1)-Y(k)=A(k+1,k)-K(k)C(k)X(k|k-1)+(%)W(%)+K(k)V(k)利用正交定理EX(k+lk)ZT(k)=O(4.4.13)将(4.4.12)代入得E A(k+1,左)K(k)C(k)父(k k-l)+T(k)W(k)+K(k)V(k)ZT(k)=E A(k+1,A)K(k)C(k)X(k|k-1)+r/)W(k)(4.4.14)+K(k)V(k)C(k)X(k|左 1)+2/伏1)+Y(k)+V(k)T注意到 E X(k|k -1)文 T(k|k -1)=0 以及 V(k),W(k)与 其(k -11 k)正交；V伙),W伙)是均值为零的白噪声，整理上

21、式可得K（Z）=网攵+1 尸（k|l）C（Z）+也+1&。（攵）;依一1）。7（攵）+叫（4.4.15）其中P(kk-1)=E X(k|左 一1)无r 伏 2 1)Sk=E I W(k)VT(k)&=EV 伙)V7/)4.4.4 误差的无偏性及误差方差阵按照我们前面给出的最优线性估计的定义，丸k +”k)要想成为X(k +1)的最优线性估计，需满足下列几点意义 估 计值文伏+1|女)是Z,Z(2),Z(k)的线性函数；(2)估计值是无偏的,即矶5(+1|初=大乂化+1)；(3)要求估计误差声(k +l|6=X(k +D-戈(k +l|k)的方差为最小，即EX(k+1|k)XT(k+11 k)=

22、min注意到前面我们推导北(女 +l|k)的时候，仅考虑了(1)和(3),下面我们来简单证明一下，事实上场(k +l|Z)确实是X(k +1)的无偏估计。从而，乂(k +l|k)就是X(+1)的最优线性预测估计了。由(3.4.12)式可知+11 J t)=A(k+l,k)K(k)C(k)X(k|j t-l)+r(k)W(k)+K(k)V(k)(4.4.16)因此，E X(k+11 初=E A(k+K(k)C(k)X(k|k -1)+F(k)W(k)+K(k)V(k)=A(k+i,k)K(k)C(k)E X(k|*-1)+F(k)E W(k)+K(k)E V(k)=Ak+1,k)K(k)C(k)

23、E X(k(4.4.17)所以，只要初始条件选择的E戌(0|0)=0,则利用上式可知，对任意时刻k,均有E区(k+l|k)=O成立。又因为皆女+1|A)=X(2+1)-1(左+1|乃故,EX(k+11 切=EX(2+1)-(2 +11 切=EX(k+)-EX(k+k)(4.4.18)=EX(k+l)即，戈(&+11&)是X(A+1)的无偏估计。从而戈伏+1 是X(女+1)的最优线性估计。下面推导p(k+l k)的递推关系式。利用定义可得P仇 +11 左)=E X(k+11 k)X k+11幻=E A伏 +K(k)C(k)宜(k|k -1)+F(k)W(k)+K(k)V(k)(4.4.19)A(

24、k+1一K(k)C(k)X(k|i t-l)+(Z)W(k)+K(k)V(k)T注意到V(k),W(k)与 父(k -11 k)互相正交，以及(3.2.14)可得P(k +11 k)=E X(k+1|k)XT(k+1|*)=E A(k+1J)-K(幻C(Z)P(k|k-1)A(Z+1,Z)K(k)C(k+K(k)RkKT(k)(4.4.20)+r(k+1,k)Qkrr(k+,k)-r(k+1,k)SkKT(i t)-K(k)S；f7(k+1,k)将上式展开整理可得P(k+1 阳=A(k+1,k)P(k|k-l)Ar(k+1,k)-A(k+l,k)P(k|k-l)Cr(%)+=(k+1,k)Sk

25、 C(k)P(k|k-Y)CT(k)+RkY -C(k)P(k k-1)A，(k+1,k)+CT(k)rT(k+,k)(4.4.21)+(k+i,k)2r(k+i,k)4.4.5 离散系统卡尔曼最优预测方程及方框图综合(4.4.10)(或(4411),(4415)和(4421)可得完整的离散系统的卡尔曼最优预测基本方程。方程戈(A+1 1 3)=A(&+l,k)X(k|k 1)+G(k +l,k)U(k)+K(k)Z(k)-Z(kk-I)4 4=A(k+1,k)X(k|k -1)+G(k +l,k)U(k)+K Z C(k)X(k|k 1)Y(k)称为最优预测估计方程方程K(k)=A(k+l,

26、k)P(k|k-i)CT(k)+r(k+1,A)S J C P(A|k-l)Cr(J t)+&丁 (4.4.23)称为最优增益阵方程方程P(k+lk)=A(k+l,k)P(k k-)AT(k+1,k)-A 伙 +l,k)P(k k-l)Cr 依)+“k+l,k)Sk C(k)P(k|k-1)C(A)+凡.C(k)P(k k-l)Ar(k+,k)+S：(k+1,A)(4.4.24)+r(k+i,k)QkrT(k+i,k)称为估计误差方差阵的递推方程按照(322.21)和(3 22.22)可作出系统模型方框图，如图4.2所示w（k）图 4.2 离散系统方框图u(k)G(k +l,k)X(k +1

27、1K(k)内亚0 f|延迟.C(k)一+A(k +l,k)，-1 Z(k+1)Z(k+l|k)I +2 X(k|k)A K(k+1)十 A f 延迟+Z(k+11 k)X(k+l|k)-C(k)|3y|A(k+l,k)|G(k+l,k)U(k)图4.5.1离散系统卡尔曼最优滤波方框图4.5.5误差协方差及最优增益阵的几种变形计算公式前面我们已经给出了最优增益阵和误差协方差的递推关系。然而，在讨论K alm an滤波的特殊问题时，有时还需要用到K 和P(k|k)的其它表示形式。下面给出几个比较常用的基本形式，并给出简单的推导过程 1。由(3.5.8)以及(3.5.15),可得，P(k|k)=P(

28、k k-r)-P(k k-T)CT(k)C(k)P(kk-r)CT(k)+RkYC(k)P(k|k l)(4.5.25)K(k)=P(kk-l)CT伙)C(左)P(女|k-l)Cr(k)+%(4.5.26)由(4.5.25)和(4.5.26)可得P(kk)=I-K(k)C(k)P(k k-1)/-K(%)C+K(k)RkKT(k)(4.5.27)P(kk)=P(kk-)-K(k)C(k)P(k|fe-l)=I-K(k)C(k)P(kk-r)(4.5.28)(4528)形式上比(4527)要简单的多，但是当计算舍入误差时，容易失去对称性和非负定性，而(4527)具有比较好的保持对称性和非负定性的

29、能力。由(4.5.26)式等式两边同时右乘C(k)P(k|l)cT(A)+R J,可得K(k)C(k)P(k|k-l)CT(k)+Rk=P(kk-1)C,(k)(4.5.29)上式展开移项可得，KgRk=P(kk-1)C(k)-K(k)C(k)P(k k-l)Cr(k)(4.5.30)即K(k)Rk=1-K(k)C(k)P(k|k-l)Cr(k)(4.5.31)故I-K(A)C/)=P(k|k_l)CT(k)RK-(攵)又由式(4.5.28)两边求逆可得，P-(k|k)=P i(k k-1)/-K(k)C(k)l将式(4532)代入，可得p-(kk)=P-(kk-DP(k|k-)CTK(k)R

30、；K-(k)(4.5.32)(4.5.33)(4.5.33)又对(3520)式两边取逆可得,L(公=C(k)P(k I k 1)C7(Q+RkC-T(k)P一(左 伙 一 1)(4.5.34)展开上式可得,K=C(k)P(k|k-i)CT(k)CT(k)P(kk-l)+RkCT(k)P”(kk-l)=C(k)+RkC-T(k)P-1(kk-Y)(4.5.35)将式(4.5.35)代入式(4.5.33),得P-kk)=CT(k)RC(fc)+RkC-T(k)p-i(P (一 1)=Cr(k)R；C(k)+CT(k)R：R k L (Ie*-(k k-l)(4.5,36)=CT(k)R C(k)+

31、P-(k k-1)又由式(4.5.28)和(4.5.31),可得K(k)Rk=P(kk)CT(k)(4.5.37)上式两边右乘Rp得，K*)=P(kk)CT(k)R(4.5.38)这一部分我们给出了在测量误差和观测误差不相关情况下，最优增益阵和误差协方差阵的几种变形计算公式。4.6离散系统卡尔曼最优平滑基本方程前面两节我们讨论了最优线性预测与滤波的基本方程。这一节我们来讨论最优线性平滑问题。前面在讨论离散系统K al m an 滤波的分类时；已经提到最优线性平滑的定义，如果已知观测值Z(O),Z,Z(j),要求找出X(k)的最优线性估计文(k|j),当k j 时,称为平滑。根据k 和j 的具体

32、变化情况，最优线性平滑又可分为三类 1,2：(1)固定区间最优平滑固定j =N,变化k,并且令k k =N,设X（N）的最优平滑值为总N|j）。（3）固定滞后最优平滑k 和j 都发生变化，但是保持）=1 k,X(k)的极大验后估计量就是使得条件密度P(X(k)|Z：)达到极大的X(k)的值，这一估计就是X(k)的固定区间最优平滑，记为文(k|N).X(k|N)和 X(k|N)可通过解下列联立方程得到6Px(k),x(k+1)|z：-=uCX(k)x(k)=X(k|N)x(k+7)=X(k+/|N)epx(k),x(k+/)|z-二-dx(k+7)x(k)=X(k|N)x(k+/)=X(k+7|

33、N)式中x(k),x(k+7)是X(k),X(k+7)的 取 值,z：是 Z-的取值。(4.6.4)(4.6.5)后面将会看到，由于所得到的方程是递推的，因此只要解(4.6.4)就可以了。由贝页斯(Buyes)公式，可得条件概率密度P x(k),x(k +W=WlP(Z/)设z：是Z：(即Z(1),Z(2),Z(N)的取值。运用联合概率密度公式，可以将上式改写成P x(k),x(k +/)I z：=P x(k)感P(Z/)：P x(k),x(k +/),z(k +/),z(k +2),z(N)I z/P(z：)P z(k +/),z(k +2)r-z(N)|z：P(z：)一 P x(k),x(

34、k +/),z(k +/),z(k +2),z(N)|z HP z(k +/),z(k +2),-z(N)|z 再利用联合概率公式，可得P“口 丫心八 I 7N _ 以出+力z(k +/),z(k +2),-z(N)|x(k),z：P x(k)|z：r|X(K),X(K T 1)Z ,J :P z(k +7),z(k +2),-z(N)|z J(4.6.6)从状态方程(4.6.1)可看出，系统状态是由高斯白噪声序列所激励，且X(0)服从高斯分布，因此 X(k)是高斯一马尔可夫序列。根据观测方程(4.6.2),Z(k)可表成X(k)和V(k)的线性函数，另外考虑到X(k +1),Z(k+/),-

35、Z(N)与V(k),V(k-V(7)互不相关，则可得Px(k+7),z(k+7),z(k+2),z(N)|x(k),z：=Px(k+7),z(k+/),z(k+2),-z(N)|x(k),x(k-x(/),v(k),v(k v(7)=Px(k+7),z(k+7),z(k+2),z(N)|x(k)因此，(4.6.6)变为Px(k),x(k+、)|z：=a曰必 史 2z(X+a-)凶 邙 匕 凶 处 回Pz(k+/),z(k+2),z(N)|z：又由于Px(k+7),z(k+7),z(k+2),z(N)|x(k)=Pz(k+7),z(k+2),z(N)|x(k 4-7),x(k)-Px(k+7)|

36、x(k)=Pz(k+7),z(k+2),z(N)|x(k+7)-Px(k+7)|x(k)则(4.6.7)化为Px(k),x(k+/)|z =Pz(k+7),z(k+2),z(N)|x(k+7)-Px(k+1)x(k)Px(k)|z,Pz(k+/),z(k+2).z(N)|z；(4.6.8)在(4.6.8)式中，所有概率分布密度函数均为高斯分布的，并且只有两个概率密度函数Px(k+/)|x(k)和Px(k)|z打含有x(k),只要计算相应的数学期望和方差阵就可以确定这两个概率密度函数。由状态方程空山可得Ex(k+7)|x(k)=A(k+1,k)x(k)VarX(k+7)|x(k)=f(k+7,k

37、)QkfT(k+1,k)因此P x(k+/)|x(k)=K/e x p -x(k+/)-A(k+1,k)x(k)f -T(k+/,k Ki r1(k+7,k)X；x(k+/)-A(k+1,k)x(k)|(4.6.9)上式中,假定r(k +J,k)Q kT(k +7,k)是正定的。由于在高斯分布的情况下，概率分布密度P x(k)|可由X(k)的期望E x(k)|z：和条件方差V a r X(k)z：来确定。在高斯分布的情况下，X(k)的条件数学期望就是最优滤波估计X(k|k),条 件 方 差 就 是 估 计 误 差 方 差P(k|k),即E x(k)|z =X(k|k),V a r X(k)|z

38、 =P(k|k)o 因此,P x(k +/)|z J =K2e x p j-1 x(k)-x(k|k)T|k)-x(k)-x(k|k)l (4.6.10)将式(4.6.10)和(4.6.9)代入(4.6.8)可得p X(k),x(k+7)|Zn=K,e x p J-x(k +7)-A(k +1,k)x(k)T-r(k +1,k)QkrT(k +/,k)J x(k +7)-A(k +1,k)x(k)r/1整K2e x p j-x(k)-x(k|k)T-p-/(k|k).x(k)-x(k|k)JP z(k +7),z(k +2),z(N)|x(k +7)P z(k +/),z(k +2)-z(N)

39、|z：理上式可得P x(k),x(k +/)|z：=K e x p -：x(k +/)-A(k +/,k)x(k)T r(k +7,k)QkrT(k +7,k)x(k +/)A(k +/,k)x(k)-x(k)-x(k|k)P (k|k)x(k)-x(k|k).P z(k +7),z(k +2),-z(N)I x(k +7)P z(k +/),z(k +2),-z(N)|z (4.6.11)式中K =K,对上式两边取对数可得l n P x(k),x(k +7)|z：=-1 x(k +7)-A(k +1,k)x(k)T.r(k +7,k)QkrT(k +7,k)x(k +7)-A(k +/,k)

40、x(k)-1 x(k)乳k|k).P(k|k).x(k)-x(k|k)+1 n,K .P z(k +/),z(k +2),z(N)|x(k +/)d-P z(k +7),z(k +2),-z(N)|Z 因此，固定区间最优平滑估计量满足下列方程a i n P x(k),x(k +7)|z )-=(J3 x(k)x(k)=X(k|N)x(k+/)=X(k+/|N)(4.6.12)(4.6.13)即AT(k+7,k)-r(k+1,k)QkrT(k+7,k)-/-X(k+7|N)-A(k+7,k)X(k|N)P (k|k)X(k|N)-X(k|k)(4.6.1 4)=0解上式可得X(k|N)=P-(k

41、|k)+AT(k+/,k)r(k+7,k)QkrT(k+A(k+7,k)-/.AT(k+7,k)r(k+1,k)Qkrr(k +1,k)厂 X(k+7|N)(4.0.13)+P-/(k|k)+Ar(k+7,k)-r(k+7,k)QkrT(k+7,k)-/A(k+1,k)-/-p-;(k|k)X(k|k)最后应用矩阵求逆引理，可得固定区间最优平滑估计X(k|N)=X(k|k)+As(k)X(k+7|N)-A(k+7,k)X(k|k)(4.6.1 6)式中As(k)=P(k|k)AT(k+1,k)f(k+1,k)QkfT(k+/,k)+A(k+1,k)P(k|k)AT(k+1,k)广由(4516)

42、可得As(k)=P(k|k)AT(k+1,k)P(k+/|k)(4.6.17)方程(4616)就是固定区间最优平滑方程。(4617)是固定区间最优平滑增益阵。为了逆向计算k=N-1,N-2,的固定区间最优平滑，必须预先算得相应时刻的滤波解X(k|k)和滤波误差方差阵P(k|k)及一步预测误差方差阵P(k+7|k)o 上述固定区间最优平滑方程的边界条件为文(N|N)。注 意%k+1|N)不必由方程(4.6.5)单独求出，由于是逆向递推,所以，在计算文(k|N)的过程中，(k+/N)已经被计算出来了。下面给出平滑误差协方差的递推关系式。平滑误差为X(k|N)=X(k)-X(k|N)(4.6.18)

43、由(4616)可得X(k|N)=X(k)-X(k|N)=X(k)-X(k|k)-As(k)X(k+7|N)-A(k+/,k)X(k|k)=X(k|k)-As(k)X(k+7|N)-A(k+2,k)X(k|k)整理得X(k|N)+As(k)X(k+/|N)=X(k|k)+As(k)A(k+7,k)X(k|k)即X(k|N)+As(k)X(k+7|N)=X(k|k)+As(k)X(k+/|k)(4.6.19)考虑到文(k+/|N)是 观 测 值 Z(7),Z(2),Z(N)的 线 性 组 合，文(k|k)是 观 测 值Z(1),Z(2),Z(k)的线性组合。因此，%(k|N)与文(k+7|N)正交

44、。发(k|N)与文(k|k)正交,则EX(k|N)X(k+/|N)=O ，EX(k|k)X(k|k)=0对(4619)两边取方差可得EX(k|N)XT(k|N)+As(k)EX(k+1 N)XT(k+71 N)A：(k)=EX(k|k)XT(k|k)+As(k)EX(k+1 k)XT(k+1 k)A：(k)即P(k|N)+As(k)EX(k+7|N)XT(k+1 N)A：(k)=P(k|k)+As(k)EX(k+1 k)XT(k+1 k)A：(k)(4.6.20)又由于EX(k+；)XT(k+/)=EX(k+7|k)+X(k+/|k)XT(k+7|k)+X(k+7|k)T=EX(k+11 k)

45、XT(k+/|k)+EX(k+1 k)XT(k+7|k)+EX(k+1k)XT(k+7|k)+EX(k+1k)XT(k+7|k)=EX(k+/|k)XT(k+/|k)+EX(k+1 k)XT(k+7|k)因此,E X(k +1|k)XT(k +l|k)=E X(k +l)XT(k +l)-P(k +l|k)(4.6.21)同理可得E X(k +1|N)X (k +1|N)=E X(k +l)X (k +1)-P(k +1|N)(4.6.22)将式(4.6.21)和(4.6.22)代入式(4.6.20)可得P(k|N)+As(k)E X(k +7)XT(k +7)-P(k +7|N)A j(k)

46、=P(k|k)+As(k)E X(k +7)XT(k +/)-P(k +Z|k)A：(k)整理上式，可得固定区间最优平滑误差方差阵的递推关系式。P(k|N)=P(k|k)+As(k)P(k +7|N)-P(k +/|k)A；(k)(4.6.23)注 意上 述 关 系 式 中k的 变 化顺 序是k =N-/,N-2,0 o其 边 界 条 件 为k =N-/时的P(k+7|N)=P(N|N)O方程(4.6.16)、(3617)和(3623)就为固定区间最优平滑的一组方程。总结上述，固定区间最优平滑的计算步骤为(1)利用 Kalman滤波公式，按k=0,1,-N-1 的顺序，计算X(k|k)、P(k

47、|k)、P(k+/1 k)。将这些量存储于计算机。同时给出终端值P(N|N)和 X(N|N)。(2)利用平滑公式(3.6.16)(36 17)(3 623)按照k=N-l,N-2,0的顺序，计算最优平滑值X(k|N)P(k|N),(k=N-l,N-2,0)。平滑的初始条件为 P(N|N)和文(N|N)。注意(4616)式的边界条件为k=N-1 时的X(k+7|N)=X(N|N)；(4617)式的边界条件为 k=N /时的 P(k+/1 N)=P(N|N)。图 3.5给出了固定区间最优平滑的计算顺序图。图 3.6给出了固定区间最优平滑方框图。X(O|N)X(1 1 N)X(2|N)X(N-2|N

48、)X N-1|N)X(N|N)N-2 N-l N图3.2.5固定区间最优平滑的计算顺序图X(k|k)X(k|k)X(k+l|k)+i X(k|N)A(k +l,k):-k A,(k)f_单位X(k +1|N)延时图3.2.6固定区间最优平滑方框图4.6.2固定点最优平滑固定点最优平滑是利用较多的观测数据Z(7),Z(2),-Z(j),对观测时间内的某一固定时刻N(O N k =N,先令(4616)中的N =j,可得X(k|j)=X(k|k)+As(k)X(k+7|j)-A(k+7,k)X(k|k)(4.6.24)令(4.6.16)中的N=j-1,可得X(k|j-7)=X(k|k)+As(k)X

49、(k+7|j-7)-A(k +7,k)X(k|k)(4.6.25)上面两式做差可得X(k|j)-X(k|j-7)=As(k)X(k+7|j)-X(k+/|j-/)(4.6.26)(4.6.26)式给出了 X(k)的估计值与X(k+/)的估计值之间的递推关系式。在(4.6.26)式中，若令k=N 为常量，则X(N|j)-X(N|j-l)=AS(N)X(N+l|j)-X(N +l|j-l)(4.6.27)如果在(3.6.26)中进一步令k=N+l可得X(N+l|j)-X(N +l|j-l)=As(N+l)X(N+2|j)-X(N +2|j-l)(4.6.28)将(3.6.28)代入(3.6.27)

50、可得X(N|j)-X(N|j-l)=As(N)As(N+1)X(N+2|j)-X(N +2|j-1)(4.6.29)继续上面的过程，直到k=j-/为止，可得X(N|j)-X(N|j-7)=f l As(i)X(j|j)-X(j|j-7)(4.6.30)i=N改写上式得X(N|j)=X(N|j-7)+B(N,j)X(j|j)-X(j|j-h (4.6.31)其中，j=N+/,N+2,，并且 j-i,B(N,j)=n As(i)=B(N,j-l)As(j-l)(4 6 32)B(N,N)=1由第四节的滤波方程得X(j|j)=X(j|j-7)+K(j)C(j)X(j|j-7)+V(j)=X(j|j-