维纳滤波和卡尔曼滤.ppt

第九章 维纳滤波和卡尔曼滤 （Wiener and Kalman Filtering）,仿生机器人-MITs Cog,An upper-torso humanoid robot that has twenty-one degrees of freedom to approximate human movement, and a variety of sensory systems that approximate human senses, including visual, vestibular, auditory, and tactile senses,第九章 维纳滤波和卡尔曼滤 （Wiener and Kalman Filtering）,随机信号或随机过程(random process)是普遍存在的。 通常把对信号或系统功能起干扰作用的随机信号称之为噪声。 噪声按功率谱密度划分可以分为白噪声（white noise）和色噪声（color noise） 均值为0的白噪声叫纯随机信号（pure random signal）。,任何随机信号都可看成是纯随机信号与确定性信号并存的混合随机信号或简称为随机信号。 要区别干扰（interference）和噪声( noise)两种事实和两个概念。非目标信号（nonobjective signal）都可叫干扰。,干扰可以是确定信号，如国内的50Hz工频干扰。干扰也可以是噪声，纯随机信号（白噪声）加上一个直流成分（确定性信号），就成了最简单的混合随机信号。 医学数字信号处理的目的是要提取包含在随机信号中的确定成分，并探求它与生理、病理过程的关系，为医学决策提供一定的依据。,例如从自发脑电中提取诱发脑电信号，就是把自发脑电看成是干扰信号，从中提取出需要的信息成分。因此我们需要寻找一种最佳线性滤波器，当信号和干扰以及随机噪声同时输入该滤波器时，在输出端能将信号尽可能精确地表现出来。,匹配滤波,匹配滤波着眼点不是尽可能保持信号不是真，而是提高输出的信噪比,维纳滤波和卡尔曼滤波就是用来解决这样一类问题的方法：从噪声中提取出有用的信号。 这种线性滤波方法也被看成是一种估计问题或者线性预测问题。 维纳滤波-对真实信号的最小均方误差(MMSE, minimum mean-square error)估计问题.,设有一个线性系统，它的单位脉冲响应是h(n)，当输入一个观测到的随机信号x(n)，简称观测值，且该信号包含噪声w(n)和有用信号s(n)，简称信号，也即,(9-1),则输出,为,(9-2),我们希望输出得到的与有用信号尽量接近，因此称为的估计值，用 来表示y(n)，我们就有了维纳滤波器的系统框图，如图9-1。这个系统的单位脉冲响应也称为对于的一种估计器。,图9-1 维纳滤波器的输入输出关系,平滑,滤波,预测,这里我们主要考虑滤波问题，即,线性估计根据其取值范围不同通常有下面几种情况:,从图9-1的系统框图中估计到的信号和我们期望得到的有用信号可能不完全相同，这里用来表示真值和估计值之间的误差,(9-3),显然是随机变量，维纳滤波和卡尔曼滤波的误差准则就是最小均方误差准则：,(9-4),维纳滤波和卡尔曼滤波都是解决线性滤波和预测问题的方法，并且都是以均方误差最小为准则的，在平稳条件下两者的稳态结果是一致的。但是它们解决问题的方法有很大区别。,维纳滤波是根据全部过去观测值和当前观测值来估计信号的当前值，因此它的解形式是系统的传递函数或单位脉冲响应； 卡尔曼滤波是用当前一个估计值和最近一个观测值来估计信号的当前值，它的解形式是状态变量值。 维纳滤波只适用于平稳随机过程，卡尔曼滤波就没有这个限制。 设计维纳滤波器要求已知信号与噪声的相关函数，设计卡尔曼滤波器要求已知状态方程和量测方程，当然两者之间也有联系。,第一节 维纳滤波器的时域解 （Time domain solution of the Wiener filter）,设计维纳滤波器的过程就是寻求在最小均方误差下滤波器的单位脉冲响应或传递函数的表达式，其实质就是解维纳霍夫（WienerHopf）方程。我们从时域入手求最小均方误差下的 ， 用 表示最佳线性滤波器。这里只讨论因果可实现滤波器的设计。,一、因果维纳滤波器,设 是物理可实现的，也即是因果序列： 因此，从式(9-1)、 (9-2)、(9-3)、(9-4)推导：,(9-5),要使得均方误差最小，则将上式对各 ，m0，1，求偏导，并且令其等于零，得：,(9-6),(9-7),即,(9-8),用相关函数R来表达上式，则得到维纳霍夫方程的离散形式：,从维纳霍夫方程中解出的h就是最小均方误 差下的最佳h，,即,求到,，这时的均方误差为最小：,由式（9-9）进一步化简得：,(9-10),二、有限脉冲响应法求解维纳霍夫方程,如何去求解维纳霍夫方程，即式（9-9）中解的问题，设是一个因果序列且可以用有限长（N点长）的序列去逼近它，则式(9-5) (9-10)分别发生变化,(9-11),(9-12),(9-13),(9-14),(9-15),于是得到N个线性方程：,写成矩阵形式有：,(9-16),简化形式：,RxxH=Rxs,(9-17),式中，Hh(0) h(1) h(N-1)，是待求的单位脉冲响应；,Rxs,，是互相关序列；,Rxx,，是自相关矩阵,只要Rxx是非奇异的，就可以求到H：,H=Rxx1 Rxs,(9-18),求得,后，这时的均方误差为最小：,非奇异,满足行列式 |A|0的方阵A称为非奇异的,否则就称为奇异的,由式（9-15）进一步化简得：,(9-19),用有限长的,来实现维纳滤波时，当,已知观测值的自相关和观测值与信号的互 相关时就可以按照式（9-15）在时域里求解,但是当N比较大时，计算量很大，并,且涉及到求自相关矩阵的逆矩阵问题。,若信号 与 噪声互不相关，即，,则有,则式（9-15）和式（9-19）化为：,(9-20),(9-21),【例9-1】已知图9-1中,且,与,统计独立，其中,的自相关序列为,，,是方差为1的单位白噪声，,试设计一个N=2维纳滤波器来估计,，并求最小均方误差。,，代入式（9-20）得,解得：,0.451，,0.165。,将上述结果代入式（9-21），求得最小均方误差：,若要进一步减小误差可以适当增加维纳滤波的阶数，但相应的计算量也会增加。,解依题意，已知信号的自相关和噪声的自相关为：,三、预白化法求解维纳霍夫方程,从上面分析知求解维纳霍夫方程比较复杂，本节用波德（Bode）和香农（Shannon）提出的白化的,方法求解维纳霍夫方程，得到系统函数,H(z)。,随机信号都可以看成是由一白色噪声,w1(n),激励一个物,理可实现,的系统,或模型的响应，如图9-2所示，,其中A(z) 表示系统的传递函数。由于x(n) = s (n) + w(n)，在图9-2的基础上给出x(n)的信号模型，图9-3所示。把这两个模型合并最后得到维纳滤波器的信号模型，图9-4所示，其中传递函数用B(z)表示。,图9-2 的信号模型,图9-3,的信号模型,图9-4 维纳滤波器的输入信号模型,白噪声的自相关函数为 ，它的z变换就等于 。图9-2中输出信号的自相关函数为 ，,根据卷积性质有,令,上式,令,对式（9-22）进行Z变换得到系统函数和相关函数 的z变换之间的关系：,(9-23),同样，对图9-4进行z变换得,(9-24),图9-4中利用卷积性质还可以找到互相关函数 之间的关系：,两边z变换得到,(9-25),如果已知观测信号的自相关函数，求它的z变换，,然后找到该函数的成对零点、极点，取其中在单,位圆内的那一半零点,、极点构成,，另外在,单位圆外的零、极点构成,，这样就保证了,是因果的，并且是最小相位系统。,从图9-4可得,(9-26),由于系统函数,的零点和极点都在单位圆内，,即是一个物理可实现的最小相位系统，则,也是一个物理可实现的最小相移网络函数。我 们就可以利用式（9-26）对,进行白化，即把,当作输入，,当作输出，,是系统,传递函数。,将图9-1重新给出，待求的问题就是最小均方误差下的最佳 ，如图9-5(a)所示，为了便于求这个 ，将图9-5(a)的滤波器分解成两个级联的滤波器： 和G（z），,如图9-5(b)所示，则,(9-27),（a）,（b）,图9-5 利用白化方法求解模型,有了上述的模型后，白化法求解维纳霍夫方程步骤如下：,1）对观测信号 的自相关函数 求z变换得到 ； 2）利用等式 ，找到最小相位系统 ； 3）利用均方误差最小原则求解因果的G（z）； 4） ，即得到维纳霍夫方程的系统函数解。,在上述步骤中， 可以通过已知的观测信号的自相关函数来求得，因而求解 的问题就归结为求解G（z）的问题了。由于G（z）的激励源是白噪声，求解变得容易多了，下面我们分析步骤3的求解过程。,按图9-5(b)有:,(9-28),均方误差为：,由于 ，代入上式，并且进行配方得：,均方误差最小也就是上式的中间一项最小，所以,(9-30),注意，这里的,是因果的。对该式求z变换，得到,(9-31),表示对,求单边z变换。所以维纳霍夫,方程的系统函数解表示为：,由式（9-25）上式可以表示为：,因果的维纳滤波器的最小均方误差为：,(9-33),利用帕塞伐尔(Parseval)定理，上式可用z域来表示：,围线积分可以取单位圆。,(9-34),【例9-2】已知图9-1中,，且,统计独立，其中,的自相关序列为,，,是方差为1的单位白噪声，试,，并求最小均方误差。,与,设计一个物理可实现的维纳滤波器来估计,解依题意，已知,，,，,，,步骤1,求z变换,步骤2,由于,，容易找到最小相位系统和白噪声方差,步骤3,利用式（932）,对括号里面求反变换，注意括号内的收敛域为,取因果部分，也就是第一项，所以,步骤4,最小均方误差为,取单位圆为积分围线，有两个单位圆内的极点， 0.8和0.5，求它们的留数和，所以,第二节 维纳预测器（Wieners Predictor）,上节讨论的维纳滤波器是一种估计器，是用观测到的当前 和全部过去的数据 、 、来估计当前的 信号值。本节将讨论维纳预测器，它同样也是一种估计器，是用过去的观测值来估计当前的或将来的信号 ，N ，也是用真值和估计值的均方误差最小为估计准则。,一、因果的维纳预测器,图9-6就是维纳预测器的模型，N0， 是希望得到的输出，而 表示实际的估计值。,图9-6维纳预测器,本节和上节一样着重讨论预测器的系统函数以及预 测的均方误差，维纳预测器和维纳滤波器比较类似， 因而分析方法也都可以借鉴前面的内容。,对于图9-6模型，设,是物理可实现的，也即,，则有,是因果序列：,(9-35),(9-36),要使得均方误差最小，则将上式对各,，m0，1，求偏导，并且等于零，得：,(9-37),即,用相关函数R来表达上式：,(9-38),(9-39),由于,，则,，z变换得,(9-40),借鉴维纳滤波器的结果类似给出维纳预测器的最佳 传递函数，对应维纳预测器,，对应维纳滤波器,，故因果的预测器的传递函数为：,(9-41),最小均方误差为,(9-42),利用帕塞伐尔(Parseval)定理，上式可用z域来表示,【例9-3】已知图7-6中,，且,与,统计独立，其中,的自相关序列为,，,是方差为1的单位白噪声，,，并求最小均方误差。,试设计一个物理可实现的维纳预测器估计,解依题意已知,，,，,，,求z变换：,由于,，容易找到最小相位系统,和白噪声方差：,由式（9-41），N1，,求z变换：,由于,，容易找到最小相位系统和,白噪声方差：,由式（9-41），N1，,对括号里面求z反变换，注意括号内的收敛域为：,，,取因果部分，也就是第一项，所以,把上式写成差分方程形式有：,最小均方误差为：,二、纯预测器（N步）,纯预测器指的是 0的情况下，对 的预测。如图7-7所示。,图9-7 N步纯预测器,这时,，用白化法来求解预测器的系统函数。因为,，从而有：,(9-44),将上式代入式（9-41）、（9-43）得：,假设B(z）是b（n）的z变换，且b（n）是实序列， 则上式可以利用帕塞伐尔定(Parseval)理进一步化简：,(9-46),又因为B(z）是最小相位系统，一定是因果的， 上式可以简化,(9-47),上式说明最小均方误差随着N的增加而增加，也即 预测距离越远误差越大。,【例9-4】已知图7-7中,，其中,的自相关序列为,，试设计一个物理可,，并求最小均方误差。,，则,实现的维纳预测器来估计,解依题意，已知,因为,容易找到最小相位系统和白噪声方差:,利用式（9-45）:,因为,，只取,的部分，有:,回到z域有：,，代入,得:,最小均方误差为：,它说明当N越大，误差越大，当N0时，没有误差。 把上述结果用模型表示如图9-8所示。,图9-8 例题9-3的纯预测模型,三、一步线性预测器,对于纯预测问题，有 ，然而预测的问题常常是要求在过去的p个观测值的基础上来预测当前值，也就是,这就是一步线性预测公式，常常用下列符合表示,(9-48),式中p为阶数，,。预测的均方误差为:,(9-49),要使得均方误差最小，将上式右边对,求偏导并且等于零，得到p个等式,(9-50),最小均方误差：,(9-51),式（9-50）就是YuleWalker（Y-W）方程，把YuleWalker（Y-W）方程和维纳霍夫方程进行比较，维纳霍夫方程要估计的量是s(n)，Y-W方程要估计的量是x(n)本身，因而解维纳霍夫方程要已知x(n)、y(n)的互相关函数，实际中这个互相关函数往往是未知的，而解Y-W方程只需要知道观测信号的自相关函数。因此Y-W方程比W-H方程更具有实用价值。,例9-5】已知图9-7中x(n)=s(n)，其中,的自相关序列为,的可实现的一步线性预测器，并求最小均方误差。 解,，,，试设计一个p2,利用Y-W方程,，可以列出2个方程式,解得：,，也即,结果和例（9-4）N=1时一致。,第三节 维纳滤波器的应用 （Application of Wiener Filter）,要设计维纳滤波器必须知道观测信号和估计信号之间的相关函数，即先验知识。如果我们不知道它们之间的相关函数，就必须先对它们的统计特性做估计，然后才能设计出维纳滤波器，这样设计出的滤波器被称为“后验维纳滤波器”。 在生物医学信号处理中比较典型的应用就是关于诱发脑电信号的提取。大脑诱发电位（Evoked Potential，EP）指在外界刺激下，从头皮上记录到的特异电位，它反映了外,周感觉神经、感觉通路及中枢神经系统中相关结构在特定刺激情况下的状态反应。在神经学研究以及临床诊断、手术监护中有重要意义。EP信号十分微弱，一般都淹没在自发脑电（EEG）之中，从EEG背景中提取诱发电位一直是个难题：EP的幅度比自发脑电低一个数量级，无法从一次观察中直接得到；EP的频谱与自发脑电频谱完全重迭，使得频率滤波失效；在统计上EP是非平稳的、时变的脑诱发电位。通过多次刺激得到的脑电信号进行叠加来提取EP，这是现今最为广泛使用的EP提取方法。,为了解决诱发电位提取问题，研究者利用维纳滤波来提高信噪比，先后有Walter、Doyle、Weerd等对维纳滤波方法进行了改进。在频域应用后验维纳滤波的核心就是由各次观察信号中分解出信号的谱估计和噪声的谱估计，通过设计出的滤波器来提高信噪比。 本节将介绍时频平面的维纳滤波（timefrequency plane wiener filtering，简称TFPW）在高分辨心电图（HRECG）中的应用。方法如下：,一、观测样本,设共有N次观测样本：xi(t) = s(t)+ wi(t), i=1,2,N。其中s(t)是周期确定的心电信号；wi(t)是第i次记录时的噪声，包括肌电、测量仪器噪声等，假设每次记录的噪声之间互不相关；xi(t)是观测信号；信号和噪声相互独立。,对每次观测用短时傅立叶变换求时频表示（TFR）：,对N次观测的时频表示（TFR）求平均：,，样本平均的时频表示（TFR）为：,(1),样本平均为：,(2),从式（2）可以得到一个基于样本平均的简单时频 平面后验维纳滤波器：,(3),二、公式修正,在时频域上对式（1）（2）进行修正，给出更实际的表示：,(4),(5),式中COV表示信号和噪声之间的方差，也就是考虑 了信号和噪声并非相互独立；IF是干扰项；,表示样本平均的噪声功率；,表示样本噪声功率,的平均。,三、TFPW的计算过程,TFPW的计算过程如图9-9所示。,图9-10 TFPW的模拟实验结果 注：原信号是两个正弦波，观测信号混有白噪声；,原信号是线性调频信号，观测信号混有白噪声 在图9-10中每一个图中从上至下分别表示：测量的单个样本，样本平均，TFPW滤波器估计的信号，原始信号。图9-10的初始信噪比设为12dB，TFPW与叠加平均法相比，信噪比有5个dB左右的改善。,五、需要进一步研究的问题,FPW滤波中由于有二次TFR中的相关噪声以及IF项，滤波器可能包含虚部，也就是包含信号的相位信息，直接在时频平面上考虑相位问题还需要进一步研究。,Rudolf Emil Kalman,匈牙利数学家 BS&MS at MIT PhD at Columbia 1960年发表的论文A New Approach to Linear Filtering and Prediction Problems（线性滤波与预测问题的新方法）,1930年出生于匈牙利首都布达佩斯。 1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。 卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文A New Approach to Linear Filtering and Prediction Problems（线性滤波与预测问题的新方法）。,简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。 他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。 近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。,引入事列,假设我们要研究的对象是一个房间的温度。 根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。 假设你对你的经验不是100%的相信，可能会有上下偏差几度。 我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。,另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。 对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）,要估算k时刻的是实际温度值。 首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。 然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。,用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg2=52/(52+42)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。,已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：(1-Kg)*52)0.5=2.35。 这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。,就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值,人造器官,人造耳与真人耳的比较,人造眼与真人眼的比较,卡尔曼滤波的算法 The Kalman Filter Algorithm,5条公式是其核心内容 1、预估计 X(k|k-1)=A X(k-1|k-1)+B U(k) . (1) 2、计算预估计协方差矩阵 Z(k)=H X(k)+V(k)（2） 3、计算卡尔曼增益矩阵 Kg(k)= P(k|k-1) H / (H P(k|k-1) H + R) . (3) 4、更新估计 X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1) (4) 5、计算更新后估计协防差矩阵 P(k|k)=（I-Kg(k) H）P(k|k-1) (5),卡尔曼滤波的算法 The Kalman Filter Algorithm,首先引入一个离散控制过程的系统。该系统可用一个线性随机差分方程（Linear Stochastic Difference equation）来描述： X(k)=A X(k-1)+B U(k)+W(k) 再加上系统的测量值： Z(k)=H X(k)+V(k) 上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。,首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态： X(k|k-1)=A X(k-1|k-1)+B U(k) (1) 式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0,到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表示covariance： P(k|k-1)=A P(k-1|k-1) A+Q (2) 式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A表示A的转置矩阵，Q是系统过程的covariance。Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。,Kg为卡尔曼增益(Kalman Gain)： Kg(k)= P(k|k-1) H / (H P(k|k-1) H + R) (3) 有了现在状态的预测结果，然后再收集现在状态的测量值。结合预测值和测量值，可以得到现在状态(k)的最优化估算值X(k|k)： X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1) (4) 其中,到现在为止，已经得到了k状态下最优的估算值X(k|k)。但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束，还要更新k状态下X(k|k)的covariance： P(k|k)=（I-Kg(k) H）P(k|k-1) (5),简单例子 （A Simple Example）,把房间看成一个系统，然后对这个系统建模。当然，我们建的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的，所以A=1。没有控制量，所以U(k)=0。因此得出： X(k|k-1)=X(k-1|k-1) (6) 式子（2）可以改成： P(k|k-1)=P(k-1|k-1) +Q (7),因为测量的值是温度计的，跟温度直接对应，所以H=1。式子3，4，5可以改成以下： Kg(k)= P(k|k-1) / (P(k|k-1) + R) (8) X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)(9) P(k|k)=（1-Kg(k)）P(k|k-1) (10),现在我们模拟一组测量值作为输入。假设房间的真实温度为25度，我模拟了200个测量值，这些测量值的平均值为25度，但是加入了标准偏差为几度的高斯白噪声（在图中为蓝线）。,为了令卡尔曼滤波器开始工作，我们需要告诉卡尔曼两个零时刻的初始值，是X(0|0)和P(0|0)。他们的值不用太在意，随便给一个就可以了，因为随着卡尔曼的工作，X会逐渐的收敛。但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的，从而使算法不能收敛。我选了X(0|0)=1度，P(0|0)=10。,该系统的真实温度为25度，图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果（该结果在算法中设置了Q=1e-6，R=1e-1）。,第三节卡尔曼滤波的信号模型 （Signal Model of Kalman Filtering）,通过前面几节内容的学习，我们知道维纳滤波是根据当前 和过去全部的观测值 来估计信号的当前值 ，它的解形式是以均方误差最小为原则下的系统的传递函数 或单位脉冲响应 。 卡尔曼滤波不需要过去全部的观测, 它是根据前一个估计值,和最近一个观测值,来估计信号的当前,推方法进行估计的，因而卡尔曼滤波对信号的平稳性 和时不变性不做要求。我们利用维纳滤波的模型引入 到卡尔曼滤波的信号模型。,，它是用状态方程和递,一、状态方程和量测方程,要给出卡尔曼滤波的信号模型，先来讨论状态方程和量测方程。图9-11是维纳滤波的模型，信号 可以认为是由白噪声 激励一个线性系统 的响应，假设响应和激励的时域关系可以用下式表示： (9-52),上式也就是一阶AR模型。在卡尔曼滤波中信号,被称为是状态变量，用矢量的,形式表示为,，在k时刻的状态用,表示，,在k1时刻的状态用,表示。,图9-11 维纳滤波的信号模型和观测信号模型,激励信号,也用矢量表示为,，激励和响应,之间的关系用传递矩阵,来表,示，它是由系统的,有一定关系。有了这些假设后,结构确定的，与,我们给出状态方程：,(9-53),上式表示的含义就是在k时刻的状态,可以由它的前一个时刻的状态,来求得，即认为k1时刻以前的各状态都已记忆在 状态,中了,图9-11 维纳滤波的信号模型和观测信号模型,卡尔曼滤波是根据系统的量测数据（即观测数据 ）对系统的运动进行估计的，所以除了状态方程 之外，还需要量测方程。还是从维纳滤波的观测 信号模型入手，图9-11的右图，观测数据和信号 的关系为：,，,一般是均值为零的高斯白噪声，卡尔曼滤波中,，量测矢量,与状态矢量,的关系为,(9-54),上式和维纳滤波的,概念上是一致的，也就是说卡尔曼滤波的一维信 号模型和维纳滤波的信号模型是一致的。,把式(9-54)推广就得到更普遍的多维量测方程,(9-55),上式中的,称为量测矩阵，它的引入原因是，,的维数不一定与状态矢量,的维数相同，因为我们不一定能观测到所有需要的 状态参数,量测矢量,假如,是,的矢量，,是,的矢量，,就是,的矩阵，,是,的矢量。,二、信号模型,有了状态方程 和量测方程 后我们就能给出卡尔曼滤波的信号模型，如图9-12所示。,图9-12 卡尔曼滤波的信号模型,【例9-6】设卡尔曼滤波中量测方程为,，已知信号的自相关函数的z变换为,噪声的自相关函数为：,，信号和噪声统计独立。求卡尔曼滤波,和,。,信号模型中的,解根据等式：,可以求得：,变换到时域得：,因此,又因为,，所以,1。,第五节 卡尔曼滤波方法 （Method of Kalman Filtering）,建立好了卡尔曼滤波的信号模型以及状态方程、量测方程后，要解决的问题就是要寻找在最小均方误差下信号 的估计值 。,一、卡尔曼滤波的一步递推法模型,把状态方程和量测方程重新给出：,(9-56),(9-57),上式中,和,是已知的，,已知，现在的问题就是如何来求当前时刻,。,是观测到的,数据，也是已知的，假设信号的,上一个估计值,的估计值,上两式中如果没有,与,，可以立即求得,，估计问题的出现就是因为信号与噪声的,与,，用上两式,和,分别用,和,表示，得：,叠加。假设暂不考虑,得到的,(9-58),(9-59),必然，观测值,和估计值,之间有误差,，它们之间的差,称为新息（innovation）：,(9-60),显然，新息的产生是由于我们前面忽略了,与,所引起的，也就是说新息里面,包含了,与,的信息成分。因而我们用新息,乘以一个修正矩阵,，用它来代替式（9-56）的,来对,进行估计：,(9-61),由（9-56）（9-61）可以画出卡尔曼滤波对,进行估计的递推模型，如图9-13所示，输入为观测值,，输出为信号估计值,。,图9-13 卡尔曼滤波的一步递推法模型,二、卡尔曼滤波的递推公式,从图9-13容易看出，要估计出 就必须要先找到最小均方误差下的修正矩阵 ，结合式（9-61）、（9-56）、（9-57）得：,.(9-62),根据上式来求最小均方误差下的,，然后把求到的,代入（9-61）则可以得,到估计值,。,设真值和估计值之间的误差为：,，误差是个矢量，因而均方误差是一个矩阵，用,表示。把式（9-62）代入得：,.(9-63),均方误差矩阵：,(9-64),表示对向量取共轭转置。为了计算方便，令,(9-65),找到和均方误差矩阵的关系：,(9-66),把式（9-63）代入式（9-64），并且利用条件：,与,都是零均值的高斯白噪声，且它们,和,互不相关，协方差矩阵分别为,与,不相关；,与,及,不相关。最后化简得：,. (9-67),把式（9-66）代入（9-67）得,令,，,，代入上式化简：,(9-68),上式第一项和第二项与修正矩阵,无关，第三项是,，于是可以求得最小均方误差下的修正矩阵,为：,半正定矩阵，要使得均方误差最小，则必须,(9-69),把上式代入(9-61)即可得均方误差最小条件下的,递推公式。,相应的式（9-68）的第三项为零，得最小均方误差为：,(9-70),综上所述，得到卡尔曼滤波的一步递推公式：,(9-71),(9-72),(9-73),(9-74),有了上面四个递推公式后我们就可以得到,和,。如果初始状态,的统计特性已知，并且令,且矩阵,都是已知的，以及观测量,也是已知的，就能用递推,计算法得到所有的,和,：将初始条件,代入式（9-71）求得,；,将,代入式（9-72）求得,和,代入式（9-73）求得,；将初始条件,和,代入式（9-74）求得,；依此类推。这样递推用计算机实现,；将,非常方便。,和维纳滤波一样，卡尔曼滤波也可以推广到卡尔曼 预测，推导过程和维纳滤波到维纳预测类似，也同 样有纯卡尔曼预测，这里不再推导。,【例9-7】设卡尔曼滤波中量测方程为,，已知信号的自相关函数的z变换为,，噪声的自相关函数为，,，信号和噪声统计独立，已知,，在k0时刻开始观测信号。试用卡尔曼滤波的 公式求,和,，k0,1,2,3,4,5,6,7；以及,和,。,稳态时的,解由例9-6的结果知,，,1，,，,，把它们代入式(9-71) (9-74)得,（1）,（2）,（3）,（4）,由于是一维情况，求逆,，把（1）代入（2）、（3）式，消去,，再把（2）和（3）联立，得到,（5）,初始条件为,，k0开始观测，利用等式,（4），（5）进行递推得：,k0，,1.0000，,1.0000，,；,k1，,0.5000，,0.5000，,k2，,0.4048，,0.4048，,k3，,0.3824，,0.3824，,k4，,0.3768，,0.3768，,k5，,0.3755，,0.3755，,k6，,0.3751，,0.3751，,；,k7，,0.3750，,0.3750，,如果给定每个时刻的观察值就可以得到每一时刻的信号 估计值，上面是递推过程，还没有达到稳态的情况。 假设到了某一时刻k1，前后时刻的均方误差相等， 也就是误差不再随着递推增加而下降，达到最小的均 方误差了，即稳态情况，式（5）中的误差,，代入（5）式可以计算到稳态时的均方误差为,即稳态时的修正矩阵,，代入式（4）得稳态时的信号估计：,化到z域有：,。,将上述结果和维纳滤波的例题7-2的结果相比 较：,，,，,，发现当卡尔曼滤波达到稳态,时和维纳滤波的结果一致，原因就是它们两种 滤波都是用的同样的估计原则：最小均方误差 准则。然而在卡尔曼滤波的过渡期间的信号估 计结果和维纳滤波当然完全不同。,第六节 卡尔曼滤波器的应用 (Application Kalman Filter),最优估计指从带有随机干扰的观测数据中估计出信号来，其中的线性最小均方误差的卡尔曼滤波占有重要的地位，自动控制系统中应用非常广泛。前面我们已经推导出卡尔曼滤波的公式，也有了卡尔曼滤波器设计的直接调用程序。应用卡尔曼滤波时，核心是把问题如何纳入卡尔曼滤波的框架里面去，往往很难获得准确可靠的噪声数据，如前面的 和,加上干扰和噪声的协方差矩阵不一定为零，有,，一旦确定了这几个,，H，,kalman,表示均方误差矩阵，,表示另外一