现代控制理论最优线性预测与滤波的基本方程

1、第十二章 最优线性预测与滤波的基本方程12.1 维纳滤波12.2 卡尔曼滤波问题的提法 12.3 离散系统卡尔曼最优预测基本方程的推导 12.4 离散系统卡尔曼最优滤波基本方程的推导 12.5 连续系统卡尔曼滤波基本方程的推导 12.6 系统噪声与观测噪声相关的卡尔曼滤波 12.7 具有输入信号的卡尔曼滤波12.8 有色噪声情况下的卡尔曼滤波 12.9 滤波的稳定性概念和滤波的发散问题第一节 维 纳 滤 波 12.1.1、维纳滤波问题的提法、维纳滤波问题的提法12.1.2、维纳-霍夫积分方程维纳滤波问题的提法设系统的观测方程为式中， 为有用信号 为观测信号 为观测误差。设 、 和 都是均值为零

2、并具有各态历经性的平稳随机过程（附录四）。 ztxtvt xt vt zt xt zt vt根据观测值 估计 ，使 估值接近于 。维纳滤波的任 务就是设计出一个线性定常系统L，如图12-1所示，使得系统的输出 与 具有最小方差，即 (12-3)这样 就作为 的估值 。 2m inJExtyt xt xt xt zt xt xt yt yt xt根据问题的性质，维纳滤波有下列三个条件： 信号与噪声都是均值为零并具有各态历经性的平稳随机过程； 滤波器是一个物理可实现的线性定常系统。当 时， ； 最优准则是滤波的方差为最小。这些条件使维纳滤波受到很大限制。00h如果系统的脉冲过渡函数为 ，则 (12

3、-4) 是系统L根据输入信号 在 上的全部过去值所给出的实际输出，如图12-2所示。 是 的线性函数 。 0ythztdh ytt ，zt0 zt yt维纳-霍夫积分方程 维纳-霍夫积分方程是确定最优滤波器脉冲过渡函数的一个方程式。根据正交定理（附录五），估计误差应与观测值正交，即 00Exthztdztt 00ExtzthEztztd(12-5) xzEx tz tRzzEz tz tR把上面两式代入式(12-5)，可 得维纳滤波在随机控制领域中是一个很大的突破，但很少被应用，这主要有如下两方面原因： 维纳霍夫积分方程很难解，即使求出了最优滤波器的脉冲过渡函数，在工程上往往很难实现； 维纳理

4、论要求所有的随机过程都平稳的，这与工程实际问题往往不相符合。0 xzzzRhRd(12-6)这就是维纳-霍夫积分方程，解此方程可得最优滤波器的脉冲过渡函数。把上面两式代入式(12-5)，可 得卡尔曼在1960年提出了另一种适合于数字计算机计算的递推滤波法，即所谓的卡尔曼滤波。这种滤波方法不需要求解积分方程，既适用于平稳随机过程，也适用于非平稳随机过程，是一种有广泛应用价值的工程方法。 卡尔曼维纳第二节 卡尔曼滤波问题的提法 在许多实际控制过程中，系统往往受到随机干扰作用，例如飞行中的飞机、导弹受到阵风的扰动。在这种情况下，线性连续系统的控制过程可用下式表示： tttttttxAxBuFw(12

5、-7) 式中， 是控制系统的n维状态向量， 是r维控制向量，假定 是均值为零的p维白噪声向量， 是n*n矩阵， 是n*r矩阵， 是n*p矩阵。对于实际控制系统，最优控制律或自适应控制律的形成需要系统的状态变量，而状态变量往往不能直接获得，需要通过测量装置进行观测，根据观测得到的信号来确定状态变量。但测量装置中一般都存在随机干扰。因此在观测得到的信号中夹杂有随机噪声。 twtA txtB tutF式中， 是m维观测向量, 是m*n矩阵，称为观测矩阵。假定 是均值为零的m维白噪声， 和 相互独立，它们的协方差阵分别为要从夹杂有随机噪声的观测信号中准确地分离出状态变量是不可能的，只有根据观测信号来估

6、计这些状态变量。通常，观测系统的观测方程为ttttzHxv tztHtvtw（12-8） tvTEtttwwQ TEtttvvQ 0TEtwv（12-9）（12-10)式中的 是狄拉克(Dirac) 函数，当 时， ； 时 ；且 。当 和 不随时间而变化时，Q和R都是白噪声的谱密度矩阵。 为对称的非负定矩阵， 为正定的对称矩阵。 正定的物理意义是观测向量 的各分量都附加有随机噪声。 和 都可对t微分。 的初始状态 是一个随机向量。 ttt t0t1tdtQ tR tQ tx0tx tztQ tRtR 现在的任务是从观测信号 中估计出状态变量，希望估计出来的 值与实际的 值愈接近愈好，因此提出最

7、优估计问题。一般都采用线性最小方差估计。 tz tx tx假定 的数学期望 ，方差矩阵 都为已知。00Etxm00000TtEtt Pxmxm0tx 线性最小方差估计问题可阐述如下：假定线性控制过程如式(12-7)所示，观测方程如式(12-8)所示。从时间 开始得到观测值 ，在区间 内已给出观测值 。要求找出 的最优线性估计 。这里记号“ ”表示利用时刻t以前的观测值 来估计 时刻的 值。所谓最优线性估计包含以下三点意义：0tt tz0ttz1xt1/ttz1t1xt/1ttx 估值 是 的线性函数。1/xtt()0ttz 要求估值误差 的方差为最小，即要求/111tttttxxx/m in1

