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

现代控制理论最优线性预测与滤波的基本方程第1页/共192页第一节维纳滤波

12.1.1、维纳滤波问题的提法12.1.2、维纳-霍夫积分方程第2页/共192页维纳滤波问题的提法设系统的观测方程为式中，为有用信号为观测信号为观测误差。设、和都是均值为零并具有各态历经性的平稳随机过程（附录四）。根据观测值估计，使估值接近于。维纳滤波的任务就是设计出一个线性定常系统L，如图12-1所示，使得系统的输出与具有最小方差，即

(12-3)这样就作为的估值。第3页/共192页根据问题的性质，维纳滤波有下列三个条件：⑴信号与噪声都是均值为零并具有各态历经性的平稳随机过程；⑵滤波器是一个物理可实现的线性定常系统。当时，；⑶最优准则是滤波的方差为最小。这些条件使维纳滤波受到很大限制。如果系统的脉冲过渡函数为，则

(12-4)

是系统L根据输入信号在上的全部过去值所给出的实际输出，如图12-2所示。是的线性函数。第4页/共192页维纳-霍夫积分方程维纳-霍夫积分方程是确定最优滤波器脉冲过渡函数的一个方程式。根据正交定理（附录五），估计误差应与观测值正交，即(12-5)把上面两式代入式(12-5)，可得第5页/共192页维纳滤波在随机控制领域中是一个很大的突破，但很少被应用，这主要有如下两方面原因：⑴维纳－霍夫积分方程很难解，即使求出了最优滤波器的脉冲过渡函数，在工程上往往很难实现；⑵维纳理论要求所有的随机过程都平稳的，这与工程实际问题往往不相符合。(12-6)这就是维纳-霍夫积分方程，解此方程可得最优滤波器的脉冲过渡函数。把上面两式代入式(12-5)，可得第6页/共192页卡尔曼在1960年提出了另一种适合于数字计算机计算的递推滤波法，即所谓的卡尔曼滤波。这种滤波方法不需要求解积分方程，既适用于平稳随机过程，也适用于非平稳随机过程，是一种有广泛应用价值的工程方法。卡尔曼维纳第7页/共192页第二节卡尔曼滤波问题的提法

在许多实际控制过程中，系统往往受到随机干扰作用，例如飞行中的飞机、导弹受到阵风的扰动。在这种情况下，线性连续系统的控制过程可用下式表示：

(12-7)

第8页/共192页式中，是控制系统的n维状态向量，是r维控制向量，假定是均值为零的p维白噪声向量，是n*n矩阵，是n*r矩阵，是n*p矩阵。对于实际控制系统，最优控制律或自适应控制律的形成需要系统的状态变量，而状态变量往往不能直接获得，需要通过测量装置进行观测，根据观测得到的信号来确定状态变量。但测量装置中一般都存在随机干扰。因此在观测得到的信号中夹杂有随机噪声。第9页/共192页式中，是m维观测向量,是m*n矩阵，称为观测矩阵。假定是均值为零的m维白噪声，和相互独立，它们的协方差阵分别为要从夹杂有随机噪声的观测信号中准确地分离出状态变量是不可能的，只有根据观测信号来估计这些状态变量。通常，观测系统的观测方程为（12-8）（12-9）（12-10)第10页/共192页式中的是狄拉克(Dirac)函数，当时，；时；且。当和不随时间而变化时，Q和R都是白噪声的谱密度矩阵。为对称的非负定矩阵，为正定的对称矩阵。正定的物理意义是观测向量的各分量都附加有随机噪声。和都可对t微分。的初始状态是一个随机向量。第11页/共192页

现在的任务是从观测信号中估计出状态变量，希望估计出来的值与实际的值愈接近愈好，因此提出最优估计问题。一般都采用线性最小方差估计。假定的数学期望，方差矩阵都为已知。第12页/共192页

线性最小方差估计问题可阐述如下：假定线性控制过程如式(12-7)所示，观测方程如式(12-8)所示。从时间开始得到观测值，在区间内已给出观测值。要求找出的最优线性估计。这里记号“”表示利用时刻t以前的观测值来估计时刻的值。所谓最优线性估计包含以下三点意义：⑴估值是的线性函数。第13页/共192页⑶要求估值误差的方差为最小，即要求⑵估值是无偏的，即第14页/共192页

比较起来预测问题稍为简单一些，平滑问题最复杂。通常讲的卡尔曼滤波指的是预测和滤波。第一类：>称为预测（或外推）问题根据和的大小关系，连续系统估计问题可分为三类：第二类：=，称为滤波问题；第三类：<称为平滑（或内插）问题。第15页/共192页设离散系统的差分方程和观测方程分别为（12-11）（12-12）式中，是n维状态向量，是r维控制向量，是m维观测向量，是n*n转移矩阵，是n*r矩阵，是n*p矩阵，是m*n矩阵。下面讨论离散系统的卡尔曼滤波问题。第16页/共192页（12-13）式中，为克罗尼克(Kroneker)函数，其特性是

