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

第一节维纳滤波

12.1.1、维纳滤波问题的提法12.1.2、维纳-霍夫积分方程第一页第二页，共191页。维纳滤波问题的提法设系统的观测方程为式中，为有用信号为观测信号为观测误差。设、和都是均值为零并具有各态历经性的平稳随机过程（附录四）。根据观测值估计，使估值接近于。维纳滤波的任务就是设计出一个线性定常系统L，如图12-1所示，使得系统的输出与具有最小方差，即(12-3)这样就作为的估值。第二页第三页，共191页。根据问题的性质，维纳滤波有下列三个条件：⑴信号与噪声都是均值为零并具有各态历经性的平稳随机过程；⑵滤波器是一个物理可实现的线性定常系统。当时，；⑶最优准则是滤波的方差为最小。这些条件使维纳滤波受到很大限制。如果系统的脉冲过渡函数为，则(12-4)是系统L根据输入信号在上的全部过去值所给出的实际输出，如图12-2所示。是的线性函数。第三页第四页，共191页。维纳-霍夫积分方程维纳-霍夫积分方程是确定最优滤波器脉冲过渡函数的一个方程式。根据正交定理（附录五），估计误差应与观测值正交，即(12-5)把上面两式代入式(12-5)，可得第四页第五页，共191页。维纳滤波在随机控制领域中是一个很大的突破，但很少被应用，这主要有如下两方面原因：⑴维纳－霍夫积分方程很难解，即使求出了最优滤波器的脉冲过渡函数，在工程上往往很难实现；⑵维纳理论要求所有的随机过程都平稳的，这与工程实际问题往往不相符合。(12-6)这就是维纳-霍夫积分方程，解此方程可得最优滤波器的脉冲过渡函数。把上面两式代入式(12-5)，可得第五页第六页，共191页。卡尔曼在1960年提出了另一种适合于数字计算机计算的递推滤波法，即所谓的卡尔曼滤波。这种滤波方法不需要求解积分方程，既适用于平稳随机过程，也适用于非平稳随机过程，是一种有广泛应用价值的工程方法。卡尔曼维纳第六页第七页，共191页。第二节卡尔曼滤波问题的提法

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

(12-7)