8、111TEttttttxxxx 估值是无偏的，即 /1EttEtxx 比较起来预测问题稍为简单一些，平滑问题最复杂。通常讲的卡尔曼滤波指的是预测和滤波。第一类： 称为预测（或外推）问题根据 和 的大小关系，连续系统估计问题可分为三类：t1t第二类： = ，称为滤波问题；第三类： k，称为预测（或外推）问题； 第二类：j=k，称为滤波问题； 第三类：jk称为平滑（或内插）问题。本章只讨论连续系统和离散系统的最优预测和最优滤波问题。第三节 离散系统卡尔曼最优预测基本方程的推导 在推导卡尔曼预测基本方程时，为了简便起见，先不考虑控制信号的作用，这样，离散系统的差分方程(12-11)变成 11,1,k

9、kkkkkk xxw(12-16)观测方程仍为式(12-12)：kkkkzHxv式中， 和 都均值为零的白噪声序列， 和 相互独立 kwkvkwkv在采样间隔内 和 为常值，其统计特性如式(12-13)所示，即kvkw00EkEkTEkjkkjTEkjkkjTEkjwvwwQvvRwv状态向量的初值 ，其统计特性是给定的，即0 x00Exm00000TEtxmxmP要求估值 是 的线性函数，并且要求估计是无偏的，即给出观测序列 ，要求找出 的线性最优估计 ，使得估计误差 的方差为最小，即 01kzzz， ，1k x1/kkx1/11/kkkkkxxx11/1!/m inTEkkkkkkxxxx

10、1/kkx1/1EkkEkxx 01kzzz， ， 下面推导卡尔曼预测基本公式。推导的方法有几种，比较简易的方法是利用正交定理，用数学归纳法推导卡尔曼估计的基本递推估计公式。 当获得观测值 之后，假定已经得到状态向量 的一个最优线性预测估计 。当还未获得 时刻的新观测值 时，根据已有的观测值，可得 时刻的系统状态向量 的两步预测估值 ： 011k zzz， ，kx/1kk xkkz1k 1k x1/1kkx1/11,/1kkkkkk xx(12-17) 由于 是 的一步最优线性估计， 也是 的最优线性预测估计，这可用正交定理来证明。由式(12-16)减式(12-17)，可得/1kk xkx1/

11、1kkx1k x1/111/11,/11,kkkkkkkkkkkkk xxxxxw1/11,/11,kkkkkkkkk xxw/1/1kkkkkxxx(12-18)式中， 因为 是 的最优线性预测估值，根据正交定理，估计误差 必须正交于 ，所以 的线性变换 也必须正交于 。式(12-18)中的 是均值为零的白噪声序列，与 相互独立，因此 正交于 。所以在没有获得 之前， 是 的最优两步线性预测。/1kk xkx/1kk x 011k zzz， ，/1kk x 1,/1kkkkx 01k zz， ，kw01k zz， ，kwkz1/1kkx1k x01k zz， ， 在新观测值 获取之后，可通过

12、修正两步估值 来得到 的一步预测估值 。kz1/1kkx1k x1/kkx设 的预测估值kz/1/1kkkkkzHx由式(12-12)kkkkzHxv减式(12-19)，得 的预测估计误差为kz/1/1/1/1kkkkkkkkkkkkkkzzzHxxvHxv(12-19) 造成 的原因有两个：对 时刻状态向量 的预测估计有误差；附加了 时刻的观测噪声干扰 。 显然 包含修正 的新信息。这样，在获得 之后，在两步估值 的基础上，用 去修正，便可得到 时刻状态向量 的一步预测估计 ，即/1kk z kkxkkv/1kk z 1/1kkxkz1/1kkx/1kk z 1k 1k x1/kkx1/1/

13、1/1kkkkkkkxxKz 或1/1,/1/1kkkkkkkkkkk xxKzHx(12-20)式中 是待定矩阵，称为最优增益矩阵或加权矩阵。 kK式(12-20)可改写成1/1,/1/1kkkkkkkkkkkk xxKHxKv 时刻系统状态方程为1k 11,1,kkkkkkk xxw由式(12-22)减式(12-21)得 的估计误差为1k x1/1/11/kkkkkkkkkkkkk xKHxwKv(12-21)(12-22)(12-23) 观察式(12-23)右边， 、 、 均分别正交于 ，因此， 正交于 。/1kk x kwkv 011k zzz， ，1/kkx 011k zzz， ，

14、若 与 正交，则 就是 的一步最优线性预测估值。因此，利用 与 的正交条件：1/kkx kz1/kkx1k x1/kkx kz1/0TEkkkxz来确定最优增益矩阵 。kK(12-24)把式(12-23)和式(12-12)代入式(12-24)，得1/11/1/10EkkkkkkkkkkkTkkkkkkk KHxwKvHxHxv【】考虑到 、 、 和 相互间都是正交的，因此上式可简化为/1kkx/1kkxkwkv1/1/10TTTEkkkkkkkkkkkkKHxxHKvv即1/1/10TTTkkkkEkkkkkk EkkKHxxHKvv设 ，又有 ，代入上式后可得最优增益矩阵为/1/1/1TEk