假定是均值为零的p维白噪声向量序列，是均值为零的m维的白噪声向量序列，和相互独立，在采样间隔内和都为常值，其统计特性如下：第17页/共192页（1）(12-14)（2）(12-15)

为非负定矩阵，为正定矩阵。和都是方差阵，而和不是方差阵。当和不随时间而变时，都是谱密度矩阵。可以证明，、与、之间存在下列关系：第18页/共192页

在的极限条件下，离散噪声序列和趋向于持续时间为零、幅值为无穷大的脉冲序列。而“脉冲”自相关函数与横轴所围的面积和分别等于连续白噪声脉冲自相关函数与横轴所围的面积和。第19页/共192页

也就是要求各状态变量估计误差的方差为最小。同时要求是的线性函数，并且估计是无偏的，即根据j和k的大小关系，离散系统估计问题也可分成三类；第一类：j>k，称为预测（或外推）问题；第二类：j=k，称为滤波问题；第三类：j<k称为平滑（或内插）问题。本章只讨论连续系统和离散系统的最优预测和最优滤波问题。第20页/共192页第三节离散系统卡尔曼最优预测基本方程的推导

在推导卡尔曼预测基本方程时，为了简便起见，先不考虑控制信号的作用，这样，离散系统的差分方程(12-11)变成(12-16)观测方程仍为式(12-12)：式中，和都均值为零的白噪声序列，和相互独立第21页/共192页在采样间隔内和为常值，其统计特性如式(12-13)所示，即状态向量的初值，其统计特性是给定的，即第22页/共192页要求估值是的线性函数，并且要求估计是无偏的，即给出观测序列，要求找出的线性最优估计，使得估计误差的方差为最小，即第23页/共192页

下面推导卡尔曼预测基本公式。推导的方法有几种，比较简易的方法是利用正交定理，用数学归纳法推导卡尔曼估计的基本递推估计公式。

当获得观测值之后，假定已经得到状态向量的一个最优线性预测估计。当还未获得时刻的新观测值时，根据已有的观测值，可得时刻的系统状态向量的两步预测估值：(12-17)第24页/共192页

由于是的一步最优线性估计，也是的最优线性预测估计，这可用正交定理来证明。由式(12-16)减式(12-17)，可得(12-18)式中，第25页/共192页

因为是的最优线性预测估值，根据正交定理，估计误差必须正交于，所以的线性变换也必须正交于。式(12-18)中的是均值为零的白噪声序列，与相互独立，因此正交于。所以在没有获得之前，是的最优两步线性预测。

在新观测值获取之后，可通过修正两步估值来得到的一步预测估值。第26页/共192页设的预测估值由式(12-12)减式(12-19)，得的预测估计误差为(12-19)第27页/共192页

造成的原因有两个：⑴对时刻状态向量的预测估计有误差；⑵附加了时刻的观测噪声干扰。显然包含修正的新信息。这样，在获得之后，在两步估值的基础上，用去修正，便可得到时刻状态向量的一步预测估计，即或(12-20)式中是待定矩阵，称为最优增益矩阵或加权矩阵。第28页/共192页式(12-20)可改写成

时刻系统状态方程为由式(12-22)减式(12-21)得的估计误差为(12-21)(12-22)(12-23)第29页/共192页

观察式(12-23)右边，、、均分别正交于，因此，正交于。

若与正交，则就是的一步最优线性预测估值。因此，利用与的正交条件：来确定最优增益矩阵。(12-24)第30页/共192页把式(12-23)和式(12-12)代入式(12-24)，得考虑到、、和相互间都是正交的，因此上式可简化为即第31页/共192页设，又有，代入上式后可得最优增益矩阵为(12-25)现在需确定估计误差方差阵的递推关系式。根据估计误差方差阵的定义，有第32页/共192页将式(12-23)代入上式得考虑到、和相互间都正交，可得：式中第33页/共192页将上式展开，经整理后得上式右边第四项与第五项之和为(12-26)第34页/共192页

显然，式(12-26)右边第四项与第五项之和在数值上等于第三项，但符号相反。这样，式(12-25)右边第三、第四和第五项之和为零，所以把式(12-25)的代入上式，可得估计误差方差阵的递推关系式为(12-27)第35页/共192页方程(12-20)、(12-25)和(12-27)构成一组完整的最优线性估计方程，现综合如下：⑴最优预测估计方程为式(12-20)，即⑵最优增益矩阵方程为(12-25)，即第36页/共192页⑶估计误差方差阵的递推方程为式(12-27)，即从式(12-27)可看出，估计误差方差阵与有关，而与观测值无关。因此，可事先估计误差方差阵，同时也可算出最优增益矩阵。第37页/共192页按式(12-16)和式(12-12)作出系统模型方块图，如图12-3所示。第38页/共192页图12-4表示由方程(12-21)所描述的卡尔曼最优预测估计方块图。第39页/共192页

