卡尔曼滤波教学

1、卡尔曼滤波教学第六章卡尔曼滤波（The Kalman filtering） 卡尔曼滤波教学第一节卡尔曼滤波信号模型第二节 卡尔曼滤波方法第三节 卡尔曼滤波的应用卡尔曼滤波教学.信号模型信号模型.状态方程和量测方程状态方程和量测方程 维纳滤波的模型：信号可以认为是维纳滤波的模型：信号可以认为是由白噪声激励一个线性系统的由白噪声激励一个线性系统的响应，假设响应和激励的时域关系可以响应，假设响应和激励的时域关系可以用下式表示：用下式表示： (6-52)上式也就是一阶上式也就是一阶AR模型。模型。 )(ns)(1nw)(zA) 1() 1()(1nwnasns卡尔曼滤波教学 在卡尔曼滤波中信号被称为是

2、状态变量，用矢量的形式表示为，激励信号也用矢量表示为，激励和响应之间的关系用传递矩阵来表示， 得出状态方程： (6-53) 上式表示的含义就是在k时刻的状态可以由它的前一个时刻的状态来求得，即认为k1时刻以前的各状态都已记忆在状态中了 )(nsS(k)(1nw(k)w1A(k)1)(kw1)A(k)S(kS(k)1S(k)1)S(k 1)S(k卡尔曼滤波教学卡尔曼滤波是根据系统的量测数据（即观测数据）对系统的运动进行估计的，所以除了状态方程之外，还需要量测方程。 在卡尔曼滤波中，用表示量测到的信号矢量序列，表示量测时引入的误差矢量，则量测矢量与状态矢量之间的关系可以写成 (6-54)w(k)S

3、(k)X(k)卡尔曼滤波教学上式和维纳滤波的概念上是一致的，也就是说卡尔曼滤波的一维信号模型和维纳滤波的信号模型是一致的。把式(6-55)推广就得到更普遍的多维量测方程 (6-55)上式中的称为量测矩阵，它的引入原因是，量测矢量的维数不一定与状态矢量的维数相同，因为我们不一定能观测到所有需要的状态参数。 w(k)S(k)X(k)w(k)C(k)S(k)X(k)卡尔曼滤波教学.信号模型信号模型根据状态方程和量测方程，卡尔曼滤波的信号模型，如图6.12所示。图6.12 卡尔曼滤波的信号模型1)(kw1)A(k)S(kS(k)1w(k)C(k)S(k)X(k)S(k)C(k)1)A(k 1zw(k)

4、(k)w1X(k)1)S(k 卡尔曼滤波教学 【例6-1】设卡尔曼滤波中量测方程为，已知信号的自相关函数的z变换为噪声的自相关函数为，信号和噪声统计独立。求卡尔曼滤波信号模型中的和。w(k)S(k)X(k)25. 18 . 0 ,)8 . 01)(8 . 01 (36. 0)(1zzzzRss)()(mmRwwA(k)C(k)卡尔曼滤波教学 解：根据等式可以求得变换到时域得：因此又因为，所以1。)()()(121zAzAzRwss)8 . 01)(8 . 01 (36. 011zzzz)()(8 . 01)(111zWzSzzzA)()(8 . 0) 1(1kwksks8 . 0A(k)w(k

5、)S(k)X(k)C(k)卡尔曼滤波教学.卡尔曼滤波方法卡尔曼滤波方法（The method of Kalman filtering）.卡尔曼滤波的一步递推法模型卡尔曼滤波的一步递推法模型把状态方程和量测方程重新给出：把状态方程和量测方程重新给出： (6-56) (6-57)假设信号的上一个估计值已知，现在的问题就是如何来求当前时刻的估计值。 1)(kw1)A(k)S(kS(k)1w(k)C(k)S(k)X(k)1)(kS(k)S卡尔曼滤波教学 用上两式得到的和分别用和表示，得： (6-58) (6-59) 必然，观测值和估计值之间有误差，它们之间的差称为新息（innovation）： (6-

6、60)显然，新息的产生是由于我们前面忽略了与所引起的 1)(kSA(k)(k)S1)(kSC(k)A(k)(k)SC(k)(k)XX(k)(k)X(k)X(k)XX(k)(k)X(k)w1w(k)卡尔曼滤波教学 用新息乘以一个修正矩阵，用它来代替式（656）的来对进行估计： (6-61)由（656）（661）可以画出卡尔曼滤波对进行估计的递推模型，如图6.13所示 (k)XH(k)(k)w1S(k)(k)XH(k)1)(kSA(k)(k)S1)(kSC(k)A(k)H(K)X(k)1)(kSA(k)S(k)卡尔曼滤波教学 输入为观测值，输出为信号估计值。图6.13 卡尔曼滤波的一步递推法模型X

7、(k)(k)S(k)SC(k)A(k)1zX(k)1)(kSH(k)(k)X(k)X卡尔曼滤波教学.卡尔曼滤波的递推公式卡尔曼滤波的递推公式从图6.13容易看出，要估计出就必须要先找到最小均方误差下的修正矩阵，结合式（661）、（656）、（657）得：(6-62)根据上式来求最小均方误差下的，然后把求到的代入（661）则可以得到估计值。(k)SH(k)1)(kSC(k)A(k)w(k)(k)H(K)C(k)S1)(kSA(k)(k)S1)(kSC(k)A(k)w(k)1)(kw1)(kSA(k)H(K)C(k)1)(kSA(k)1H(k)w(k)1)(kw1)(kS(k)H(K)C(k)AH

8、(k)C(k)1)I(kSA(k)1H(k)H(k)(k)S卡尔曼滤波教学 设真值和估计值之间的误差为：误差是个矢量，因而均方误差是一个矩阵，用表示。把式（662）代入得 (6-63)均方误差矩阵： (6-64)表示对向量取共轭转置。 (k)SS(k)(k)S(k)(k)SS(k)(k)SH(k)w(k)1)(kw1)(kS1)A(k)S(kH(K)C(k)I1(k)S(k)SE(k)卡尔曼滤波教学为了计算方便，令 (6-65)找到和均方误差矩阵的关系： (6-66)把式（663）代入式（664），最后化简得： (k)S(k)(S(k)SE(S(k)(k)1)(kSA(k)1)(kw1)k1)

9、(A(k)S(kSA(k)1)(kw1)E(A(k)S(k(k)111)(k1)w(kEwA(k)1)(kS1)1)(S(k(kS1)A(k)ES(k111)Q(k1)A(k)A(k)(k(k)S(k)SE(k)k)H(k)R(k)H(H(k)C(k)1)IQ(k1)A(k)A(k)(kH(K)C(k)I卡尔曼滤波教学 把式（666）代入（667）得令，代入上式化简： (6-68)要使得均方误差最小，则必须 (k)k)H(k)R(k)H(H(k)C(k)(k)IH(K)C(k)IR(k)H(k)(k)C(k)H(k)C(k)H(k)(k)C(k)(k)H(K)C(k)(k)SSR(k)(k)C

10、(k)C(k)(k)C(k)U(k)H(k)H(k)SSUH(k)H(K)U(k)111)U(SH(k)S)U(SH(k)SU)U(SS(k)01)U(SH(k)S卡尔曼滤波教学求得最小均方误差下的修正矩阵为： (6-69)把上式代入(6-61)即可得均方误差最小条件下的递推公式。最小均方误差为： (6-70)1R(k)(k)C(k)C(k)(k)C(k)H(k)(k)S(k)1U)U(SS(k)(k)H(k)C(k)I卡尔曼滤波教学 综上所述，得到卡尔曼滤波的一步递推公式： (6-71) (6-72) (6-73) (6-74)(k)1)Q(k1)A(k)A(k)(k1R(k)(k)C(k)

11、C(k)(k)C(k)H(k)(k)(k)H(k)C(k)I(k)S1)(kSC(k)A(k)H(K)X(k)1)(kSA(k)卡尔曼滤波教学【例6-2】设卡尔曼滤波中量测方程为 已知信号的自相关函数的z变换为，噪声的自相关函数为，信号和噪声统计独立，已知在k0时刻开始观测信号。试用卡尔曼滤波的公式求和，k0,1,2,3,4,5,6,7；以及稳态时的和。解：由例6-6的结果知， w(k)S(k)X(k)25. 18 . 0 ,)8 . 01)(8 . 01 (36. 0)(1zzzzRss)()(mmRww1)0(, 0) 1(S(k)S(k)(k)S(k)8 . 0A(k)1C(k)36.

12、021wQ(k)1)(var(kwR(k)卡尔曼滤波教学 把上式代入式(6-71) (6-74)得 （1） （2） （3） （4）求逆把（1）代入（2）、（3）式，消去，再把（2）和（3）联立，得到 （5） (k)36. 064. 01)(k1(k)(k)H(k)1(k)(k)H(k) 1(k)S1)(kSH(K)X(k)1)(kS8 . 08 . 01(k)(k)H(k)1(k) (k)1/(k)H(k)1.361)(k0.361)(k(k)64. 064. 0卡尔曼滤波教学初始条件为，k0开始观测，利用等式（4），（5）进行递推得： k0，1.0000，1.0000， k1，0.5000，

13、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， 上面是递推过程，还没有达到稳态的情况。 1)0(, 0) 1(S)0()0(H)0(0X)(S) 1 (）（ 1H) 1 (5 . 0) 0 (4 . 01XS)( S)2()2(H) 2(4048. 0) 1 (4762. 02XS)(S)3()3(H) 3 (3824. 0) 2(4941. 03XS)(S)4()4(H) 4 (3768. 0) 3 (4985

14、. 04XS)( S)5()5(H) 5 (3755. 0) 4 (4996. 05XS)( S)6()6(H) 6(3751. 0) 5(4999. 06XS)(S)7()7(H) 7 (3750. 0) 6 (5000. 07XS)( S卡尔曼滤波教学 假设到了某一时刻k1，前后时刻的均方误差相等，也就是误差不再随着递推增加而下降，达到最小的均方误差了，即稳态情况，式（5）中的误差代入（5）式可以计算到稳态时的均方误差为：即稳态时的修正矩阵，代入式4得稳态时的信号估计：化到z域有：。) 1()(kk375. 0) 1()(kk375. 0)(kHX(k)1)(kS375. 05 . 0(k

15、)S)(zH15 . 01375. 0z卡尔曼滤波教学.卡尔曼滤波器的应用卡尔曼滤波器的应用(Application Kalman filter) 【例6-3】已知条件和例62一样，状态方程和测量方程为： 其中 ， 信号和噪声统计独立。求卡尔曼滤波器的稳态 和 。1)(kw1)A(k)S(kS(k)1w(k)C(k)S(k)X(k)8 . 0A1C36. 021wQ(k)1)(var(kwR(k)H(k)(k)卡尔曼滤波教学 解：根据函数调用sysss（A，B，C，D，1），得到离散卡尔曼状态模型，采样周期这里设为1。A，C已知，由于函数调用中是设计了两个观测信号的，我们这里只有一个观测信号，

16、所以B取0 1，后一个1表示噪声 的系数。D取0。实际的语句如下： sys=ss(A,B,C,D,1) 然后调用函数S，L，H，kalman（sys，Q，R），设计离散卡尔曼滤波器。实际语句和计算结果如下： s，l，h，=kalman(sys,0.36,1) l =0.3000 =0.6000 h =0.3750 =0.3750 这里省略了输出的S，它表示的信息是达到稳态后系统状态模型，H和 表示系统稳态的最终值 ) 1(1kw卡尔曼滤波教学 有了修正矩阵和均方误差，代入式（674）就可以根据观测信号得到卡尔曼滤波的估计值了。 从上面例题知道，只要确定了状态模型，就可以调用函数很快设计出卡尔曼

17、滤波器，下面来看看卡尔曼滤波器在生物医学信号中的应用。 在生物医学信号处理中脑电图的肌电伪迹和其它噪声的消除，以及诱发电位的提取都有研究者尝试用卡尔曼滤波器来处理。本节介绍卡尔曼滤波器在诱发电位提取中的应用，方法如下：卡尔曼滤波教学1.自发电位模型（EEG）和诱发电位（EP）模型的建立。如图6.14所示，EEG信号通过用AR模型建立，激励是白噪声，EP信号的激励是单位脉冲序列，用等式表示如下： 阶AR模型d表示从该时刻开始有单位脉冲刺激。 pnwineaneEEGpii),()()(:1qiimiiidndinscnsEP01)()()(:卡尔曼滤波教学 图6.14 EEG和EP模型卡尔曼滤波教学 从图6.14知道，观测信号是EEG和EP的线性相加，用 表示第i次刺激后测量的信号，对M次测量平均得： 叠加平均后的信号长度为N。利用先验知识建立好图6.14的模型。假设单次诱发信号和平均诱发信号的关系是延时和幅度变化但波形一致的情况，即)(nyi1, 1, )(1)(1NdddnnyMnyMii)()(jjjnnsAns卡尔曼滤波教学2.卡尔曼状态方程和量测方程的建立。 其中X X表示状态变量，包括诱发信号、单位脉 冲信号、自发信号，长mpq1 A 是系统矩阵，