卡尔曼滤波续

1、 .5 卡尔曼滤波的信号模型卡尔曼滤波的信号模型状态方程与量测方程状态方程与量测方程n 4.5.1 离散状态方程离散状态方程( )( ) (1)( ) (1)(1)x kA k x kB k u kk11kkkkxAx简写简写:状态变量，可多个kx k:过程噪声，可时变n 4.5.2 量测方程量测方程kkkkyC x:为测量噪声，可时变为测量噪声，可时变 kky:为测量数据或观测数据，可为测量数据或观测数据，可多个，但同状态量的个数无多个，但同状态量的个数无关。关。一般可以一般可以假定假定这两个噪声是这两个噪声是相互独立的、均值为零的正态白色噪声：相互独立的、均值为零的正态白色噪声：( ) (

2、0,)( ) (0,)kkp wNQp vNR 4.6 卡尔曼过滤的方法与公式卡尔曼过滤的方法与公式11kkkkxAxkkkkxCy1kkkxAx1kkkkkkxACxCy当不考虑噪音时当不考虑噪音时:先验估计先验估计已知已知:kkkexxkkkexx先验估计误差先验估计误差后验估计误差后验估计误差TkkkEPe eTkkkE Pe e先验估计误差方差先验估计误差方差后验估计误差方差后验估计误差方差ykkkkeyyy 新息新息基于均方误差最小准则推出基于均方误差最小准则推出卡尔曼递推公式卡尔曼递推公式1111(4.5.20 )()(4.5.20 )()(4.5.20 )(4.5.20 )(4.

3、5.20 )kkkkkkkkkTTkkkk kkkTkk kkkkkkkabcde xA xxxH yCxHPC CPCRPA P AQPI HC P 000 xEx000000varTPExxxxx初值的确定初值的确定 例4 设设 x为实离散时间随机过程，具有功为实离散时间随机过程，具有功率谱密度：率谱密度：)8 . 01)(8 . 01 (36. 0)(1zzzxx1)(z0)(zx并已知并已知 1var , 0001xPx在在k=0时开始观察信号时开始观察信号)(kkkkxyy，试用卡尔曼滤波计算公式求，试用卡尔曼滤波计算公式求kx ，并将计算结果与维纳过滤方法计算，并将计算结果与维纳过

4、滤方法计算(例例1)的结果进行的结果进行比较比较(此例此例)(zxx与例与例1中的相同中的相同)。解解 从给定的从给定的)(zxx，可以求得，可以求得kx的的状态方程状态方程。因为。因为1( )( )1 0.8W zX zz所以所以 10.8kkkxx8 . 0kA以及kkkxy又由于又由于 )8 . 01)(8 . 01 (36. 0)(1zzzxx所以所以 1(0)var ,36. 0 , 1vlkkkkRQC代入上式，得代入上式，得(1)(2)(3)(4)kkkkkkkkkkkkkPHPPPPPHxyHxx)1 (36. 064. 0) 1()8 . 0(8 . 01111将式将式(2)

5、代入式代入式(4)，得，得121) 1() 1(1)1 (kkkkkkkkkPPPPPPPHPkkkkkkkHPPPPPP12) 1(1) 1(36. 164. 036. 064. 011kkkPPP036. 072. 064. 02PP 从式从式(3)与式与式 (5)中消去中消去kP求稳态解，将上式中的求稳态解，将上式中的PPPkk用及1代入并化简，得代入并化简，得(5)所以所以8364. 0236. 064. 04)72. 0(72. 02P(只取正值解只取正值解)6 . 036. 064. 01kkPP)(836 . 16 . 01kkkkPPPH所以所以所以所以kkkkkkyxxyxx

6、835 . 0)8 . 0(838 . 0111由此可见，已知前一个估计值由此可见，已知前一个估计值 1kx与当前的量测值与当前的量测值 ky，就可以求得当前的估计值，就可以求得当前的估计值kx 。 15 . 018/3)()()(zzYzXzH1var , 0001xPxkkkkyyyxyyyyyyyxxyyyyyxxyx11021202102121010101002283 22838385 . 1875. 0835 . 02838385 . 1835 . 08315 . 018/3)(zzH)()5 . 0(83)(nunhnmiiiyhmnymhnx)()()( 0083)0()0( y

7、yhx0101185 . 183)0() 1 ( yyyhyhx012002875. 085 . 183)2() 1 ()0()2( yyyyhyhyhx)2/4/(83210yyy z=0.01*randn(1,150)-0.37727;xx(1)=-0.31;Q=0.000001;R=1;p(1)=0.02; s(1)= -0.37727;for k=2:1:150 s(k)= -0.37727;xs(k)=xx(k-1); 预测ps(k)=p(k-1)+Q;K(k)=ps(k)/(ps(k)+R);xx(k)=xs(k)+K(k)*(z(k)-xs(k);p(k)=(1-K(k)*ps(

8、k);endplot(xx);hold on plot(z,r)plot(s,k)课堂仿真实验：读程序分析问题课堂仿真实验：读程序分析问题分析的结论：利用卡尔曼滤波器进行滤波n1：观测信号（测量信号）为z，是一个直流信号（-0.37727）含有加性白噪声，实际方差R=0.01；n2：因此状态方程和测量方程很容易写出；A=1；C=1n3：给出两个初始值-0.31和0.00001（可通过改变初始值来看看结果，得出什么结论）n4：仿真给出的R=1（同实际不符合），看结果会如何。通过改变不同的R值，通过仿真结果得出相应的结论050100150-0.42-0.4-0.38-0.36-0.34-0.32-

9、0.3R=0.01050100150-0.42-0.4-0.38-0.36-0.34-0.32-0.3R=0.00001050100150-0.4-0.39-0.38-0.37-0.36-0.35-0.34-0.33-0.32-0.31-0.3R=1050010001500-0.42-0.4-0.38-0.36-0.34-0.32-0.3R=1对上述实验现象的解释和理解n理论上，从公式（理论上，从公式（c）可知，）可知，R（测量误差）（测量误差）越大，越大，H（卡尔曼系数）越小，修正量则越（卡尔曼系数）越小，修正量则越小，则公式（小，则公式（b）得到的状态量的估计值越）得到的状态量的估计值越接

10、近于先验估计结果。即观测数据（测量数接近于先验估计结果。即观测数据（测量数据）不准确，可信度小；据）不准确，可信度小；反之，反之，R越小，说明测量误差越小，则测试结越小，说明测量误差越小，则测试结果越可信，因此状态量的估计值越接近于测果越可信，因此状态量的估计值越接近于测量值。量值。因此，实际工程应用中，因此，实际工程应用中，R的准确获得非常关的准确获得非常关键。键。课堂例题讲解（多维情况）n在讲解例题前，再回顾一下卡尔曼滤波算法的递推公式、两个方程及推导的过程1111(4.5.20 )()(4.5.20 )()(4.5.20 )(4.5.20 )(4.5.20 )kkkkkkkkkTTkkk

11、kkkkTkkkkkkkkkabcde xA xxxHyC xHP CC P CRPA PAQPIH CPn当状态变量的数量为N时，则X为N*1矩阵；则A为N*N矩阵，反映的是状态量之间相邻两个时刻之间的关系；则Q矩阵的大小为多少？物理意义是什么？n测量（观测）量的个数为M，M可以不等于N则C为M*N矩阵，反映的是测量值同状态量之间的关系；则R矩阵 的大小为多少？物理意义是什么？ 。n同样P矩阵是多大矩阵？ 初始值矩阵是什么？nH（卡尔曼系数）矩阵大小为多少？上述非常重要，因为在编写程序的时候要赋初值的！下节课提问参考：111( ), ( )( )0, HnnkEnkQvv若其他222( ),

12、 ( )( )0, HnnkEnkQvv若其他1 ( )nQ过程噪声相关矩阵2 ( )nQ观测噪声相关矩阵Q：R： 例题：我们知道要确定空间中的一点位置需要知道径向距离和方位角。雷达是根据接收到的脉冲与发射脉冲的时间间隔来确定目标的距离和速度。假设雷达跟踪目标为一飞行器，雷达信号发射时间间隔为T（T也表示空间扫描的时间间隔），在时间k径向距离为p（k），在时间k+1径向距离为p（k+1），u1（k）、u2（k）分别表示在区间T目标径向速度及方位角速度的变化，且其是随机、独立、均值为零,方差分别为 和 的白噪声信号。如在k时刻对目标观测的径向距离和方位分别为z1(k)、z2(k)，观测噪声v1(

13、k)、v2(k)为不相关、均值为零方差分别为 和 的白噪声信号。利用卡尔曼滤波利用卡尔曼滤波实现对目标位置的准确估计。要求写出：状态方程、量实现对目标位置的准确估计。要求写出：状态方程、量测方程、状态转移矩阵、系统噪声向量及其相关矩阵、测方程、状态转移矩阵、系统噪声向量及其相关矩阵、量测噪声向量及其相关矩阵等，并简述获得目标位置估量测噪声向量及其相关矩阵等，并简述获得目标位置估计的过程。计的过程。21222324 解：状态方程： 其中：分别表示径向距离、速度、方位角、方位角速度。则( )(1)( )KkX kA X kD w k123( )( ),( ),( ), (4)Tkx kx kx k

14、xx10001000010001KTAT0101KD ( ) 0102w kuu 解：测量方程： 其中：kKkkyC Xv1 ( )2( )Tkyz kz k10000010KC1( )2( )kv kvv k222100000000000000kQ242300kRn例题：假设某直升机在离散时间(n-1)的滚动姿态角为 、滚动姿态角速度为 ，经过T秒后，在n时刻的滚动姿态角为 、滚动姿态角速度为 ；又知道滚动姿态的角加速度为:且满足：式中， 为n时刻飞机喷气所产生的加速度，且为已知； 是外力干扰产生的加速度，是均值为零、方差为 的平稳白噪声过程。现可测滚动姿态的角度值，设测试结果为 ，并已知测试噪声 的均值为零、方差为 。根据上述已知内容，要求