从图12-4可看出，最优预测估计由三部分组成：⑴估值为观测值的线性函数；⑵最优增益矩；⑶单位负反馈回路。

现在需要验证第二节时最优估计提出的三项要求：⑴估值为观测值的线性函数；⑵估计值的方差为最小；⑶估值是无偏的，即第40页/共192页

在上面推导预测估计方程时，是按照⑴和⑵两项要求推导的，现需要说明估值是无偏的问题。对式(12-16)的两端取数学期望，考虑到，可得再对式(12-21)的两端取数学期望，考虑到，可得(12-28)(12-29)第41页/共192页将式(12-28)减式(12-29)，得到如果初始条件为或则(12-30)第42页/共192页根据式(12-30)的递推关系，可得因此

所以，只要初始估计选为，所得估计是无偏的。第43页/共192页卡尔曼预测估计递推方程的计算步骤如下：⑴在时刻给定初值：估值误差方差阵初值：⑵根据公式(12-25)计算时刻最优增益阵：第44页/共192页⑶根据公式(12-20)计算的最优估值：⑷根据公式(12-27)计算：⑸根据已知的计算时刻的。⑹根据计算的估值。重复上述递推计算步骤，可得第45页/共192页例12-1设系统状态方程和观测方程为

和都是均值为零的白噪声序列，且不相关，其统计特性如下：初值观测值试求的最优预测估值。第46页/共192页解:

与前面的有关方程对照，可得最优预测估值方程为最优增益矩阵为估值误差方差阵递推方程为第47页/共192页下面计算：第48页/共192页取的初值第49页/共192页第四节离散系统卡尔曼最优滤波基本方程的推导

系统状态方程、观测方程和噪声特性如式(12-16)、式(12-12)和式(12-13)所示。最优滤波问题简述如下：给出观测序列，要求找出的最优线性估计，使得估计误差的方差为最小，并且要求估计是无偏的。采用与上节类似的推导方法――数学归纳法和正交定理，导出最优滤波估计方程，即离散系统卡尔曼滤波方程。第50页/共192页推导的具体步骤如下：⑴假定由观测值估计得到状态向量的一步最优预测估值和观测向量的预测估值为:⑵当获得之后，求得与的误差，即⑶以去修正，得到的滤波估值为或(12-31)式中为待定的最优增益矩阵。第51页/共192页⑷求估计误差。式(12-31)可改写成则滤波估计误差为第52页/共192页⑸利用与正交求。

由于是的最优估值，的估计误差必须正交于。即

把和的表示式代入上式，并考虑到，，之间正交，可得第53页/共192页由上式直接得到最优增益矩阵：(12-32)第54页/共192页⑹按照估值误差方差阵定义推导的递推计算公式：整理并简化上式，可得滤波估值误差方差阵计算公式如下：(12-33)第55页/共192页⑺为了得到与之间的递推关系，式(12-31)中的可由下式计算得到：(12-34)⑻为了得到与的递推关系式，应将式(12-33)中的表示成的关系式。由式(12-34)求得的最优预测估值误差为第56页/共192页而于是(11_35)第57页/共192页

综上所述，方程(12-31)、(12-32)、(12-33)、(12-34)和(12-35)构成卡尔曼最优滤波基本方程组，与综合如下：第58页/共192页若给定初始统计特性及，要得到无偏估计，应取初值离散系统卡尔曼最优滤波的方块图如图12-5所示。第59页/共192页

从式(12-32)可分析和对的影响。当增大时，观测噪声大，观测值可靠度低，于是加权阵应取得小一些，以减弱观测噪声的影响。所以式(12-32)中随的增大而减小。当增大时，意味着第步转移的随机误差大，对状态预测修正应加强，于是应增大。从式(12-35)可知，当增大时，增大，也增大，表示对状态预测修正加强。第60页/共192页

在讨论卡尔曼滤波的特殊问题时，常需用到和的另一些表达式。根据式(12-32)可得以下公式：(12-36)(12-37)(12-38)(12-39)第61页/共192页根据式(12-33)可得(12-40)(12-41)将式(12-38)代入(12-40)得的另一表达式：(12-42)式(12-42)在形式上虽比式(12-41)简单，但当计算过程具有舍入误差时，容易失去对称性和非负定性，而式(12-41)具有较强的保持对称性和非负定性的能力。第62页/共192页

将式(12-42)展开，应用矩阵求逆引理（附录二），又可得的另一种形式：再由式(12-39)和式(12-42)可得(12-43)(12-44)(12-45)需要指出，从式(12-36)至式(12-45)都是对滤波情况而言。第63页/共192页