15、kkkkkxxPTEkkkvvR11,/1/1TTkkkkkkkkkkk KPHHPHR(12-25)现在需确定估计误差方差阵 的递推关系式。 1/kkP根据估计误差方差阵的定义，有1/1/1/TkkEkkkkPxx将式(12-23)代入上式得1/1,/11,1,/11/TkkEkkkkkkTkkkkkkkkkkkTkkkkk PKHxwKvKHxwKv【】考虑到 、 和 相互间都正交，可得：/1kk x kwkv1/1,/1 1,1,1/TkkkkkkkkkkkkTTkkkkkkkk PKHPKHKRKQ式中 ()()TQE w k wkk将上式展开，经整理后得 1/1,/11/11/1,/

16、1/11,1/TkkkkkkkkkkkkTTTkkkkkkkkTTTkkkkkkkkkTkkkkk PPKHPPHKKHPHKKRKQ上式右边第四项与第五项之和为/1/111,/1/1/11,/1TTTkkkkkkkkkTTkkkkkkkTTkkkkkkkkkkTTkkkkkkTTkkkkkk KHPHKKRKKHPHRKPHHPHRHPHRKPHK(12-26) 显然，式(12-26)右边第四项与第五项之和在数值上等于第三项，但符号相反。这样，式(12-25)右边第三、第四和第五项之和为零，所以1/1/11,/11/1/1/TTkkkkkkkkkkkkkkTkkkkk PPKHPQ把式(12

17、-25)的 代入上式，可得估计误差方差阵的递推关系式为kK1/1,/11/11,/1/1/11,1,1,TkkkkkkkkTTkkkkkkkkkkTTkkkkkkkkkk PPPHHPHRHPQ(12-27)方程(12-20)、(12-25)和(12-27)构成一组完整的最优线性估计方程，现综合如下： 最优预测估计方程为式(12-20)，即1/1,/1/1kkkkkkkkkkk xxKzHx 最优增益矩阵方程为(12-25)，即11,/1/1TTkkkkkkkkkkk KPHHPHR 估计误差方差阵的递推方程为式(12-27)，即1/1,/11/11,/1/1/11,1,1,Tkkkkkkkk

18、TTkkkkkkkkkkTTkkkkkkkkkk PPPHHPHRHPQ从式(12-27)可看出，估计误差方差阵与 有关，而与观测值 无关。因此，可事先估计误差方差阵 ，同时也可算出最优增益矩阵 。kkQR和kz1/kkPkK按式(12-16) 和式(12-12)作出系统模型方块图，如图12-3所示。图12-4表示由方程(12-21)所描述的卡尔曼最优预测估计方块图。 从图12-4可看出，最优预测估计由三部分组成：估值为观测值的线性函数；最优增益矩 ；单位负反馈回路。kK 现在需要验证第二节时最优估计提出的三项要求：估值为观测值的线性函数；估计值的方差为最小；估值是无偏的，即1/1EkkEkx

19、x 在上面推导预测估计方程时，是按照和两项要求推导的，现需要说明估值 是无偏的问题。对式(12-16)的两端取数学期望，考虑到 ，可得0Ekw11,Ekkk Ek xx再对式(12-21)的两端取数学期望，考虑到 ，可得0E v k1/1,/1/11,/1/1Ekkkk Ekkkk Ex kkkk Ekkkk Ekkk xxKHxKHxx(12-28)(12-29)将式(12-28)减式(12-29)，得到11/1,/1/1Ekkkkk Ekkkkk Ekkk xxxxKHxx如果初始条件为 0 /000EExxm 0 /000Exxm或则0 /000 /00EEExxx(12-30)根据式(

20、12-30)的递推关系，可得 11/0022 /1011/0EEEkkkxxxxxx因此1/10,1,2EkkEkkxx 所以，只要初始估计选为 ，所得估计是无偏的。 0 /00EExx卡尔曼预测估计递推方程的计算步骤如下： 在 时刻给定初值：0t0 /0000Exxxm估值误差方差阵初值：0 /000000TE PPxxxx 根据公式(12-25)计算 时刻最优增益阵 ：0t0K101 0000000TT KPHHPHR， 根据公式(12-20)计算 的最优估值 ： 1x 1/0 x1/01 000000 xxKzHx， 根据公式(12-27)计算 ：1/0P11/01 001 01 000

21、0000001 01 01 00TTTT PPPHHPHRHPQ， 根据已知的 计算 时刻的 。1/0P1t 1K 根据 计算 的估值 。 1K2x 2 /1x重复上述递推计算步骤，可得2 /1 ,2 ,3 / 2/1/1 ,1/kkkkkkkPKxxPKx， ，例12-1 设系统状态方程和观测方程为10.5kktkkkxxwzxv 和 都是均值为零的白噪声序列，且不相关，其统计特性如下：kwkv0 ,01,2EkEkEkjEkjkjkjwvwwvv初值 00 ,0 /010ExmP观测值 001422zzz，试求 的最优预测估值。kx解: 与前面的有关方程对照，可得1,0.51,1112kk

22、kkkkkkHQR，最优预测估值方程为最优增益矩阵为1/0.5/1/1kkkkkkkkxxKzx10.5/1/12kkkkkKPP估值误差方差阵递推方程为211/0.25/10.25/1/121kkkkkkkkPPPP下面计算 ：1/02 /13 / 2xxx，和 0 /011100.5112611/00.250.2511.1673110.51.1671.16720.184212 /10.251.1670.251.167 1.167211.1842120.51.18421.184220.1859 PKPKPK取 的初值 0 /0 x 0 /00001 1/00.50006 2 /10.500.

