卡尔曼滤波器附录

—110—附录：卡尔曼滤波器1基本概念1.1基本概念所谓滤波，是指从接收到受干扰的信号里提取所需要的信号，从数学上来说，就是通过处理一系列的带有误差的实际测量数据，从而得出所需要的各种量的估计。滤波问题，就是从夹杂着随机噪声的不足的测量信息中，求出被控对象内部状态向量X的最优估值Xˆ，使计算机按最优控制规律XUUˆ去进行最优控制。图1滤波原理框图1.2被估值系统首先讨论离散线性系统kkkkkkkWXX,1,11（1）1111kkkkVXHZ（2）其中，X：n维状态向量；：nn阶状态转移矩阵；W：p维动态系统噪声；：pn阶干扰转移矩阵；Z：m维测量向量：H：nm阶量测矩阵；V：m维测量噪声。系统噪声W与测量噪声V是相互独立的高斯白噪声，统计特性如下：0kWE，klkTlkQWWE，0kQ（3）01kVE，lkkTlkRVVE1111，01kR（4）0TlkVWE（5）最优控制程序被控对象信息测量最优估值程序XˆZXUˆXW（状态噪声）V（测量噪声）—111—状态向量的初始条件为：00XXE（6）00000PXXXXET（7）而且，0X与随机过程kW、1kV均不相关，即对任意k有00TkWXE，010TkVXE（8）图2离散系统动力学特性1.3Kalmanfilter估计准则所设计的状态滤波器应具有这种性质：能实时地接受可获得的量测信息1kZ，产生状态的估值1ˆkX，使估值误差111ˆ~kkkXXX在所选择的估值准则下为最小。即0~1kXE（9）min~21kXE（10）（9）与（10）两式给出了Kalmanfilter的最优估计准则：状态估值是无偏的最小方差估计。Kalmanfilter用递推方法实时处理数据。讲义上例子，重在说明递推。对常数，可认为是一部雷达测距仪，测量两点之间距离，由于测量仪表本身的精度有限，因此要求保证精度必须进行多次测量，有kiVXZii,,2,1设0iVE，jijiVVETji20利用第k次量测结果，有kiikZkX11ˆ在1k时刻，有kk,1延迟1kX1kZ+kW1kV1kHkk,1kX—112—kkkkkiikiikXZkXZkZkkkZkXˆ11ˆ111111ˆ111111由上述公式可以看出递推的含义。图3滤波器的实时数据处理1.4Kalmanfilter滤波器模型因为被估值系统是一个线性离散的时间系统，所求状态滤波器也是一个线性离散的时间系统。该系统应以量测信号1kZ作为输入，由1kZ产生的状态1kX的最优估值1ˆkX作为它的输出。设有0ˆ111111kYGXZKYFYkkkkkkkk(11)式中，Y：q维滤波器状态向量（实际上是n维）；Xˆ：n维滤波器输出向量，状态X的估值；F：qq阶滤波器转移矩阵，又叫反馈矩阵；K：mq阶滤波器前向增益矩阵；G：qn阶滤波器输出矩阵。kF、1kK与1kG，都是待定的系数矩阵，它们将根据估值准则来确定。图4滤波器系统结构图推导Kalmanfilter计算公式过程，就是寻找上述系统中的前项增益阵1kK、反馈阵kF、输出矩阵1kG和滤波器的维数q。iZiXˆFilter转移反馈1kK延迟1kY1ˆkX+1kGkFkY前向增益输出1kZ—113—（一）无偏滤波器由无偏性确定q、1kG与kF。000ˆXEXXE（kkEXXEˆ或者0~XE）因为0ˆX是非随机向量，00ˆXX，另外，滤波器的初始状态0Y应满足0000ˆXXYG为使0Y有唯一解，0G的秩应为n。这意味nq，为使其尽可能简化，取nq，且使,1,0kIGk。所以，（11）式中的第1个方程为111ˆˆkkkkkZKXFX（12）由1111,1,11kkkkkkkkkkkVXHZWXX及（12）式，可得到11,111,111,11111,1,1111~ˆˆ~kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkVKWHKIXFXHKFVXHKXFWXXXX（13）考虑到0kWE，01kVE，0~1kXE由（13）式，有0,111,1kkkkkkkkXEHKF（14）（14）式中，0kXE，因为kkkkkkkkkkXEWEXEXE,1,1,11由于00XXE，通过递推可知，kXE一般不为零。所以，使（14）式成立的条件是0,111,1kkkkkkkHKF（15）即kkkkkkkHKF,111,1（16）方程（16）的成立，体现了系统输出误差的无偏性，即0~XE。把（16）式代入（12）式，可得到滤波器方程为kkkkkkkkkkXHZKXXˆˆˆ,1111,11（17）方程的初始条件为：00ˆXX。由上述推导得到的滤波器称为无偏滤波器，原理如图5所示。—114—图5无偏滤波器原理方框图定义：当新的量测值1kZ未获得前，滤波器给出状态1kX的估值称为状态预测估值，记为kkX,1ˆ，有kkkkkXXˆˆ,1,1（18）同时给出量测1kZ的估值kkkkkkkkkXHXHZˆˆˆ,11,11,1kkZ,1ˆ称为量测预测估值。当获得新的测量值1kZ之后，滤波器用1kZ与kkZ,1ˆ之差，经过1kK进行线性“校正”，从而得到在1k时刻的系统状态的最优估值。这个估值误差的偏差是无偏的。由（17）式知，下一步应该求1kK。（二）无偏最小方差滤波器111,111,11kTkkkkTkkkkRHPHHPK（19）TkkkkkTkkkkkkkQPP,1,1,1,1,1（20）kkkkkPHKIP,1111（21）上述（17）至（21）式构成卡尔曼滤波器的滤波算法公式。1kZkkZ,1ˆ1kHkXˆ延迟1ˆkXkkX,1ˆ_kk,11kKkkZ,1~++—115—1.5Kalmanfilter算法程序框图图6Kalmanfilter算法流程图性质：（1）被估值系统第1k个时刻的状态1kX的卡尔曼滤波值1ˆkX，就是1kX的无偏估计，而且滤波误差的方差阵1kP是基于量测121,,,kZZZ的1kX的所有线性估计中最小的均方误差阵。即ZYXZYXEZXXZXXETTˆˆ（2）对一维情况，从（19）式可看出，当1kR时，1kK，说明当量测噪声增大时，增益矩阵应该取得小一些，以减弱量测噪声的影响。（3）由（20）式可知，0P或者kQ，或两者同时下降，则kkP,1，所以1kK。因为TXXXXEP00000ˆˆ，0P变小表示初始估计较好，TkkkWWEQ，kQ表示状态转移的随机波动小，所以新的量测值对状态的预测估值的校正影响应该下降，因此1kK应当变小。综上所述，可以得出结论：1kK与kQ成正比例，与1kR成反比。有人说：增益矩阵依赖置初始值0P、0ˆX，k0TkkkkkTkkkkkkkQPP,1,1,1,1,1111,111,11kTkkkkTkkkkRHPHHPKkkkkkkkkkkXHZKXXˆˆˆ,1111,11kkkkkPHKIP,1111kk1kQ1kR1kZ—116—于信kQ与噪kR之比。1.6举例【例1】设系统模型为11115.0kkkkkkvxzwxx式中，x、z、w、v均是标量。kw、1kw是互补相关的零均值白噪声，有1kTTkkQwwE，2111kTTkkRvvE系统初始条件为0ˆ00xEx，100VarxP。已知两次测量数据41z，22z求2ˆx与2P。解：由已知条件可知：5.0,1kk，1,1kk，11kH1kQ，10P，21kR利用公式kTkkkkkkkQPP,1,1,1111,111,11kTkkkkTkkkkRHPHHPKkkkkkPHKIP,1111计算如下：25.115.015.000,100,10,1QPPT1352125.11125.111110,1110,11RHPHHPKTT54.1132005.01413505.0ˆˆˆ00,111100,11XHZKXX77.0131025.1113510,1111PHKIP263115.07.05.011,211,21,2QPPT8331226311263111221,2221,22RHPHHPKTT—117—所以，得到28.113205.02833113205.0ˆˆˆ11,222211,22XHZKXX746.01,2222PHKIP由于量测系统的白噪声的方差较大，量测值的可靠性不高，因而估值1ˆkX及误差方差1kP在k时收敛速度慢，所以必须经过多次测量才能得到较好的估计。【例2】系统模型为111011011kkkkkkVXZWXX设1000kQ，kkR12，1000100P其它已知条件同前例。求101,,KK。解：利用公式TkkkkkTkkkkkkkQPP,1,1,1,1,1111,111,11kTkkkkTkkkkRHPHHPK此例中1,1kk，011kH根据已知条件，计算如下1110102010001101100010101100,100,10,1QPPT4762.09524.060.073.02111011110102001011110102011110,1110,11RHPHHPKTT60.073.02Kk1kK1.00.80.60.40.22kK012345678910kK图7增益变化曲线—118—kK阵中有两个元素，即21kkkKKK利用计算出的)10,,1(kKk值分别绘制1kK、2kK曲线，结果如图7所示。由图可见：当k为奇数时，kK较大，原因是奇数次观测的误差较小，有较高的稳定性，因而校正时，采用较大的权系数。同时要注意，P的计算要保证对称性。2Kalmanfilter的深入讨论2.1白噪声下一般离散线性系统滤波（1）有控制项的线性离散系统的滤波设被估值系统kkkkkkkkkkWUXX,1,1,11（22）11111kkkkkVMXHZ（23）式中，kU、1kM是已知的非随机序列，kU是r维系统外加控制项，kk,1是rn阶驱动矩阵，1kM是m维量测系统“系统误差”。、与H的意义同前，kW、1kV及初始状态0X的假设也同前