必须指出，在实际应用卡尔曼滤波算法时，每一步都要求和是对称的。虽然式(12-36)和式(12-35)在理论上是对称的，但是在运算过程中，有限字长和舍入误差可能引起和不对称，从而导致滤波系统的性能严重下降，甚至导致不稳定。这种情况当比较小时尤其明显。第64页/共192页例12-2设二阶系统模型和标量观测模型为输入噪声是平稳的，，测量噪声是非平稳的，

。换句话说，为偶数时的噪声比为奇数时的噪声大。假定初始状态的方差阵，欲计算。第65页/共192页解:

运用方程式(12-35)、(12-32)和初始条件可算得再由式(12-42)计算出验后方差阵为利用式(12-35)可计算下一步的验前方差阵为第66页/共192页然后再计算等。直到计算到所要求的为止。读者可按照上述方法继续计算下去，从计算结果可以看出，当为奇数时，由于测量噪声较小，所以较大；当为偶数时，则较小。在以后，增益就近似地达到周期性的稳态解。第67页/共192页第五节连续系统卡尔曼滤波基本方程的推导

在推导连续系统卡尔曼滤波基本方程时，也先不考虑控制信号的作用，这样系统的状态方程为式中，为n维状态向量，为系统噪声，其均值为零，为矩阵，为矩阵。(12-46)第68页/共192页观测方程为(12-47)式中，为维观测值向量，为矩阵，为维白噪声，其均值为零。假设和相互独立，它们的协方差阵为式中各符号的意义如本章第二节所述。第69页/共192页初始状态向量是一个随机向量，其均值和方差阵分别为在区间内已给出观测值，要求找出的最优线性估计，使得估计误差方差阵的迹为最小，即估计误差各分量方差之和为最小：第70页/共192页

要求估值是观测值的线性函数，并且要求估计是无偏的，即这里，只讨论预测和滤波问题，即。第71页/共192页

关于连续系统卡尔曼滤波公式的推导，采用卡尔曼在1962年提出的方法：离散系统的采样间隔，取离散型卡尔曼滤波方程的极限，连续型卡尔曼滤波方程，在离散型卡尔曼预测估计方程式(12-20)中，令采样间隔为，，则式(12-20)变为(12-48)由式(12-25)有第72页/共192页考虑到则(12-49)转移矩阵可用下面近似式表示(12-50)第73页/共192页把式(12-49)和式(12-50)代入到(12-48)，得到即第74页/共192页令，考虑到，对上式等号两边取极限，则得到连续系统的最优滤波方程：式中(12-51)(12-52)为最优增益矩阵，这里必须正定。第75页/共192页

根据估计误差方差阵的递推方程(12-27)，用同样的方法，并考虑到可得第76页/共192页考虑到则第77页/共192页即令，对上式等号两边取极限可得连续系统误差方差职的矩阵黎卡提微分方程：其初始条件为，解此方程可得。(12-53)第78页/共192页

估计误差方差的矩阵微分方程与和的二阶矩和有关，而与观测值无关。因此可事先算出方差阵，同时也可算出。

方程(12-51)、(12-52)和(12-53)构成一组连续系统的卡尔曼滤波方程。第79页/共192页下面讨论估计是否无偏的问题。如果初始条件选成则式(12-51)给出的估值是无偏的。由式(12-46)得由式(12-51)得第80页/共192页从方程(12-47)可得所以用减上式，可得第81页/共192页上式的解为

是转移矩阵，它是下列微分方程的解：

如果，就有。这样，估计是无偏的。第82页/共192页

根据式(12-46)和式(12-47)，可作出系统模型方块图，如图12-6所示。第83页/共192页

根据式(12-51)可作出连续系统卡尔曼滤波方块图，如图12-7所示。

从图12-7可看出：连续系统的卡尔曼滤波器是由三部分组成:⑴系统模型；⑵最优增益矩阵；⑶单位负反馈回路。第84页/共192页例12-3设系统状态方程和观测方程如下：

和都是均值为零的白噪声，其统计特性如下：设。试设计卡尔曼滤波器。第85页/共192页解与系统方程相比较，可得滤波方程为最优增益矩阵为估计误差方差阵为第86页/共192页令，则用分离变量法，可得第87页/共192页对上式积分，可得当时，代入上式可得，则从上式求得当比较大时，趋近于稳态值1。第88页/共192页最优增益系数为当比较大时，趋近于稳态值。

稳态值值也可以按下列方式求得，当较大时，则黎卡提微分方程转变为黎卡提代数方程：第89页/共192页因估计误差方差必为正值，故稳态的值为1。卡尔曼滤波方块图如图12-8所示。第90页/共192页

和随时间的变化曲线如图12-9所示。从该图可看出，即使是定常系统，最优增益系数K也是变系数。当时间不断增大，K趋近于某一稳态值。和的初始段与的初值()有很大关系，但和的稳态值与没有关系。第91页/共192页