23、1842 400.7368 3 / 20.50.73680.1859 20.73680.6032Exxmxxx第四节 离散系统卡尔曼最优滤波基本方程的推导 系统状态方程、观测方程和噪声特性如式(12-16)、式(12-12)和式 (12-13)所示。最优滤波问题简述如下：给出观测序列 ，要求找出 的最优线性估计 ，使得估计误差 的方差为最小，并且要求估计是无偏的。采用与上节类似的推导方法数学归纳法和正交定理，导出最优滤波估计方程，即离散系统卡尔曼滤波方程。 011k zzz， ，1k x1/1kkx1/111/1kkkkkxxx推导的具体步骤如下： 假定由观测值 估计得到状态向量 的一步最优预

24、测估值 和观测向量 的预测估值 为: 01kzzz， ，1k x1/kkx1k z1/kkz1/11/kkkkkzHx 当获得 之后，求得 与 的误差，即1k z1k z1/kkz1/11/11/1kkkkkkkkkzzzHxv 以 去修正 ，得到 的滤波估值为1/kkx 1/kkx1k x1/11/11/kkkkkkkxxKz 1/11/1111/kkkkkkkkkxxKzHx或(12-31)式中 为待定的最优增益矩阵。1k K 求估计误差 。1/1kkx 式(12-31)可改写成1/11/111/1kkkkkkkkkxxKHxv则滤波估计误差为1/111/111/111/11/111/1k

25、kkkkkkkkkkkkkkkkkkkxxxxxKHxvxKHxv 利用 与 正交求 。1/1kkx 1z k 1k K 由于 是 的最优估值， 的估计误差必须正交于 。即1/1kkx1k x1k x1z k 1/110TEkkkxz 把 和 的表示式代入上式，并考虑到 ， ， 之间正交，可得1/1kkx 1z k 1/1kkx 1/1kkx 1k v1/111/111/1111/11/11/1/1111/1/11111/TEkkkEkkkkkkkkTkkkkkkkTTEkkkkkkkTTTEkkkkkkEkkTkkxzxKHxKvHxHxvxxHKHxxHKvvPH1111/1110Tkkk

26、kkkkkKHPHKR由上式直接得到最优增益矩阵：111/111/11TTkkkkkkkkkKPHHPHR(12-32) 按照估值误差方差阵定义推导 的递推计算公式：1/1kkP1/11/11/11/111/111/1(1)1/(1)1TkkEkkkkEkkkkkkkkTkkK kHkkkK kkPxxxKHxKvxxv整理并简化上式，可得滤波估值误差方差阵计算公式如下：1/11/1/1111/111/1TkkkkkkkTkkkkkkkkPPPHHPHRHP(12-33) 为了得到 与 之间的递推关系，式(12-31)中的 可由下式计算得到：1/1kkx/kkx1/kkx1/1/kkkkkk

27、xx，(12-34) 为了得到 与 的递推关系式，应将式(12-33)中的 表示成 的关系式。1/1kkP/kkP1/kkP/kkP由式(12-34)求得 的最优预测估值误差为 1k x1/11/1,1,1,/1,/1,kkkkkkkkkkkkkkkkkkkkkk xxxxwxxw而 1/1,1/1,/1,1,/1,TkkEkkkkEkkkkkkkTkkkkkkk Pxxxwxw于是1/1,/1,1,1,TTkkkk EkkkkkkTTkk Ekkkk Pxxww1/1,/1,1,1,TTkkkkkkkkkkkkk PPQ(11_35) 综上所述，方程(12-31)、(12-32)、(12-3

28、3)、(12-34)和(12-35)构成卡尔曼最优滤波基本方程组，与综合如下：(1)1/11/1111/kkkkkkkkkxxKzHx(2)1/1,/kkkkkk xx1(3)11/111/11TTkkkkkkkkkKPHHPHR(4)1/!1/1/1111/111/1TkkkkkkkTkkkkkkkkPPPHHPHRHP(5)1/1,/1,1,1,TTkkkkkkkkkkkkk PPQ若给定初始统计特性 及 ，要得到无偏估计，应取初值0Ex0P 0 /0000 /000 /0(0)0 /0ETPEx xxmxxx离散系统卡尔曼最优滤波的方块图如图12-5所示。 从式(12-32)可分析 和

29、对 的影响。当 增大时，观测噪声大，观测值可靠度低，于是加权阵 应取得小一些，以减弱观测噪声的影响。所以式(12-32)中 随 的增大而减小。当 增大时，意味着第 步转移的随机误差大，对状态预测修正应加强，于是 应增大。从式(12-35)可知，当 增大时， 增大， 也增大，表示对状态预测修正加强。1k QkKk/1kk PkR1k QkKkRkKkKkR1k QkK 在讨论卡尔曼滤波的特殊问题时，常需用到 和 的另一些表达式。根据式(12-32)可得以下公式：kK/1kk P1/1/1TTkkkkkkkkkKPHHPHR/1/1TTkkkkkkkkkKHPHRPH/1/1TTkkkkkkkkk