第七页第八页，共191页。式中，是控制系统的n维状态向量，是r维控制向量，假定是均值为零的p维白噪声向量，是n*n矩阵，是n*r矩阵，是n*p矩阵。对于实际控制系统，最优控制律或自适应控制律的形成需要系统的状态变量，而状态变量往往不能直接获得，需要通过测量装置进行观测，根据观测得到的信号来确定状态变量。但测量装置中一般都存在随机干扰。因此在观测得到的信号中夹杂有随机噪声。第八页第九页，共191页。式中，是m维观测向量,是m*n矩阵，称为观测矩阵。假定是均值为零的m维白噪声，和相互独立，它们的协方差阵分别为要从夹杂有随机噪声的观测信号中准确地分离出状态变量是不可能的，只有根据观测信号来估计这些状态变量。通常，观测系统的观测方程为（12-8）（12-9）（12-10)第九页第十页，共191页。式中的是狄拉克(Dirac)函数，当时，；时；且。当和不随时间而变化时，Q和R都是白噪声的谱密度矩阵。为对称的非负定矩阵，为正定的对称矩阵。正定的物理意义是观测向量的各分量都附加有随机噪声。和都可对t微分。的初始状态是一个随机向量。第十页第十一页，共191页。现在的任务是从观测信号中估计出状态变量，希望估计出来的值与实际的值愈接近愈好，因此提出最优估计问题。一般都采用线性最小方差估计。假定的数学期望，方差矩阵都为已知。第十一页第十二页，共191页。线性最小方差估计问题可阐述如下：假定线性控制过程如式(12-7)所示，观测方程如式(12-8)所示。从时间开始得到观测值，在区间内已给出观测值。要求找出的最优线性估计。这里记号“”表示利用时刻t以前的观测值来估计时刻的值。所谓最优线性估计包含以下三点意义：⑴估值是的线性函数。第十二页第十三页，共191页。⑶要求估值误差的方差为最小，即要求⑵估值是无偏的，即第十三页第十四页，共191页。比较起来预测问题稍为简单一些，平滑问题最复杂。通常讲的卡尔曼滤波指的是预测和滤波。第一类：>称为预测（或外推）问题根据和的大小关系，连续系统估计问题可分为三类：第二类：=，称为滤波问题；第三类：<称为平滑（或内插）问题。第十四页第十五页，共191页。设离散系统的差分方程和观测方程分别为（12-11）（12-12）式中，是n维状态向量，是r维控制向量，是m维观测向量，是n*n转移矩阵，是n*r矩阵，是n*p矩阵，是m*n矩阵。下面讨论离散系统的卡尔曼滤波问题。第十五页第十六页，共191页。（12-13）式中，为克罗尼克(Kroneker)函数，其特性是假定是均值为零的p维白噪声向量序列，是均值为零的m维的白噪声向量序列，和相互独立，在采样间隔内和都为常值，其统计特性如下：第十六页第十七页，共191页。（1）(12-14)（2）(12-15)为非负定矩阵，为正定矩阵。和都是方差阵，而和不是方差阵。当和不随时间而变时，都是谱密度矩阵。可以证明，、与、之间存在下列关系：第十七页第十八页，共191页。在的极限条件下，离散噪声序列和趋向于持续时间为零、幅值为无穷大的脉冲序列。而“脉冲”自相关函数与横轴所围的面积和分别等于连续白噪声脉冲自相关函数与横轴所围的面积和。第十八页第十九页，共191页。也就是要求各状态变量估计误差的方差为最小。同时要求是的线性函数，并且估计是无偏的，即根据j和k的大小关系，离散系统估计问题也可分成三类；第一类：j>k，称为预测（或外推）问题；第二类：j=k，称为滤波问题；第三类：j<k称为平滑（或内插）问题。本章只讨论连续系统和离散系统的最优预测和最优滤波问题。第十九页第二十页，共191页。第三节离散系统卡尔曼最优预测基本方程的推导在推导卡尔曼预测基本方程时，为了简便起见，先不考虑控制信号的作用，这样，离散系统的差分方程(12-11)变成(12-16)观测方程仍为式(12-12)：式中，和都均值为零的白噪声序列，和相互独立第二十页第二十一页，共191页。在采样间隔内和为常值，其统计特性如式(12-13)所示，即状态向量的初值，其统计特性是给定的，即第二十一页第二十二页，共191页。要求估值是的线性函数，并且要求估计是无偏的，即给出观测序列，要求找出的线性最优估计，使得估计误差的方差为最小，即第二十二页第二十三页，共191页。下面推导卡尔曼预测基本公式。推导的方法有几种，比较简易的方法是利用正交定理，用数学归纳法推导卡尔曼估计的基本递推估计公式。当获得观测值之后，假定已经得到状态向量的一个最优线性预测估计。当还未获得时刻的新观测值时，根据已有的观测值，可得时刻的系统状态向量的两步预测估值：(12-17)第二十三页第二十四页，共191页。由于是的一步最优线性估计，也是的最优线性预测估计，这可用正交定理来证明。由式(12-16)减式(12-17)，可得(12-18)式中，第二十四页第二十五页，共191页。因为是的最优线性预测估值，根据正交定理，估计误差必须正交于，所以的线性变换也必须正交于。式(12-18)中的是均值为零的白噪声序列，与相互独立，因此正交于。所以在没有获得之前，是的最优两步线性预测。在新观测值获取之后，可通过修正两步估值来得到的一步预测估值。第二十五页第二十六页，共191页。设的预测估值由式(12-12)减式(12-19)，得的预测估计误差为(12-19)第二十六页第二十七页，共191页。造成的原因有两个：⑴对时刻状态向量的预测估计有误差；⑵附加了时刻的观测噪声干扰。显然包含修正的新信息。这样，在获得之后，在两步估值的基础上，用去修正，便可得到时刻状态向量的一步预测估计，即或(12-20)式中是待定矩阵，称为最优增益矩阵或加权矩阵。第二十七页第二十八页，共191页。式(12-20)可改写成时刻系统状态方程为由式(12-22)减式(12-21)得的估计误差为(12-21)(12-22)(12-23)第二十八页第二十九页，共191页。观察式(12-23)右边，、、均分别正交于，因此，正交于。若与正交，则就是的一步最优线性预测估值。因此，利用与的正交条件：来确定最优增益矩阵。(12-24)第二十九页第三十页，共191页。把式(12-23)和式(12-12)代入式(12-24)，得考虑到、、和相互间都是正交的，因此上式可简化为即第三十页第三十一页，共191页。设，又有，代入上式后可得最优增益矩阵为(12-25)现在需确定估计误差方差阵的递推关系式。根据估计误差方差阵的定义，有第三十一页第三十二页，共191页。将式(12-23)代入上式得考虑到、和相互间都正交，可得：式中第三十二页第三十三页，共191页。将上式展开，经整理后得上式右边第四项与第五项之和为(12-26)第三十三页第三十四页，共191页。显然，式(12-26)右边第四项与第五项之和在数值上等于第三项，但符号相反。这样，式(12-25)右边第三、第四和第五项之和为零，所以把式(12-25)的代入上式，可得估计误差方差阵的递推关系式为(12-27)第三十四页第三十五页，共191页。方程(12-20)、(12-25)和(12-27)构成一组完整的最优线性估计方程，现综合如下：⑴最优预测估计方程为式(12-20)，即⑵最优增益矩阵方程为(12-25)，即第三十五页第三十六页，共191页。⑶估计误差方差阵的递推方程为式(12-27)，即从式(12-27)可看出，估计误差方差阵与有关，而与观测值无关。因此，可事先估计误差方差阵，同时也可算出最优增益矩阵。第三十六页第三十七页，共191页。按式(12-16)和式(12-12)作出系统模型方块图，如图12-3所示。第三十七页第三十八页，共191页。图12-4表示由方程(12-21)所描述的卡尔曼最优预测估计方块图。第三十八页第三十九页，共191页。从图12-4可看出，最优预测估计由三部分组成：⑴估值为观测值的线性函数；⑵最优增益矩；⑶单位负反馈回路。现在需要验证第二节时最优估计提出的三项要求：⑴估值为观测值的线性函数；⑵估计值的方差为最小；⑶估值是无偏的，即第三十九页第四十页，共191页。在上面推导预测估计方程时，是按照⑴和⑵两项要求推导的，现需要说明估值是无偏的问题。对式(12-16)的两端取数学期望，考虑到，可得再对式(12-21)的两端取数学期望，考虑到，可得(12-28)(12-29)第四十页第四十一页，共191页。将式(12-28)减式(12-29)，得到如果初始条件为或则(12-30)第四十一页第四十二页，共191页。根据式(12-30)的递推关系，可得因此所以，只要初始估计选为，所得估计是无偏的。第四十二页第四十三页，共191页。卡尔曼预测估计递推方程的计算步骤如下：⑴在时刻给定初值：估值误差方差阵初值：⑵根据公式(12-25)计算时刻最优增益阵：第四十三页第四十四页，共191页。⑶根据公式(12-20)计算的最优估值：⑷根据公式(12-27)计算：⑸根据已知的计算时刻的。⑹根据计算的估值。重复上述递推计算步骤，可得第四十四页第四十五页，共191页。例12-1设系统状态方程和观测方程为和都是均值为零的白噪声序列，且不相关，其统计特性如下：初值观测值试求的最优预测估值。第四十五页第四十六页，共191页。解:与前面的有关方程对照，可得最优预测估值方程为最优增益矩阵为估值误差方差阵递推方程为第四十六页第四十七页，共191页。下面计算：第四十七页第四十八页，共191页。取的初值第四十八页第四十九页，共191页。第四节离散系统卡尔曼最优滤波基本方程的推导系统状态方程、观测方程和噪声特性如式(12-16)、式(12-12)和式(12-13)所示。最优滤波问题简述如下：给出观测序列，要求找出的最优线性估计，使得估计误差的方差为最小，并且要求估计是无偏的。采用与上节类似的推导方法――数学归纳法和正交定理，导出最优滤波估计方程，即离散系统卡尔曼滤波方程。第四十九页第五十页，共191页。推导的具体步骤如下：⑴假定由观测值估计得到状态向量的一步最优预测估值和观测向量的预测估值为:⑵当获得之后，求得与的误差，即⑶以去修正，得到的滤波估值为或(12-31)式中为待定的最优增益矩阵。第五十页第五十一页，共191页。⑷求估计误差。式(12-31)可改写成则滤波估计误差为第五十一页第五十二页，共191页。⑸利用与正交求。由于是的最优估值，的估计误差必须正交于。即把和的表示式代入上式，并考虑到，，之间正交，可得第五十二页第五十三页，共191页。由上式直接得到最优增益矩阵：(12-32)第五十三页第五十四页，共191页。⑹按照估值误差方差阵定义推导的递推计算公式：整理并简化上式，可得滤波估值误差方差阵计算公式如下：(12-33)第五十四页第五十五页，共191页。⑺为了得到与之间的递推关系，式(12-31)中的可由下式计算得到：(12-34)⑻为了得到与的递推关系式，应将式(12-33)中的表示成的关系式。由式(12-34)求得的最优预测估值误差为第五十五页第五十六页，共191页。而于是(11_35)第五十六页第五十七页，共191页。综上所述，方程(12-31)、(12-32)、(12-33)、(12-34)和(12-35)构成卡尔曼最优滤波基本方程组，与综合如下：第五十七页第五十八页，共191页。若给定初始统计特性及，要得到无偏估计，应取初值离散系统卡尔曼最优滤波的方块图如图12-5所示。第五十八页第五十九页，共191页。从式(12-32)可分析和对的影响。当增大时，观测噪声大，观测值可靠度低，于是加权阵应取得小一些，以减弱观测噪声的影响。所以式(12-32)中随的增大而减小。当增大时，意味着第步转移的随机误差大，对状态预测修正应加强，于是应增大。从式(12-35)可知，当增大时，增大，也增大，表示对状态预测修正加强。第五十九页第六十页，共191页。在讨论卡尔曼滤波的特殊问题时，常需用到和的另一些表达式。根据式(12-32)可得以下公式：(12-36)(12-37)(12-38)(12-39)第六十页第六十一页，共191页。根据式(12-33)可得(12-40)(12-41)将式(12-38)代入(12-40)得的另一表达式：(12-42)式(12-42)在形式上虽比式(12-41)简单，但当计算过程具有舍入误差时，容易失去对称性和非负定性，而式(12-41)具有较强的保持对称性和非负定性的能力。第六十一页第六十二页，共191页。将式(12-42)展开，应用矩阵求逆引理（附录二），又可得的另一种形式：再由式(12-39)和式(12-42)可得(12-43)(12-44)(12-45)需要指出，从式(12-36)至式(12-45)都是对滤波情况而言。第六十二页第六十三页，共191页。必须指出，在实际应用卡尔曼滤波算法时，每一步都要求和是对称的。虽然式(12-36)和式(12-35)在理论上是对称的，但是在运算过程中，有限字长和舍入误差可能引起和不对称，从而导致滤波系统的性能严重下降，甚至导致不稳定。这种情况当比较小时尤其明显。第六十三页第六十四页，共191页。例12-2设二阶系统模型和标量观测模型为输入噪声是平稳的，，测量噪声是非平稳的，。换句话说，为偶数时的噪声比为奇数时的噪声大。假定初始状态的方差阵，欲计算。第六十四页第六十五页，共191页。解:运用方程式(12-35)、(12-32)和初始条件可算得再由式(12-42)计算出验后方差阵为利用式(12-35)可计算下一步的验前方差阵为第六十五页第六十六页，共191页。然后再计算等。直到计算到所要求的为止。读者可按照上述方法继续计算下去，从计算结果可以看出，当为奇数时，由于测量噪声较小，所以较大；当为偶数时，则较小。在以后，增益就近似地达到周期性的稳态解。第六十六页第六十七页，共191页。第五节连续系统卡尔曼滤波基本方程的推导在推导连续系统卡尔曼滤波基本方程时，也先不考虑控制信号的作用，这样系统的状态方程为式中，为n维状态向量，为系统噪声，其均值为零，为矩阵，为矩阵。(12-46)第六十七页第六十八页，共191页。观测方程为(12-47)式中，为维观测值向量，为矩阵，为维白噪声，其均值为零。假设和相互独立，它们的协方差阵为式中各符号的意义如本章第二节所述。第六十八页第六十九页，共191页。初始状态向量是一个随机向量，其均值和方差阵分别为在区间内已给出观测值，要求找出的最优线性估计，使得估计误差方差阵的迹为最小，即估计误差各分量方差之和为最小：第六十九页第七十页，共191页。要求估值是观测值的线性函数，并且要求估计是无偏的，即这里，只讨论预测和滤波问题，即。第七十页第七十一页，共191页。关于连续系统卡尔曼滤波公式的推导，采用卡尔曼在1962年提出的方法：离散系统的采样间隔，取离散型卡尔曼滤波方程的极限，连续型卡尔曼滤波方程，在离散型卡尔曼预测估计方程式(12-20)中，令采样间隔为，，则式(12-20)变为(12-48)由式(12-25)有第七十一页第七十二页，共191页。考虑到则(12-49)转移矩阵可用下面近似式表示(12-50)第七十二页第七十三页，共191页。把式(12-49)和式(12-50)代入到(12-48)，得到即第七十三页第七十四页，共191页。令，考虑到，对上式等号两边取极限，则得到连续系统的最优滤波方程：式中(12-51)(12-52)为最优增益矩阵，这里必须正定。第七十四页第七十五页，共191页。根据估计误差方差阵的递推方程(12-27)，用同样的方法，并考虑到可得第七十五页第七十六页，共191页。考虑到则第七十六页第七十七页，共191页。即令，对上式等号两边取极限可得连续系统误差方差职的矩阵黎卡提微分方程：其初始条件为，解此方程可得。(12-53)第七十七页第七十八页，共191页。估计误差方差的矩阵微分方程与和的二阶矩和有关，而与观测值无关。因此可事先算出方差阵，同时也可算出。方程(12-51)、(12-52)和(12-53)构成一组连续系统的卡尔曼滤波方程。第七十八页第七十九页，共191页。下面讨论估计是否无偏的问题。如果初始条件选成则式(12-51)给出的估值是无偏的。由式(12-46)得由式(12-51)得第七十九页第八十页，共191页。从方程(12-47)可得所以用减上式，可得第八十页第八十一页，共191页。上式的解为是转移矩阵，它是下列微分方程的解：如果，就有。这样，估计是无偏的。第八十一页第八十二页，共191页。根据式(12-46)和式(12-47)，可作出系统模型方块图，如图12-6所示。第八十二页第八十三页，共191页。根据式(12-51)可作出连续系统卡尔曼滤波方块图，如图12-7所示。从图12-7可看出：连续系统的卡尔曼滤波器是由三部分组成:⑴系统模型；⑵最优增益矩阵；⑶单位负反馈回路。第八十三页第八十四页，共191页。例12-3设系统状态方程和观测方程如下：和都是均值为零的白噪声，其统计特性如下：设。试设计卡尔曼滤波器。第八十四页第八十五页，共191页。解与系统方程相比较，可得滤波方程为最优增益矩阵为估计误差方差阵为第八十五页第八十六页，共191页。令，则用分离变量法，可得第八十六页第八十七页，共191页。对上式积分，可得当时，代入上式可得，则从上式求得当比较大时，趋近于稳态值1。第八十七页第八十八页，共191页。最优增益系数为当比较大时，趋近于稳态值。稳态值值也可以按下列方式求得，当较大时，则黎卡提微分方程转变为黎卡提代数方程：第八十八页第八十九页，共191页。因估计误差方差必为正值，故稳态的值为1。卡尔曼滤波方块图如图12-8所示。第八十九页第九十页，共191页。和随时间的变化曲线如图12-9所示。从该图可看出，即使是定常系统，最优增益系数K也是变系数。当时间不断增大，K趋近于某一稳态值。和的初始段与的初值()有很大关系，但和的稳态值与没有关系。第九十页第九十一页，共191页。当滤波达到稳态过程后，由于趋于某一常值，作为滤波器的输出，为滤波器的输入，则可得输出与输入之间的传递函数由传递函数可知，本例的卡尔曼滤波器为一惯性环节。第九十一页第九十二页，共191页。第六节系统噪声与观测噪声相关的卡尔曼滤波离散系统连续系统第九十二页第九十三页，共191页。一离散系统系统的状态方程和观测方程与式(12-16)和式(12-12)完全相同，即和都均值为零的白噪声序列，两者相关，则在和相关的情况下，最优估计方程的推导与前面所述的推导方法和步骤基本相同。第九十三页第九十四页，共191页。下面不加推导，直接写出结论。最优线性估计的卡尔曼预测方程为(12-54)(12-55)(12-56)第九十四页第九十五页，共191页。将式(12-54)、(12-55)、(12-56)与第三节中的式(12-20)、(12-25)、(12-27)相比较，可得以下结论：⑴与相关情况下的最优预测估计方程(12-54)与两者不相关情况下的最优预测估计方程(12-20)完全相同的。⑵和的相关性只影响最优增益矩阵和估计误差方差阵。很明显，若，式(12-55)和(12-56)分别变成式(12-25)和式(12-27)。第九十五页第九十六页，共191页。二连续系统系统的状态方程和观测方程与式(12-46)和式(12-47)完全相同，即和是均值为零的白噪声，且两者相关，则第九十六页第九十七页，共191页。采用第五节所用的推导方法，可得和相关情况下的连续系统最优滤波方程、最优增益矩阵、估计误差方差阵。(12-57)(12-58 )(12-59)第九十七页第九十八页，共191页。将式(12-57)、(12-58)、(12-59)与式(12-51)、(12-52)、(12-53)相比较，可得以下两点结论：⑴与相关情况下的最优滤波方程(12-57)和两者不相关情况下的最优滤波方程(12-51)完全相同，这说明与两者的相关性不影响估值方程。⑵和的相关性只影响计算公式，若则式(12-58)和(12-59)分别变成式(12-52)和式(12-53)。第九十八页第九十九页，共191页。在上面讨论卡尔曼滤波基本方程时，为了简便起见，假定作用于系统的控制信号等于零。实际上，系统总会受到控制信号的作用。下面讨论具有输入控制信号的卡尔曼滤波方程。第七节具有输入信号的卡尔曼滤波一离散系统二连续系统第九十九页第一百页，共191页。一离散系统系统的状态方程和测量方程为式(12-60)中的为已知的非随机控制序列，式(12-61)中的为观测系统的系统误差项，也是已知的非随机序列。在采样间隔内，和都是常值。（12-60）（12-61）第一百页第一百零一页，共191页。和的统计特性如下：采用与前面相同的推导方法，可得到具有输入控制信号的卡尔曼滤波方程。下面不加推导地直接写出具有输入控制信号的最优预测的卡尔曼滤波方程。第一百零一页第一百零二页，共191页。（12-62）（12-63）（12-64）第一百零二页第一百零三页，共191页。将式(12-62)、(12-63)、(12-64)与式(12-54)、(12-55)、(12-56)相比较，可得以下两点结论：⑴当系统具有输入控制信号和测量系统误差时，它们只对的估值方程有影响，而对最优增益矩阵和估计误差方差阵的计算公式无任何影响。⑵和的相关性不影响的估值方程，只影响和的计算公式。因此式(12-63)与式(12-55)完全相同，式(12-64)与式(12-56)完全相同。第一百零三页第一百零四页，共191页。具有输入控制信号的离散系统方块图和卡尔曼预测方块图如图12-10和图12-11所示。具有输入控制信号的离散系统，其系统噪声和测量噪声均是白噪声，且两者相关。对于这样的系统，前面已讨论过直接考虑与相关的卡尔曼滤波方程的推导。这里通过工程实例，再介绍另一种将噪声相关问题转化为不相关问题处理的卡尔曼滤波方程的推导。第一百零四页第一百零五页，共191页。例12-4在进行导弹容错控制设计时，需要利用导弹俯仰加速度回路和状态信息进行故障诊断，以判断回路中的加速度计是否发生故障。设某地空导弹俯仰通道加速度回路的数学模型为其中，状态变量，与弹体俯仰角速度率成比例，与弹体俯仰加速度率成比例，与弹体俯仰加速度的导数成比例。控制变量俯仰制导指令。输出变量为加速度计测得的弹体加速度。试设计卡尔曼滤波器，估计出状态变量。(12-65)(12-66)第一百零五页第一百零六页，共191页。要求仿真计算1000步，初始条件：是周期为400步、幅值为1的方波信号。导弹俯仰通道加速度回路数学模型中的系数矩阵为第一百零六页第一百零七页，共191页。和均为白噪声序列，且两者相关。其统计特性如下：第一百零七页第一百零八页，共191页。解⑴滤波方程的推导由式(12-66)可得将状态方程(12-65)变为下式：或其中为待定矩阵(12-67)第一百零八页第一百零九页，共191页。设将和代入式(12-67)，得新状态方程：式(12-68)中可视为新的控制项，可视为新的系统噪声，仍是白噪声。(12-68)第一百零九页第一百一十页，共191页。当，与不相关，必须即(12-69)第一百一十页第一百一十一页，共191页。由此求得待定矩阵。将式(12-69)代入式(12-68)，可得式(12-70)中，视为控制项。由式(12-70)和式(12-66)构成系统数学模型，直接利用前面系统噪声和测量噪声不相关的卡尔曼滤波基本方程组，即式(12-31)至式(12-35)，并考虑具有输入控制信号的影响，可得到具有输入控制信号，和相关情况下的卡尔曼滤波方程组。(12-70)第一百一十一页第一百一十二页，共191页。由式(12-31)写出由式(12-34)并考虑输入作用的影响，写出与的转移关系式，即(12-72)(12-71)第一百一十二页第一百一十三页，共191页。由式(12-32)和式(12-33)写出根据的定义，写出而(12-73)(12-74)第一百一十三页第一百一十四页，共191页。则即(12-75)第一百一十四页第一百一十五页，共191页。至此，我们推导了具有输入控制和噪声相关情况的卡尔曼滤波方程组，下面就以此方程组计算例题的状态估计问题。⑵状态最优估值计算与数学仿真计算步骤：①令，组定状态最优估值初值估计误差方差阵初值。在本例计算中，选取=0，=diag[106，106，106]。②利用式(12-75)计算预测误差方差阵。③利用式(12-73)计算增益矩阵。第一百一十五页第一百一十六页，共191页。④利用式(12-72)计算的最优预测估值。⑤利用式(12-71)计算的最优滤波估值。⑥利用式(12-74)计算估计误差方差阵。⑦令增1，返回第2步，重复上述计算步骤。数字仿真流程图如图12-12所示。第一百一十六页第一百一十七页，共191页。⑶计算结果如图12-13、图12-14、图12-15所示。图中表示出输入控制信号为方波情况下的状态、和及其估值、和。图12－13第一百一十七页第一百一十八页，共191页。图12－14图12－15第一百一十八页第一百一十九页，共191页。二、连续系统具有输入控制信号的连续系统状态方程和观测方程如下：式中，为已知的非随机函数，为观测系统的系统误差项，也是已知的非随机函数，和都是均值为零的白噪声，并且两者是相关的。和的统计特性如下：(12-76)(12-77)第一百一十九页第一百二十页，共191页。在式(12-62)、式(12-63)和式(12-64)中，令采样间隔,并考虑到然后求极限，可得具有输入控制信号的连续系统的卡尔曼滤波方程：(12-78)第一百二十页第一百二十一页，共191页。最优增益矩阵为估计误差方差阵为(12-79)(12-80)第一百二十一页第一百二十二页，共191页。将式(12-78)、式(12-79)、式(12-80)与式(12-57)、式(12-58)、式(12-59)相比较，可得以下两点结论：⑴当系统具有输入控制信号和测量系统误差时，它们只对的估值方程有影响，而对最优增益矩阵方程和估计方差阵的计算公式无任何影响。⑵和的相关性不影响的估值方程，只影响和的计算公式。因此式(12-79)与式(12-58)相同，式(12-80)与式(12-59)相同。第一百二十二页第一百二十三页，共191页。具有输入控制信号的连续系统方块图和卡尔曼滤波方块图如图12-16和图12－17所示。图12－16图12－17第一百二十三页第一百二十四页，共191页。第八节有色噪声情况下的卡尔曼滤波前面推导卡尔曼滤波方程时，假定和都是白噪声。实际上，和可能是有色噪声。所谓白噪声，就是不同时刻的噪声都是互不相关的，而有色噪声是在不同时刻的噪声都是相关的。某些特定的有色噪声可用白噪声通过成形滤波器来表示。这样就可直接应用上面的滤波方程。下面将先介绍成形滤波器，然后再通过实例说明如何把某些特定的有色噪声用成形滤波器来处理的问题。第一百二十四页第一百二十五页，共191页。一连续系统的成形滤波器设是一平稳随机过程，其相关函数为式中，为常数，为时间间隔。将噪声的相关函数进行富氏变换，可得噪声的频谱密度为第一百二十五页第一百二十六页，共191页。可以将分解成由上式可直接写出相应的传递函数：第一百二十六页第一百二十七页，共191页。显然，随机过程相当于单位强度白噪声（谱密度的白噪声称为单位强度白噪声）长时间作用于传递函数为的线性系统的结果。该系统是一个惯性环节，其时间常数为，放大系数为，可用图12-18表示。第一百二十七页第一百二十八页，共191页。把单位强度白噪声转变为有色噪声的线性系统称为成形滤波器，上述的惯性环节即为成形滤波器。如果把上述成形滤波器的特性用微分方程表示，则有若令则成形滤波器方程为第一百二十八页第一百二十九页，共191页。式中为白噪声，且有因此，对于某些特定的有色噪声，可用单位强度白噪声通过成形滤波器来表示。通过以上分析可知，如果有色噪声的相关函数是指数函数，则在谱密度中的最高次项为，则成形滤波器用一阶微分方程表示。第一百二十九页第一百三十页，共191页。对于多维的有色噪声，如其相关函数为指数函数，则其成形滤波器方程可用一阶向量微分方程表示：式中，为多维有色噪声、为的系数矩阵，为均值等于零的白噪声过程，其统计特性为第一百三十页第一百三十一页，共191页。对于一般的平稳随机过程，如果谱密度为的有理函数，且能分解成式中和是的多项式。和的根都在上半复数平面内，即和的根在复平面的左半部。则随机过程相当于单位强度白噪声长时间作用于频率特性为的线性系统的结果，则成形滤波器的传递函数为第一百三十一页第一百三十二页，共191页。二离散系统的成形滤波器设为一平稳随机序列，已知其相关函数为这里不加推导地写出成形滤波器方程如下：式中，为成形滤波器的转移矩阵：第一百三十二页第一百三十三页，共191页。为均值不零的白噪声序列，其统计特性为第一百三十三页第一百三十四页，共191页。三系统中附加噪声的几种情况上面讨论了控制系统和观测系统含有白噪声的卡尔曼滤波。下面讨论系统的附加噪声为有色噪声的卡尔曼滤波。有色噪声的卡尔曼滤波有三种情况：⑴控制系统附加噪声为有色噪声，观测系统噪声为白噪声；⑵控制附加噪声为白噪声，观测系统噪声为有色噪声；⑶控制系统和观测系统的附加噪声均为有色噪声。这里只讨论情况⑴和⑵和卡尔曼滤波。关于情况⑶的卡尔曼滤波，读者可把情况⑴和⑵的推导方法组合起来，进行情况⑶的卡尔曼滤波方程的推导，故在此不作讨论。第一百三十四页第一百三十五页，共191页。四