当滤波达到稳态过程后，由于趋于某一常值，作为滤波器的输出，为滤波器的输入，则可得输出与输入之间的传递函数由传递函数可知，本例的卡尔曼滤波器为一惯性环节。第92页/共192页第六节系统噪声与观测噪声相关的卡尔曼滤波离散系统连续系统第93页/共192页一离散系统

系统的状态方程和观测方程与式(12-16)和式(12-12)完全相同，即

和都均值为零的白噪声序列，两者相关，则在和相关的情况下，最优估计方程的推导与前面所述的推导方法和步骤基本相同。第94页/共192页下面不加推导，直接写出结论。最优线性估计的卡尔曼预测方程为(12-54)(12-55)(12-56)第95页/共192页

将式(12-54)、(12-55)、(12-56)与第三节中的式(12-20)、(12-25)、(12-27)相比较，可得以下结论：⑴与相关情况下的最优预测估计方程(12-54)与两者不相关情况下的最优预测估计方程(12-20)完全相同的。⑵和的相关性只影响最优增益矩阵和估计误差方差阵。很明显，若，式(12-55)和(12-56)分别变成式(12-25)和式(12-27)。第96页/共192页二连续系统系统的状态方程和观测方程与式(12-46)和式(12-47)完全相同，即

和是均值为零的白噪声，且两者相关，则第97页/共192页

采用第五节所用的推导方法，可得和相关情况下的连续系统最优滤波方程、最优增益矩阵、估计误差方差阵。(12-57)(12-58 )(12-59)第98页/共192页

将式(12-57)、(12-58)、(12-59)与式(12-51)、(12-52)、(12-53)相比较，可得以下两点结论：⑴与相关情况下的最优滤波方程(12-57)和两者不相关情况下的最优滤波方程(12-51)完全相同，这说明与两者的相关性不影响估值方程。⑵和的相关性只影响计算公式，若则式(12-58)和(12-59)分别变成式(12-52)和式(12-53)。第99页/共192页

在上面讨论卡尔曼滤波基本方程时，为了简便起见，假定作用于系统的控制信号等于零。实际上，系统总会受到控制信号的作用。下面讨论具有输入控制信号的卡尔曼滤波方程。第七节具有输入信号的卡尔曼滤波一离散系统二连续系统第100页/共192页一离散系统系统的状态方程和测量方程为

式(12-60)中的为已知的非随机控制序列，式(12-61)中的为观测系统的系统误差项，也是已知的非随机序列。在采样间隔内，和都是常值。（12-60）（12-61）第101页/共192页

和的统计特性如下：

采用与前面相同的推导方法，可得到具有输入控制信号的卡尔曼滤波方程。下面不加推导地直接写出具有输入控制信号的最优预测的卡尔曼滤波方程。第102页/共192页（12-62）（12-63）（12-64）第103页/共192页将式(12-62)、(12-63)、(12-64)与式(12-54)、(12-55)、(12-56)相比较，可得以下两点结论：⑴当系统具有输入控制信号和测量系统误差时，它们只对的估值方程有影响，而对最优增益矩阵和估计误差方差阵的计算公式无任何影响。⑵和的相关性不影响的估值方程，只影响和的计算公式。因此式(12-63)与式(12-55)完全相同，式(12-64)与式(12-56)完全相同。第104页/共192页

具有输入控制信号的离散系统方块图和卡尔曼预测方块图如图12-10和图12-11所示。

具有输入控制信号的离散系统，其系统噪声和测量噪声均是白噪声，且两者相关。对于这样的系统，前面已讨论过直接考虑与相关的卡尔曼滤波方程的推导。这里通过工程实例，再介绍另一种将噪声相关问题转化为不相关问题处理的卡尔曼滤波方程的推导。第105页/共192页

例12-4在进行导弹容错控制设计时，需要利用导弹俯仰加速度回路和状态信息进行故障诊断，以判断回路中的加速度计是否发生故障。设某地空导弹俯仰通道加速度回路的数学模型为其中，状态变量，与弹体俯仰角速度率成比例，与弹体俯仰加速度率成比例，与弹体俯仰加速度的导数成比例。控制变量俯仰制导指令。输出变量为加速度计测得的弹体加速度。试设计卡尔曼滤波器，估计出状态变量。(12-65)(12-66)第106页/共192页

要求仿真计算1000步，初始条件：是周期为400步、幅值为1的方波信号。导弹俯仰通道加速度回路数学模型中的系数矩阵为第107页/共192页

和均为白噪声序列，且两者相关。其统计特性如下：第108页/共192页解⑴滤波方程的推导由式(12-66)可得将状态方程(12-65)变为下式：或其中为待定矩阵(12-67)第109页/共192页设将和代入式(12-67)，得新状态方程：式(12-68)中可视为新的控制项，可视为新的系统噪声，仍是白噪声。(12-68)第110页/共192页当，与不相关，必须即(12-69)第111页/共192页由此求得待定矩阵。将式(12-69)代入式(12-68)，可得