30、kKRPHKHPH1/1TkkkkkkkKRKHPH(12-36)(12-37)(12-38)(12-39)根据式(12-33)可得/1/1/1/1TTkkkkkkkkkkkkTTTkkkkkkkkkPPPHKKHPKHPHKKRK/1TTkkIkkkkIkkkkkPKHPKHKRK(12-40)(12-41)将式(12-38)代入(12-40)得 的另一表达式：/kkP/1kkIkkkkPKHP(12-42)式(12-42)在形式上虽比式(12-41)简单，但当计算过程具有舍入误差时，容易失去对称性和非负定性，而式(12-41)具有较强的保持对称性和非负定性的能力。/kkP 将式(12-42

31、)展开，应用矩阵求逆引理（附录二），又可得 的另一种形式：111/1TkkkkkkkPPHRH再由式(12-39)和式(12-42)可得/TkkkkkKRPH1/TkkkkkKPHR(12-43)(12-44)(12-45)需要指出，从式(12-36)至式(12-45)都是对滤波情况而言。 必须指出，在实际应用卡尔曼滤波算法时，每一步都要求 和 是对称的。虽然式(12-36)和式(12-35)在理论上是对称的，但是在运算过程中，有限字长和舍入误差可能引起 和 不对称，从而导致滤波系统的性能严重下降，甚至导致不稳定。这种情况当比较小时尤其明显。1/1kkP1/kkP1/1kkP1/kkP例12-

32、2 设二阶系统模型和标量观测模型为11101110kkkkkkkxxwzxv至输入噪声 是平稳的， ， 测量噪声 是非平稳的， 。换句话说， 为偶数时的噪声比为奇数时的噪声大。假定初始状态的方差阵 ， 欲计算 。 tw0001kQkv21kk Rk100010PkK解: 运用方程式(12-35)、(12-32)和初始条件 可算得0P 20100.951/0,110110.48PK再由式(12-42)计算出验后方差阵 为1/1P0.950.481/10.486.24P利用式(12-35)可计算下一步的验前方差阵 为2 /1P8.146.712 /16.717.24P然后再计算 等。直到计算到所要

33、求的 为止。读者可按照上述方法继续计算下去，从计算结果可以看出，当 为奇数时，由于测量噪声较小，所以 较大；当 为偶数时，则 较小。在 以后，增益就近似地达到周期性的稳态解。2K10k kkKkkK4k 第五节 连续系统卡尔曼滤波基本方程的推导 在推导连续系统卡尔曼滤波基本方程时，也先不考虑控制信号的作用，这样系统的状态方程为 tttttxAxFw式中， 为n维状态向量， 为系统噪声，其均值为零， 为 矩阵， 为 矩阵。tx tw tAnnF tnp(12-46)观测方程为 ttttzHxv(12-47)式中， 为 维观测值向量， 为 矩阵， 为 维白噪声，其均值为零。 tzm tHmntvm

34、假设 和 相互独立，它们的协方差阵为 twtv 0TEkktTEkttTEkwwQvvRwv式中各符号的意义如本章第二节所述。初始状态向量 是一个随机向量，其均值和方差阵分别为0tx00/0000000EtTtttEtt xmPPxmxm在区间 内已给出观测值 ，要求找出 的最优线性估计 ，使得估计误差方差阵的迹为最小，即估计误差各分量方差之和为最小：0ttz1tx/1ttx/m in1111TEttttttxxxx 要求估值 是观测值 的线性函数，并且要求估计是无偏的，即 /1ttx()0ttz/11EttEtxx这里，只讨论预测和滤波问题，即 。1tt 关于连续系统卡尔曼滤波公式的推导，采

35、用卡尔曼在1962年提出的方法：离散系统的采样间隔 ，取离散型卡尔曼滤波方程的极限，连续型卡尔曼滤波方程，在离散型卡尔曼预测估计方程式(12-20)中，令采样间隔为 ， ，则式(12-20)变为0t 11tttttttkk ， ttkkKK/,/tt ttt tt tttttt ttk xxKzHx(12-48)由式(12-25)有1,/TTttt tt ttttt tttkk KPHHPHR考虑到ttkRR则1,/1,/tTTttt tt ttttt ttttkTTttt tt tttttt tttt RKPHHPHPHHPHR(12-49)转移矩阵可用下面近似式表示tttt 1A(12-5

36、0)把式(12-49)和式(12-50) 代入到(12-48)，得到/1,/tt tt ttttt ttTTttt ttt ttt Htt ttttttt tt xxAxPHPHRzHx即/,/1/tt tt tttt tttt tt tttTTttt ttttttt tt xxAxPHPHRzHx令 ，考虑到 ，对上式等号两边取极限，则得到连续系统的最优滤波方程：0t , t tI/t ttt tttkt txAxKzHx1/Ttt tktKPHR式中 (12-51) (12-52)为最优增益矩阵，这里 必须正定。 tR 根据估计误差方差阵的递推方程(12-27)，用同样的方法，并考虑到,t

37、tttkkRQRQ可得/,/,/1/,TTtt ttt tt tttt ttt tt ttttTTtt ttttt tttt tttTtt ttt tt PPPHRHPHHPQ考虑到 ,tt tItt A,tt ttt F则/,/1/,Ttt tIttt ttIttTttt tt ttttTt tttttttTTt tttt tttttt PAPAPHHPHRHQPFF即/,/1/,tt tt ttTTtt ttt tttttt ttttTTtt tt ttttt ttttTTttt tttt tttt PPAPPAAPAPHHPHRHPFQF令 ，对上式等号两边取极限可得连续系统误差方差职的