控制系统含有有色噪声，观测系统含白噪声情况的卡尔曼滤波问题1.连续系统系统状态方程和观测方程如下：式中，是均值为零的有色噪声，是均值为零的白噪声。可用下列成形滤波器方程表示：(12-81)(12-82)(12-83)第一百三十五页第一百三十六页，共191页。设为扩大维数后的状态向量，即设为扩大状态变量维数后的系统附加噪声，即显然，中只包含白噪声。第一百三十六页第一百三十七页，共191页。扩大状态变量维数后的系统状态方程和观测系统方程为或式中(12-84)第一百三十七页第一百三十八页，共191页。或式中式(12-84)和式(12-85)分别为扩大状态变量维数后的控制系统方程和观测系统方程。由于只包含白噪声，为白噪声，故可直接应用前面推导出的卡尔曼滤波方程。(12-85)第一百三十八页第一百三十九页，共191页。2.离散系统离散系统状态方程和观测系统方程如下：式中，为有色白噪声序列，是均值为零的白噪声序列。可用下述成形滤波器方程来表示：式中为白噪声序列。(12-86)(12-87)(12-88)第一百三十九页第一百四十页，共191页。把作为状态变量的一部分，设为扩大维数后的状态变量，即设为扩大状态变量维数后的系统附加噪声，即显然，在中只包含白噪声。第一百四十页第一百四十一页，共191页。扩大状态变量维数后的系统状态方程和观测系统方程为或式中(12-89)第一百四十一页第一百四十二页，共191页。或式中式(12-89)和式(12-90)分别为扩大状态变量维数后的控制系统方程和观测系统方程。由于只包含白噪声，为白噪声，故可直接应用前面推导的离散系统卡尔曼滤波基本方程。(12-90)第一百四十二页第一百四十三页，共191页。例12-5在进行导弹容错控制系统设计时，需要利用导弹俯仰通道阻尼回路的状态信息进行故障诊断，用来判断回路中速率陀螺仪是否发生故障。设某地空导弹俯仰通道阻尼回路的数学模型为其中，状态变量为与弹体俯仰角速率成比例的量，为与弹体俯仰角速率成比例的量。控制变量为俯仰制导指令，输出变量为由速率陀螺测得的弹体俯仰角速率。试设计卡尔曼滤波器，估计出状态变量。（注：与不相关）(12-91)(12-92)第一百四十三页第一百四十四页，共191页。要求仿真计算1000步，初始条件：，是周期为400步、幅值为1的方波信号。导弹俯仰通道阻尼回路数学模型中的转移矩阵为其他系数矩阵为第一百四十四页第一百四十五页，共191页。为有色噪声序列，其自相关函数为是均值为零的白噪声序列，其统计特性为第一百四十五页第一百四十六页，共191页。解⑴成形滤波器设计由自相关函数的形式，直接写出成形滤波器如下：式中，，取采样周期秒，因此取。为白噪声，其统计特性为(12-93)第一百四十六页第一百四十七页，共191页。⑵构造扩大状态变量维数的状态方程和观测方程扩展状态变量扩展状态方程和观测方程为(12-94)(12-95)第一百四十七页第一百四十八页，共191页。式中这样，我们得到了系统噪声与观测噪声均为白噪声序列、且互不相关的扩大状态变量维数的扩展系统，针对此扩展系统可直接利用前面所推导出的卡尔曼滤波方程。第一百四十八页第一百四十九页，共191页。⑶卡尔曼滤波方程组(12-96)(12-97)(12-98)(12-99)(12-100)第一百四十九页第一百五十页，共191页。⑷计算步骤1.令，给定状态最优估值初值和估值误差方差阵初值P(0/0)。在本例计算中，选取。2.利用式(12-100)计算预测误差方差阵。3.利用式(12-98)计算最优增益矩阵。4.利用式(12-97)计算扩展状态的最优预测值。5.利用式(12-96)计算扩展状态的最优预测值6.利用式(12-99)计算扩展状态。7.令增1，返回第2步，重复上述计算步骤。第一百五十页第一百五十一页，共191页。⑸数学仿真仿真程序流程图如图12-19所示。计算结果如图12-20和图12-21所示。第一百五十一页第一百五十二页，共191页。五、控制系统含白噪声、观测系统含有色噪声情况的卡尔曼滤波问题1.连续系统系统状态方程为观测方程为式中，是均值为零的白噪声，其统计特性为(12-101)(12-102)第一百五十二页第一百五十三页，共191页。是均值为零的正态分布有色噪声，可用成形滤皮器方程表示如下：式中，是均值为零的白噪声，其统计特性为与互不相关，即。(12-103)第一百五十三页第一百五十四页，共191页。现设法改变观测方程(12-102)，使等效观测方程的附加噪声为白噪声。考察方程(12-103)可看出是白噪声，从这一点出发，引入一个等效观测值则也是白噪声。(12-106)(12-105)第一百五十四页第一百五十五页，共191页。依据式(12-102)和式(12-103)，可将式(12-105)变为式中(12-106)第一百五十五页第一百五十六页，共191页。这样，把观测方程(12-102)转变成等效观测方程(12-106)。式(12-106)的附加噪声为白噪声，因与都是均值为零的白噪声，则也是均值为零的白噪声。考虑到与的不相关性，的统计特性如下：式中第一百五十六页第一百五十七页，共191页。由于中包含，因此和是相关的，故式中由于等效观测方程(12-106)的附加噪声为白噪声，因此完全可用前面推导出的系统噪声和观测噪声相关的连续系统的卡尔曼滤波公式(12-57)、式(12-58)和式(12-59)。第一百五十七页第一百五十八页，共191页。根据式(12-57)最优滤波方程为把和代入上式，可得有色噪声情况的最优滤波方程为(12-107)第一百五十八页第一百五十九页，共191页。根据式(12-58)，最优增益矩阵为把和以及代入上式，可得有色噪声情况的最优增益矩阵为(12-108)第一百五十九页第一百六十页，共191页。根据式(12-59)，估计误差方差阵为把和及代入上式，可得有色噪声情况的估计误差方差阵方程(12-109)第一百六十页第一百六十一页，共191页。从式(12-108)可看出，为了使滤波的存在，要求矩阵为正定矩阵。2.离散系统系统状态方差为式中，是均值为零的白噪声序列，其统计特性为(12-110)第一百六十一页第一百六十二页，共191页。观测方程为式中是均值为零的正态分布有色噪声序列，可用成形滤波器方程表示如下：式中是均值为零的白噪声序列，其统计特性：(12-112)(12-111)第一百六十二页第一百六十三页，共191页。下面采用与处理连续系统的方法相同，设法改变观测方程，使等效观测方程的附加噪声为白噪声，然后直接应用前面已推导出的卡尔曼滤波方程。把式(12-112)代入式(12-111)，得再把式(12-111)改写为以乘上式两边，得(12-114)(12-113)第一百六十三页第一百六十四页，共191页。将式(12-113)减式(12-114)，并令，则把式(12-110)代入上式可得式(12-115)为等效观测方程，是等效观测值。(12-115)第一百六十四页第一百六十五页，共191页。等效观测方程与原始观测方程相比较，有两个特点：第一，等效观测值只包含白噪声，它与系统附加噪声是相关的；第二，在形式上看作时刻的观测值，看起来是线性函数，实际上却是的线性函数。因此，针对系统状态方程(12-110)和等效观测方程(12-115)的滤波方程的估值和估计误差方差阵，就是针对式(12-110)和式(12-111)的估值及。第一百六十五页第一百六十六页，共191页。由于等效观测方程的白噪声与系统附加噪声相关，因此可用滤波方程式(12-20)、式(12-25)和式(12-27)来处理。将式(12-115)与式(12-12)相比较可知：(12-117)(12-118)(12-119)第一百六十六页第一百六十七页，共191页。另外，考虑到等效观测方程(12-115)的估值及是原观测方程(12-111)的对应估值及。因为在中包含观测值的信息，这样可得有色噪声情况下的离散系统滤波方程如下：(12-119)(12-120)(12-121)第一百六十七页第一百六十八页，共191页。式(12-119)中式(12-120)和式(12-121)中的，分别由式(12-116)、式(12-117)和式(12-118)表示。第一百六十八页第一百六十九页，共191页。应当考虑初始值对估值的影响问题。把的初值对估值的影响归结为的初始估值对的影响。若第一次测量时间，验前估算为及，并且。根据线性最小方差估计，和的初值为最优滤波方块图如图12-22所示。第一百六十九页第一百七十页，共191页。以上用改变观测方程的方法，避免了扩大状态变量维数，这种方法在控制和导航等方面都可能应用。第一百七十页第一百七十一页，共191页。第九节滤波的稳定性概念和滤波的发散问题一、滤波的稳定性概念二、滤波的发散问题第一百七十一页第一百七十二页，共191页。一、滤波的稳定性概念前面比较详细地讨论了线性系统卡尔曼滤波基本方程的推导和递推计算步骤。在估值计算时，需要利用一连串的观测数据，按照滤波基本方程进步递推计算，可得状态向量的最优估值。这里，需要确切知道的初值和估计误差方差阵的初值。但在许多实际问题中，往往不可能确切知道初值和，甚至根本不知道这些初值。为了进步滤波计算，只能假定初值和。这对滤波结果会造成什么影响呢？第一百七十二页第一百七十三页，共191页。如果用正确的初值和，按滤波方程可得最优的滤波值及；如果选取不确切的初值和，可得非最优的和，在时间充分长之后，如果能收敛于，能收敛于，则初值不确切的影响可忽略不计。这种情况下，滤波是稳定的。反之，滤波是不稳定的。滤波是否稳定，与滤波系统的稳定性有关，而滤波系统的稳定性