式(12-70)中，视为控制项。由式(12-70)和式(12-66)构成系统数学模型，直接利用前面系统噪声和测量噪声不相关的卡尔曼滤波基本方程组，即式(12-31)至式(12-35)，并考虑具有输入控制信号的影响，可得到具有输入控制信号，和相关情况下的卡尔曼滤波方程组。(12-70)第112页/共192页由式(12-31)写出由式(12-34)并考虑输入作用的影响，写出与的转移关系式，即(12-72)(12-71)第113页/共192页由式(12-32)和式(12-33)写出根据的定义，写出而(12-73)(12-74)第114页/共192页则即(12-75)第115页/共192页

至此，我们推导了具有输入控制和噪声相关情况的卡尔曼滤波方程组，下面就以此方程组计算例题的状态估计问题。⑵状态最优估值计算与数学仿真计算步骤：①令，组定状态最优估值初值估计误差方差阵初值。在本例计算中，选取=0，=diag[106，106，106]。②利用式(12-75)计算预测误差方差阵。③利用式(12-73)计算增益矩阵。第116页/共192页④利用式(12-72)计算的最优预测估值。⑤利用式(12-71)计算的最优滤波估值。⑥利用式(12-74)计算估计误差方差阵。⑦令增1，返回第2步，重复上述计算步骤。数字仿真流程图如图12-12所示。第117页/共192页⑶计算结果如图12-13、图12-14、图12-15所示。图中表示出输入控制信号为方波情况下的状态、和及其估值、和。图12－13第118页/共192页图12－14图12－15第119页/共192页二、连续系统具有输入控制信号的连续系统状态方程和观测方程如下：式中，为已知的非随机函数，为观测系统的系统误差项，也是已知的非随机函数，和都是均值为零的白噪声，并且两者是相关的。和的统计特性如下：(12-76)(12-77)第120页/共192页

在式(12-62)、式(12-63)和式(12-64)中，令采样间隔

,并考虑到

然后求极限，可得具有输入控制信号的连续系统的卡尔曼滤波方程：(12-78)第121页/共192页最优增益矩阵为估计误差方差阵为(12-79)(12-80)第122页/共192页将式(12-78)、式(12-79)、式(12-80)与式(12-57)、式(12-58)、式(12-59)相比较，可得以下两点结论：⑴当系统具有输入控制信号和测量系统误差时，它们只对的估值方程有影响，而对最优增益矩阵方程和估计方差阵的计算公式无任何影响。⑵和的相关性不影响的估值方程，只影响和的计算公式。因此式(12-79)与式(12-58)相同，式(12-80)与式(12-59)相同。第123页/共192页具有输入控制信号的连续系统方块图和卡尔曼滤波方块图如图12-16和图12－17所示。图12－16图12－17第124页/共192页第八节有色噪声情况下的卡尔曼滤波

前面推导卡尔曼滤波方程时，假定和都是白噪声。实际上，和可能是有色噪声。所谓白噪声，就是不同时刻的噪声都是互不相关的，而有色噪声是在不同时刻的噪声都是相关的。某些特定的有色噪声可用白噪声通过成形滤波器来表示。这样就可直接应用上面的滤波方程。下面将先介绍成形滤波器，然后再通过实例说明如何把某些特定的有色噪声用成形滤波器来处理的问题。第125页/共192页一连续系统的成形滤波器设是一平稳随机过程，其相关函数为式中，为常数，为时间间隔。将噪声的相关函数进行富氏变换，可得噪声的频谱密度为第126页/共192页可以将分解成由上式可直接写出相应的传递函数：第127页/共192页

显然，随机过程相当于单位强度白噪声（谱密度的白噪声称为单位强度白噪声）长时间作用于传递函数为的线性系统的结果。该系统是一个惯性环节，其时间常数为，放大系数为，可用图12-18表示。第128页/共192页

把单位强度白噪声转变为有色噪声的线性系统称为成形滤波器，上述的惯性环节即为成形滤波器。如果把上述成形滤波器的特性用微分方程表示，则有若令则成形滤波器方程为第129页/共192页式中为白噪声，且有

因此，对于某些特定的有色噪声，可用单位强度白噪声通过成形滤波器来表示。通过以上分析可知，如果有色噪声的相关函数是指数函数，则在谱密度中的最高次项为，则成形滤波器用一阶微分方程表示。第130页/共192页对于多维的有色噪声，如其相关函数为指数函数，则其成形滤波器方程可用一阶向量微分方程表示：式中，为多维有色噪声、为的系数矩阵，为均值等于零的白噪声过程，其统计特性为第131页/共192页

对于一般的平稳随机过程，如果谱密度为的有理函数，且能分解成