38、矩阵黎卡提微分方程：0t 1/TTTt ttt tt ttt ttttt ttttPAPPAPHRHPFQF其初始条件为 ，解此方程可得 。/00ttP/t tP(12-53) 估计误差方差的矩阵微分方程与 和 的二阶矩 和 有关，而与观测值 无关。因此可事先算出方差阵 ，同时也可算出 。 tw tv tQ tR tz/ttP tK 方程(12-51)、(12-52)和(12-53)构成一组连续系统的卡尔曼滤波方程。下面讨论估计是否无偏的问题。如果初始条件 选成00/ttx000/EttEtxx则式(12-51)给出的估值 是无偏的。由式(12-46)得/ttx 0EttEtEtxAxw由式(

39、12-51)得 /EtttEtttEztHtEttxAxKx从方程(12-47)可得 0EztHtEtEtxw所以 /EtttEttttEtEttxAxKHxx用 减上式，可得 EttEtxAx ( )( )dEx tttttEx ttdtxAKHx上式的解为0( )/,( )/Ex tttt tEx ttt xx 是转移矩阵，它是下列微分方程的解：0, t t 0000,t ttttt tttIAHK， 如果 ，就有 。这样，估计是无偏的。000/EtEttxx /EttEtxx 根据式(12-46)和式(12-47)，可作出系统模型方块图，如图12-6所示。 根据式(12-51)可作出连续

40、系统卡尔曼滤波方块图，如图12-7所示。 从图12-7可看出：连续系统的卡尔曼滤波器是由三部分组成: 系统模型； 最优增益矩阵； 单位负反馈回路。例12-3 设系统状态方程和观测方程如下： tttttt xxwzxv 和 都是均值为零的白噪声，其统计特性如下： tw tv 02.520EtEtEtttEttEtwvwwvvwv设 。试设计卡尔曼滤波器。00 / 03.0ExPm解 与系统方程相比较，可得 1112.52ttttt AHFQR，滤波方程为 /tttttttt xxKzx最优增益矩阵为 1/2tttKP估计误差方差阵为21/2/2.52tttttt PPP令 ，则/ttPP 211

41、451522dPPPPPdt 用分离变量法，可得 1512dPdtPP 11116152dPdPdtPP 315dPdPdtPP 对上式积分，可得215tPaeP当 时， 代入上式可得，则0t 0 / 03P31154tPeP从上式求得33514/114ttePtte当 比较大时， 趋近于稳态值1。t/ttP最优增益系数为 335141214tteKte当 比较大时， 趋近于稳态值 。t Kt1 / 2 稳态值 值也可以按下列方式求得，当 较大时， 则黎卡提微分方程转变为黎卡提代数方程：/Pttt0P 2122.502PP 245015015PPPPPP 或因估计误差方差 必为正值，故稳态的

42、值为1。/Ptt/Ptt卡尔曼滤波方块图如图12-8所示。 和 随时间的变化曲线如图12-9所示。从该图可看出，即使是定常系统，最优增益系数K也是变系数。当时间不断增大，K趋近于某一稳态值。 和 的初始段与 的初值 ( ) 有很大关系，但 和 的稳态值与 没有关系。/Ptt Kt/Ptt Kt/Ptt0 / 0P/Ptt Kt0 / 0P 当滤波达到稳态过程后，由于 趋于某一常值， 作为滤波器的输出， 为滤波器的输入，则可得输出与输入之间的传递函数 Kt/ttx tz 0.3331110.666111KxsKKsssKssKz由传递函数可知，本例的卡尔曼滤波器为一惯性环节。第六节 系统噪声与观

43、测噪声相关的卡尔曼滤波离散系统连续系统一一 离散系统离散系统 系统的状态方程和观测方程与式(12-16)和式(12-12)完全相同，即11,1,kkkkkkk xxwkkkkzHxv 和 都均值为零的白噪声序列，两者相关，则kwkvTkkjTkkjTkkjEkjEkjEkj wwQvvRwvC在 和 相关的情况下，最优估计方程的推导与前面所述的推导方法和步骤基本相同。 kwkv下面不加推导，直接写出结论。 最优线性估计的卡尔曼预测方程为 1 /1/1/1kkkkkkkkkkk xxKzHx，11,/11 /1TkTkkkkkkkkkkkkk KPHCHPHR11 /1,/11,1,/11,/1

44、/11,1,1,1,TTkTTkTTTkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk PPPHCHPHRHPCQ(12-54) (12-55) (12-56) 将式(12-54)、(12-55)、(12-56)与第三节中的式(12-20) 、(12-25)、(12-27)相比较，可得以下结论： 与 相关情况下的最优预测估计方程(12-54)与两者不相关情况下的最优预测估计方程(12-20)完全相同的。 kwkv 和 的相关性只影响最优增益矩阵和估计误差方差阵。很明显，若 ，式(12-55)和(12-56)分别变成式(12-25)和式(12-27)。kwkv0kC二二 连续系统

