哈工大信号检测与处理第4章课程4-2新

1第四章滤波方法4.5卡尔曼滤波1960年由Kalman和Bucy提出（空间技术的发展）是线性最小均方误差滤波把对信号的先验知识用信号的模型形式表示时域状态变量法递推形式的线性最小均方误差算法。卡尔曼滤波建立在已知随机信号模型的基础上；它适用于时变非平稳随机序列。动态估计2第四章滤波方法EstimationwithApplicationstoTrackingandNavigationByYaakovBar-Shalom,X.RongLi,ThiagalingamKirubarajanSTOCHASTICPROCESSESANDFILTERINGTHEORYByAndrewH.Jazwinski3•Continuous-DiscreteSystem•DiscreteSystem•ContinuousSystem()()tttdxFtxdtGtd()kkktkyMtxv111(,)()kkkkkkxttxtw()kkkkyMtxv()()tttdxFtxdtGtd()tttdzMtxdtd4•Continuous-DiscreteSystem•Continuous-DiscreteFilter()()tttdxFtxdtGtd()kkktkyMtxvˆˆ()ttttdxFtxdt()()()()()ttttttdPdtFtPPFtGtQtGt1kktttˆˆˆ()()kkkkkktttkkktttxxKtyMtx()()kkkkkktttkktttPPKtMtP1()()()()kkkkttkkkkkttKtPMtMtPMtR{}()ttEddQtdt(0,),0kkkvNRR0,{},{}.ttkxvareindependent000ˆ(,)tttxNxP5•ContinuousSystem•ContinuousFilter()()tttdxFtxdtGtd()tttdzMtxdtd1ˆˆˆ()()()()tttttttttdxFtxdtPMtRtdzMtxdt1()()()()()()()()ttttttttttdPdtFtPPFtGtQtGtPMtRtMtP{}(),()0ttEddRtdtRt{}()ttEddQtdt{},{}.ttareindependent()()tttdxdtFtxGtw()tttyMtxv6784.5.1卡尔曼滤波的基本概念一个实际的系统可用如下形式表示：设向量非平稳序列1kx和1ky用下面的动态方程描述：)0(111,kvxCywxxkkkkkkkkk（4—33）kx是状态向量，ky是观测向量，kw是输入噪声，kv是观测噪声，1,kk是从1k时刻到k时刻的状态转移阵。上述动态方程可由系统的机理推导得来或由实验数据辨识得到。现假设已知。有如下假设：1）kw和kv为零均值白噪声即：[]0,[,][]0,[,]()TkkjkjkkjTkkjkjkkjEwCovwwEwwQEvCovvvEvvR其中kQ对称半正定kR对称正定，均为已知。92）kw和kv不相关即[,]0(,)TkjkjCovwvEwvkj3）初始状态0x是随机向量，且与kw、kv不相关，即000000[,][()]0[,][()]0TkkTkkCovxwExExwCovxvExExv卡尔曼滤波：——状态估计在已知动态方程（4—33）（状态和观测方程）和样本观测数据ky，1ky，…情况下，求随机序列样本——状态kx的估计值kxˆ。由方程可看出：1)若无观测噪声可直接由ky求得kx，2)若无输入噪声可直接由1kx推出kx。但实际是kw，kv均不为零，所以不能用（4—33）方程求。卡尔曼通过对称为一步预测观测误差——新息的修正加之最小均方误差调整准则很好地解决了带有噪声的状态估计问题。现代控制理论中的状态观测器是一种最简单的估计算法。它只适用于定常系统。卡尔曼也适用于时变系统，而且由于充分利用了噪声的统计特性，精度也高很多。卡尔曼的递推思想与新息：递推计算和新息是卡尔曼滤波的基本思想，请看如：)(11111211kkkkkyykykyyyy平均计算变成一种递推计算，大大减少了计算量，把1k估计看成是在k基础上的修正，分析修正项11()1kkyyk它的作用：104.5.2卡尔曼滤波的新息递推：卡尔曼滤波对现状态kX的估计ˆkX采用的是：过去状态对现状态的预测1ˆkkX，加上观测偏差1ˆ()kkkkyCX的适当加权修正kK即：)ˆ(ˆˆ11kkkkkkkkXCyKXX（4—34）1ˆkkX是基于1k次对k次的预测估计。11,1ˆˆkkkkkXX（4—35）所以)ˆ(ˆˆ11,11,kkkkkkkkkkXCyKXX（4—36）预测+修正=加权·新息其中kK——修正加权称为增益矩阵，它决定着递推方向，质量。如何确定kK是卡尔曼滤波器的核心。把kK建立成一种能够递推的结构形式是卡尔曼的独到之处！实时性能好！只要增益矩阵kK已知，就可以用（4—36）式递推地求得这一步的状态估计值。而kK的大小反映了对观测ky和新息,11ˆ()kkkkkyCX的重视程度，直接影响到状态估计的准确性，是状态估计的关键。确定kK的准则：最小均方误差准则。min)ˆ()ˆ(kkTkkxxxxEJ（4—37）即KKkdefkkTkktrPxxxxtrEJmin)ˆ()ˆ(（4—38）其中tr表示方阵迹，即对角元之和。kP是状态估计的均方误差矩阵。极小化J，即0kkKtrP，（4—39）求kK。11选择kK使J最小，得卡尔曼滤波器递推公式为：11,11,1kkkTkkkkkQPP（4—40）111kTkkkKTkkkkRCPCCPK（4—41）)ˆ(ˆˆ11,11,kkkkkkkkkkXCyKXX（4—42）1)(kkKKkPCKIP（4—43）1kkP，KK，和KP的计算与观测Ky无关，只要条件容许，可以事先计算好，而实时滤波只要递推kXˆ求状态估计即可。卡尔曼滤波：是时域内不断地“预测——修正——递推”过程。12110000011111()())2212NNkkkkkkkkNkkkkJxxPxxyCxRyCxwQw递推最小二乘方法01,11{,,;,,}min.NNNkkkkkxxwwJcondxxw134.5.3卡尔曼滤波举例举例：随机状态方程为：11110110kkkkkkxxwyxv（例5-1）其中1112212200[]0,[,]01[]0,[,][2(1)]kjkjkkjkjkjkjkkkjkj（例5-2）这里输入噪声kw是平稳白噪声，而观测噪声kv是非平稳白噪声。所以即使在稳态时，观测ky也是一个非平稳序列。为简单起见，初始的状态估计误差的协方差阵设为对角阵，即211112202112222()()()100010()()()xxxxxxPExxxxxx（例5-3）14求增益矩阵1210,,,KKK和具体的卡尔曼滤波递推公式。解状态方程中的各参数矩阵是定常的，即1）由式（例5-1）,111,1001kkkcc2）由式（例5-2）噪声参数为：00,2(1)01kkkQQR3）则由式（4—40）11,11,1kkkTkkkkkQPP预测误差协方差阵的递推公式为：111111000011101TkkkkPPQP（例5-4）4）则由式（4—41）111kTkkkKTkkkkRCPCCPK增益矩阵的递推公式为：111111110(10)2(1)0TTkkkkkkkkkkkKPCCPCRPP（例5-5）155）则由式（4—43）1)(kkKKkPCKIP估计误差协方差阵的递推公式为：11()[(10)]kKKkkkkPIKCPIKP（例5-6）6）令k=1将（例5-3）代入（例5-4）求得1020101011P；将10P代入（例5-5）求得120211021K将1K代入（例5-6）求得120102121101312121P7）同理令k=2，3，4，5用（例5-4）（例5-5）（例5-6）递推公式可算得前5步的结果如下表16k0123451kkP201010118.166.716.717.249.05554.213.62.22.22.75.62332kK0.950.450.730.60.90.50.550.330.850.45kP1000100.950.480.486.242.191.821.823.210.90.50.51.71.621120.850.450.450.6317前十步的增益矩阵见图，123456789100.20.40.60.81.0kk1kk2kk1kk2k卡尔曼滤波前十步的增益矩阵波动是由于观测噪声的方差是波动的由图可见当4kk时K即趋于稳定的周期序列，即这时有：(0.85,0.45),(0.55,0.33),TkTkKk为奇数为偶数18根据这一特殊情况，滤波计算变得很容易，由式（4—42）)ˆ(ˆˆ11,11,kkkkkkkkkkXCyKXX4k是状态估计的递推公式为：11111111()1111[(10)]0101110.85[(11)];0.4501110.55[(11)];0.3301kkkkkkkkkkkkkkkXXKyCXXKyXXyXkXyXk为奇数为偶数分析讨论：①上面已说过，增益矩阵反映了对新息的重视程度。由上式可知，当k为奇数时增益矩阵较大(0.85,0.45),(0.55,0.33),TkTkKk为奇数为偶数，这说明这时的新息较可信，这是由于当k为奇数时观测噪声较小的缘故（此时2(1)1kkR，偶数时2(1)3kkR）。19②由上表可以看到，估计误差的协方差KP和预测误差的协方差1kkP，都逐渐变小，这说明估计越来越准，但当k为奇数时协方差更小，反映了这时由于观测噪声小。估计就更准确。在Kalman滤波器的实现过程中，应注意几点：1、首先是滤波方程中的系统状态空间模型应该与真实系统的模型基本一致；2、保证待估计状态的可观测性要好；3、初始条件以及噪声协方差矩阵的选取要合理。对于最后一点，具体来说：1）预估状态初值]0|1[x不能偏离真值]0[x太远，否则滤波器或者不收敛，或者收敛速度太慢，所以滤波初值应根据经验和实际情况适当选取，不应该随便地全部选为零。202）预估状态的初始协方差矩阵)0|1(P是对预估状态初值偏差的一种反映，这一偏差由两部分构成，一个是真值]