式中和是的多项式。和的根都在上半复数平面内，即和的根在复平面的左半部。则随机过程相当于单位强度白噪声长时间作用于频率特性为的线性系统的结果，则成形滤波器的传递函数为第132页/共192页二离散系统的成形滤波器设为一平稳随机序列，已知其相关函数为这里不加推导地写出成形滤波器方程如下：式中，为成形滤波器的转移矩阵：第133页/共192页

为均值不零的白噪声序列，其统计特性为第134页/共192页三系统中附加噪声的几种情况

上面讨论了控制系统和观测系统含有白噪声的卡尔曼滤波。下面讨论系统的附加噪声为有色噪声的卡尔曼滤波。有色噪声的卡尔曼滤波有三种情况：⑴控制系统附加噪声为有色噪声，观测系统噪声为白噪声；⑵控制附加噪声为白噪声，观测系统噪声为有色噪声；⑶控制系统和观测系统的附加噪声均为有色噪声。这里只讨论情况⑴和⑵和卡尔曼滤波。关于情况⑶的卡尔曼滤波，读者可把情况⑴和⑵的推导方法组合起来，进行情况⑶的卡尔曼滤波方程的推导，故在此不作讨论。第135页/共192页

四

控制系统含有有色噪声，观测系统含白噪声情况的卡尔曼滤波问题1.连续系统系统状态方程和观测方程如下：式中，是均值为零的有色噪声，是均值为零的白噪声。可用下列成形滤波器方程表示：(12-81)(12-82)(12-83)第136页/共192页设为扩大维数后的状态向量，即设为扩大状态变量维数后的系统附加噪声，即显然，中只包含白噪声。第137页/共192页扩大状态变量维数后的系统状态方程和观测系统方程为或式中(12-84)第138页/共192页或式中

式(12-84)和式(12-85)分别为扩大状态变量维数后的控制系统方程和观测系统方程。由于只包含白噪声，为白噪声，故可直接应用前面推导出的卡尔曼滤波方程。(12-85)第139页/共192页2.离散系统离散系统状态方程和观测系统方程如下：

式中，为有色白噪声序列，是均值为零的白噪声序列。可用下述成形滤波器方程来表示：式中为白噪声序列。(12-86)(12-87)(12-88)第140页/共192页

把作为状态变量的一部分，设为扩大维数后的状态变量，即设为扩大状态变量维数后的系统附加噪声，即显然，在中只包含白噪声。第141页/共192页扩大状态变量维数后的系统状态方程和观测系统方程为或式中(12-89)第142页/共192页或式中

式(12-89)和式(12-90)分别为扩大状态变量维数后的控制系统方程和观测系统方程。由于只包含白噪声，为白噪声，故可直接应用前面推导的离散系统卡尔曼滤波基本方程。(12-90)第143页/共192页

例12-5在进行导弹容错控制系统设计时，需要利用导弹俯仰通道阻尼回路的状态信息进行故障诊断，用来判断回路中速率陀螺仪是否发生故障。设某地空导弹俯仰通道阻尼回路的数学模型为其中，状态变量为与弹体俯仰角速率成比例的量，为与弹体俯仰角速率成比例的量。控制变量为俯仰制导指令，输出变量为由速率陀螺测得的弹体俯仰角速率。试设计卡尔曼滤波器，估计出状态变量。（注：与不相关）(12-91)(12-92)第144页/共192页要求仿真计算1000步，初始条件：，是周期为400步、幅值为1的方波信号。导弹俯仰通道阻尼回路数学模型中的转移矩阵为其他系数矩阵为第145页/共192页为有色噪声序列，其自相关函数为是均值为零的白噪声序列，其统计特性为第146页/共192页解⑴成形滤波器设计由自相关函数的形式，直接写出成形滤波器如下：

式中，，取采样周期秒，因此取。为白噪声，其统计特性为(12-93)第147页/共192页⑵构造扩大状态变量维数的状态方程和观测方程扩展状态变量扩展状态方程和观测方程为(12-94)(12-95)第148页/共192页式中

这样，我们得到了系统噪声与观测噪声均为白噪声序列、且互不相关的扩大状态变量维数的扩展系统，针对此扩展系统可直接利用前面所推导出的卡尔曼滤波方程。第149页/共192页⑶卡尔曼滤波方程组(12-96)(12-97)(12-98)(12-99)(12-100)第150页/共192页⑷计算步骤1.令，给定状态最优估值初值和估值误差方差阵初值P(0/0)。在本例计算中，选取。2.利用式(12-100)计算预测误差方差阵。3.利用式(12-98)计算最优增益矩阵。4.利用式(12-97)计算扩展状态的最优预测值。5.利用式(12-96)计算扩展状态的最优预测值6.利用式(12-99)计算扩展状态。7.令增1，返回第2步，重复上述计算步骤。第151页/共192页⑸数学仿真仿真程序流程图如图12-19所示。计算结果如图12-20和图12-21所示。第152页/共192页五、控制系统含白噪声、观测系统含有色噪声情况的卡尔曼滤波问题1.连续系统系统状态方程为观测方程为式中，是均值为零的白噪声，其统计特性为(12-101)(12-102)第153页/共192页