45、连续系统系统的状态方程和观测方程与式(12-46)和式(12-47)完全相同，即 tttttkkkkxAxFwzHxv 和 是均值为零的白噪声，且两者相关，则kwkv TTTEkttEkttEktt wwQvvRwvC 采用第五节所用的推导方法，可得 和 相关情况下的连续系统最优滤波方程、最优增益矩阵、估计误差方差阵。kwkv /ttttttttttxAxKzHx 1/TtttttttKPHFCR 1/TTTTTttttttttttttttttttttttPAPPAPHFCRHPCFFQF(12-57)(12-58)(12-59) 将式(12-57)、(12-58)、(12-59)与式(12-

46、51)、(12-52)、 (12-53)相比较，可得以下两点结论： 与 相关情况下的最优滤波方程(12-57)和两者不相关情况下的最优滤波方程(12-51)完全相同，这说明 与 两者的相关性不影响估值方程。 kwkvkwkv 和 的相关性只影响 计算公式，若 则式(12-58)和(12-59)分别变成式(12-52)和式(12-53)。kwkv /tttKP和 0tC 在上面讨论卡尔曼滤波基本方程时，为了简便起见，假定作用于系统的控制信号等于零。实际上，系统总会受到控制信号的作用。下面讨论具有输入控制信号的卡尔曼滤波方程。第七节 具有输入信号的卡尔曼滤波一 离散系统二 连续系统一一 离散系统离

47、散系统系统的状态方程和测量方程为11,1,1,kkkkkkkkkk xxGuwkkkkkzHxyv 式(12-60)中的 为已知的非随机控制序列，式(12-61)中 的为观测系统的系统误差项，也是已知的非随机序列。在采样间隔 内， 和 都是常值。 kukytkuky（12-60）（12-61） 和 的统计特性如下：kwkv0TkkjTkkjTkkjEkEkEkjEkjEkj wvwwQvvRwvC 采用与前面相同的推导方法，可得到具有输入控制信号的卡尔曼滤波方程。下面不加推导地直接写出具有输入控制信号的最优预测的卡尔曼滤波方程。1 /1,/11,/1kkkkkkkkkkkkkkk xxGuKz

48、Hxy11,/11,/1TkTkkkkkkkkkkkkk KPHCHPHR11 /1,/11,1,/11,/1/11,1,1,1,TTkTkTTTkTkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkk PPPHCHPHRHPCQ（12-62）（12-63）（12-64）将式(12-62)、(12-63)、(12-64)与式(12-54)、(12-55)、 (12-56)相比较，可得以下两点结论： 当系统具有输入控制信号 和测量系统误差 时，它们只对 的估值方程有影响，而对最优增益矩阵 和估计误差方差阵 的计算公式无任何影响。kukykxkK1 /kkP 和 的相关性不影响 的估值方

49、程，只影响 和 的计算公式。因此式(12-63)与式(12-55)完全相同，式(12-64)与式(12-56)完全相同。kwkv1k xkK1 /kkP 具有输入控制信号的离散系统方块图和卡尔曼预测方块图如图12-10和图12-11所示。 具有输入控制信号的离散系统，其系统噪声 和测量噪声 均是白噪声，且两者相关。对于这样的系统，前面已讨论过直接考虑 与 相关的卡尔曼滤波方程的推导。这里通过工程实例，再介绍另一种将噪声相关问题转化为不相关问题处理的卡尔曼滤波方程的推导。kwkvkwkv 例12-4 在进行导弹容错控制设计时，需要利用导弹俯仰加速度回路和状态信息进行故障诊断，以判断回路中的加速度

50、计是否发生故障。设某地空导弹俯仰通道加速度回路的数学模型为11,1,1,kkkkkkkkkk xxGuwkkkkzHxv其中，状态变量 ， 与弹体俯仰角速度率成比例， 与弹体俯仰加速度率成比例， 与弹体俯仰加速度的导数成比例。控制变量 俯仰制导指令。输出变量 为加速度计测得的弹体加速度。试设计卡尔曼滤波器，估计出状态变量。 123Tkkkk xxxx，1kx2kx3kxkukz(12-65)(12-66) 要求仿真计算1000步，初始条件： 是周期为400步、幅值为1的方波信号。 0000Ekxxu，导弹俯仰通道加速度回路数学模型中的系数矩阵为0101,0010.4965851.7640862

51、.232575kk011,01,111kkkk G，00.3243770.357865kH 和 均为白噪声序列，且两者相关。其统计特性如下：kwkv00EkEkwv，220.50.5kkEkQEkwvR，0.5kEkkwvC解 滤波方程的推导由式(12-66)可得0kkkkzHxv将状态方程(12-65)变为下式：11,1,1,1,kkkkkkkkkkkkkkkk xxGuwJzHxv或11,1,1,1,1,1,kkkkkkkkkkkkkkkkkkk xJHxGuJzwJv其中 为待定矩阵 1,kkJ(12-67)设*1,1,1,kkkkkkk JH*1,1,kkkkkkk wwJv将 和 代

52、入式(12-67)，得新状态方程：*1,kk*kw*11,1,1,kkkkkkkkkkk xxGuJvw式(12-68)中 可视为新的控制项， 可视为新的系统噪声，仍是白噪声。1,1,kkkkkkGuJz*kw(12-68)*1,1,1,1,TTkkkjEkkEkkkkkkjkkkk wvwJvvCJR当 ， 与 不相关，必须*0TEkj wv*kwkv1,1,0kkkkkkCJR即11,1,kkkkkk JC R(12-69)由此求得待定矩阵 。将式(12-69)代入式(12-68)，可得1,kkJ*1*11,1,1,kkkkkkkkkkkkk xxGuC Rzw 式(12-70)中， 视为

53、控制项。由式(12-70)和式(12-66)构成系统数学模型，直接利用前面系统噪声和测量噪声不相关的卡尔曼滤波基本方程组，即式(12-31)至式(12-35)，并考虑具有输入控制信号的影响，可得到具有输入控制信号， 和 相关情况下的卡尔曼滤波方程组。11,1,kkkkkkkk GuC Rzkwkv(12-70)由式(12-31)写出1 /11 /1111 /kkkkkkkkkxxKzHx由式(12-34)并考虑 输入作用的影响，写出 与 的转移关系式，即11,1,kkkkkkkk GuC Rz1 /kkx/kkx*1111 /1,/1,1,1,1,/1,1,kkkkkkkkkkkkkkkkkk

54、kkkkHkkkkkkkkk xxGuC RzC RxGuC Rz11 /1,/1,1,/kkkkkkkkkkkkkkHkkk xxGuC Rzx(12-72)(12-71)由式(12-32)和式(12-33)写出1111 /111 /1TTkkkkkkkkkKPHHPHR111 /11 /1 /111 /111TTkkkkkkkkkkkkkkPPPHHPHRHP根据 的定义，写出1 /kkP1 /1 /1 /TkkEkkkkPxx而*1 /11 /1 /kkkkkkkkkk xxxxw(12-73)(12-74)则*11*1 /1,/1,/1,/1,1,/1,1,1,1,1,1,/1,1,1

55、,1TTTTTkkkkTTkkkEkkkkkkkkkkkkkkkkEkkkkkkkkEkkkkkkkkkkkkkkkkkkkkkkk PxwxwPwwPwC RvwC RvPQ1,1,kTTkkkkkC RC即1111 /1,1,/1,1,1,1,1,1,kkTkkTTTkkkkkkkkkkkkkkkkkkkkkkkkkk PC RHPC RHQC RC(12-75) 至此，我们推导了具有输入控制和噪声相关情况的卡尔曼滤波方程组，下面就以此方程组计算例题的状态估计问题。 状态最优估值计算与数学仿真计算步骤： 令 ，组定状态最优估值初值 估计误差方差阵初值 。在本例计算中，选取 =0， =dia

56、g106，106，106。0k 0 / 0 x0 / 0P0 / 0 x0 / 0P 利用式(12-75)计算预测误差方差阵 。1 /kkP 利用式(12-73)计算增益矩阵 。1k K 利用式(12-72)计算 的最优预测估值 。 利用式(12-71)计算 的最优滤波估值 。 利用式(12-74)计算估计误差方差阵 。 令增1，返回第2步，重复上述计算步骤。数字仿真流程图如图12-12所示。1k x1k x1 /kkx1 /1kkx1 /1kkP 计算结果如图12-13、图12-14、图12-15所示。图中表示出输入控制信号为方波情况下的状态 、 和 及其估值 、 和 。1kx2kx3kx1

57、/kkx2/kkx3/kkx图12121313图12121414图12121515二、连续系统二、连续系统具有输入控制信号的连续系统状态方程和观测方程如下： tttttttxAxBuFw tttttzHxyv式中， 为已知的非随机函数， 为观测系统的系统误差项，也是已知的非随机函数， 和 都是均值为零的白噪声，并且两者是相关的。 和 的统计特性如下： tu tykwkvkwkv 0TTTEtEtEtttEtttEttt wvwwQvvRwvC(12-76)(12-77) 在式(12-62)、式(12-63)和式(12-64)中，令采样间隔 ,并考虑到0t ,kkkttttttQRCQRC 然后

58、求极限，可得具有输入控制信号的连续系统的卡尔曼滤波方程： /tttttttttttttxAxBuKzHxy(12-78)最优增益矩阵为 1/TtttttttKPHFCR估计误差方差阵为 1/TTTTTttttttttttttttttttttttPAPPAPHFCRHPCFFQF(12-79)(12-80)将式(12-78)、式(12-79)、式(12-80)与式(12-57)、式(12-58)、式(12-59)相比较，可得以下两点结论： 当系统具有输入控制信号 和测量系统误差 时，它们只对 的估值方程有影响，而对最优增益矩阵方程和估计方差阵的计算公式无任何影响。 和 的相关性不影响 的估值方程

59、，只影响 和 的计算公式。因此式(12-79)与式(12-58)相同，式(12-80)与式(12-59)相同。 tu ty txkwkv tx tK/ttP具有输入控制信号的连续系统方块图和卡尔曼滤波方块图如图12-16和图1217所示。图12121616图12121717第八节 有色噪声情况下的卡尔曼滤波 前面推导卡尔曼滤波方程时，假定 和 都是白噪声。实际上， 和 可能是有色噪声。所谓白噪声，就是不同时刻的噪声都是互不相关的，而有色噪声是在不同时刻的噪声都是相关的。某些特定的有色噪声可用白噪声通过成形滤波器来表示。这样就可直接应用上面的滤波方程。下面将先介绍成形滤波器，然后再通过实例说明如

60、何把某些特定的有色噪声用成形滤波器来处理的问题。kwkvkwkv一一 连续系统的成形滤波器连续系统的成形滤波器设 是一平稳随机过程，其相关函数为 t0aeRR式中， 为常数， 为时间间隔。将噪声的相关函数进行富氏变换，可得噪声的频谱密度为2200202cos20cosRSRdRed 可以将 分解成S2020RRSjj 由上式可直接写出相应的传递函数： 202011RRWssss为 拉 氏 算 子 显然，随机过程 相当于单位强度白噪声（谱密度 的白噪声称为单位强度白噪声）长时间作用于传递函数为 的线性系统的结果。该系统是一个惯性环节，其时间常数为 ，放大系数为 ，可用图12-18表示。 t1S