是均值为零的正态分布有色噪声，可用成形滤皮器方程表示如下：式中，是均值为零的白噪声，其统计特性为

与互不相关，即。(12-103)第154页/共192页

现设法改变观测方程(12-102)，使等效观测方程的附加噪声为白噪声。考察方程(12-103)可看出是白噪声，从这一点出发，引入一个等效观测值则也是白噪声。(12-106)(12-105)第155页/共192页依据式(12-102)和式(12-103)，可将式(12-105)变为式中(12-106)第156页/共192页

这样，把观测方程(12-102)转变成等效观测方程(12-106)。式(12-106)的附加噪声为白噪声，因与都是均值为零的白噪声，则也是均值为零的白噪声。考虑到与的不相关性，的统计特性如下：式中第157页/共192页由于中包含，因此和是相关的，故式中由于等效观测方程(12-106)的附加噪声为白噪声，因此完全可用前面推导出的系统噪声和观测噪声相关的连续系统的卡尔曼滤波公式(12-57)、式(12-58)和式(12-59)。第158页/共192页根据式(12-57)最优滤波方程为把和代入上式，可得有色噪声情况的最优滤波方程为(12-107)第159页/共192页根据式(12-58)，最优增益矩阵为

把和以及代入上式，可得有色噪声情况的最优增益矩阵为(12-108)第160页/共192页根据式(12-59)，估计误差方差阵为把和及代入上式，可得有色噪声情况的估计误差方差阵方程(12-109)第161页/共192页

从式(12-108)可看出，为了使滤波的存在，要求矩阵为正定矩阵。2.离散系统系统状态方差为式中，是均值为零的白噪声序列，其统计特性为(12-110)第162页/共192页观测方程为式中是均值为零的正态分布有色噪声序列，可用成形滤波器方程表示如下：式中是均值为零的白噪声序列，其统计特性：(12-112)(12-111)第163页/共192页下面采用与处理连续系统的方法相同，设法改变观测方程，使等效观测方程的附加噪声为白噪声，然后直接应用前面已推导出的卡尔曼滤波方程。把式(12-112)代入式(12-111)，得再把式(12-111)改写为以乘上式两边，得(12-114)(12-113)第164页/共192页

将式(12-113)减式(12-114)，并令，则把式(12-110)代入上式可得式(12-115)为等效观测方程，是等效观测值。(12-115)第165页/共192页等效观测方程与原始观测方程相比较，有两个特点：

第一，等效观测值只包含白噪声，它与系统附加噪声是相关的；

第二，在形式上看作时刻的观测值，看起来是线性函数，实际上却是的线性函数。

因此，针对系统状态方程(12-110)和等效观测方程(12-115)的滤波方程的估值和估计误差方差阵，就是针对式(12-110)和式(12-111)的估值及。第166页/共192页

由于等效观测方程的白噪声与系统附加噪声相关，因此可用滤波方程式(12-20)、式(12-25)和式(12-27)来处理。将式(12-115)与式(12-12)相比较可知：(12-117)(12-118)(12-119)第167页/共192页另外，考虑到等效观测方程(12-115)的估值及是原观测方程(12-111)的对应估值及。因为在中包含观测值的信息，这样可得有色噪声情况下的离散系统滤波方程如下：(12-119)(12-120)(12-121)第168页/共192页式(12-119)中式(12-120)和式(12-121)中的，分别由式(12-116)、式(12-117)和式(12-118)表示。第169页/共192页

应当考虑初始值对估值的影响问题。把的初值对估值的影响归结为的初始估值对的影响。若第一次测量时间，验前估算为及，并且。根据线性最小方差估计，和的初值为最优滤波方块图如图12-22所示。第170页/共192页以上用改变观测方程的方法，避免了扩大状态变量维数，这种方法在控制和导航等方面都可能应用。第171页/共192页第九节滤波的稳定性概念和滤波的发散问题一、滤波的稳定性概念二、滤波的发散问题第172页/共192页一、滤波的稳定性概念

前面比较详细地讨论了线性系统卡尔曼滤波基本方程的推导和递推计算步骤。在估值计算时，需要利用一连串的观测数据，按照滤波基本方程进步递推计算，可得状态向量的最优估值。这里，需要确切知道的初值和估计误差方差阵的初值。但在许多实际问题中，往往不可能确切知道初值和，甚至根本不知道这些初值。为了进步滤波计算，只能假定初值和。这对滤波结果会造成什么影响呢？第173页/共192页

如果用正确的初值和，按滤波方程可得最优的滤波值及；如果选取不确切的初值和，可得非最优的和，在时间充分长之后，如果能收敛于，能收敛于，则初值不确切的影响可忽略不计。这种情况下，滤波是稳定的。反之，滤波是不稳定的。滤波是否稳定，与滤波系统的