最优估计之线性离散系统卡尔曼滤波

最优估计第6章线性离散系统卡尔曼滤波线性离散系统卡尔曼滤波器的推导带有控制项和测量系统偏差时的卡尔曼滤波器系统干扰和测量噪声相关时的卡尔曼滤波器有色噪声下的卡尔曼滤波器卡尔曼滤波器稳定性和鲁棒性线性离散系统的最优预测与平滑36.1引言•1960年卡尔曼（Kalman）将状态空间分析方法引入滤波理论，得到时域上的递推滤波算法，即卡尔曼滤波。(1)要求随机过程是平稳的，要求太强；(2)以单入单出系统推导出的，难以推广到高维的情况；(3)维纳-霍夫方程很难求解，在工程上难以实现；(4)是批处理方法，不能满足在线快速处理大量数据的需要。•维纳滤波的不足：•与维纳滤波相比，卡尔曼滤波不仅适用于非平稳随机过程，而且可以递推实现。系统和量测的数学模型噪声及初值统计特性状态和估计方差初值量测数据系统状态的最优估值KalmanFilter:40),(),(0][),(0][jkRRjkRvEQjkRwEwvkjkvvkkjkwwk，，，•噪声和初值的统计特性：0000)var(PxxEx，0),0(0),0(00TkxvTkxwvExkRwExkRkkkkkkkkkkkvxHzwxx11,11,•离散模型：已知：。态计算当前时刻的最优状，和上一时刻的估计值利用现有观测值kkkkkkxxzzzz|1|1121ˆˆ,,,,要求：零均值，方差阵为对角阵过程噪声和观测噪声不相关初值和过程噪声、观测噪声均不相关离散卡尔曼滤波器公式推导的线性函数。是1211|1,,,ˆkkkzzzx5•预测/时间更新(利用系统方程对状态及观测做一步预测)：•校正/测量更新(利用新观测对预测状态进行校正)：KF公式2：观测预测]ˆ[ˆˆˆˆ1|1||1|1|kkkkkkkkkkkkkzzKxxzzxKF公式3：状态估计待定校正增益阵如何求？kK最小。估计误差方差增益矩阵求取原则：使]~~[||kkTkkxxEkkkkkxxxˆ~|KF公式1：状态预测6.2直观推导法推导基本步骤：预测，校正。1|11,1|ˆˆkkkkkkxx0][1kwE1|1|ˆˆkkkkkxHz0][kvE}~][][~][~~]{[1|1|1|1|TkTkkkkkTkkTkkkkTkTkkkTkkTkkkkkkKvxHKIHKIxvKKvvKHKIxxHKIE])ˆ[ˆ(ˆ~1|1|||kkkkkkkkkkkkzzKxxxxx0~0~1|1|TkkkTkkkxEvvxE，TkkkTkkkkkkTkkkkKRKHKIPHKIxxE][][~~1|||估计误差：kkkkkkvKxHKI1|~][])ˆ[ˆ(1|1|kkkkkkkkkkxHvxHKxx11|ˆ~kkkkkxxx]~[~1|1|kkkkkkkvxHKx估计误差方差：TkTkTkkTkkkkkkkkTkkkkKvHKIxvKxHKIExxE][~~)(~~1|1|||TkkkTkkkkkkKRKHKIPHKI][]{[1|TkkkTkkkkkkkkKRKHKIPHKIP][][1||11|1|)(kTkkkkTkkkkRHPHHPK0kkKJ（2）将Jk对Kk的偏导数为零，即1|1|11|1|1||][)(kkkkkkkkTkkkkTkkkkkkkPHKIPHRHPHHPPPKF公式5：增益矩阵估计误差方差矩阵，与KF公式4等价KF公式4：估计误差方差矩阵kkkPJ|trace为了求得Kk，（1）选择代价函数：ABABAtrAT2)]([利用公式：以下求1|kkP1|1|ˆ~kkkkkxxx0]~[1,11|11,TkkTkkkkkwxE0]~[1,1|111,TkkTkkkkkxwETkkkkkTkkkkkkTkkkkkkkkkkkkkkkkQPwxwxEP1,11,1,1|11,11,1|11,11,1|11,1|]~][~[KF公式6：预测误差方差矩阵11,1|11,~kkkkkkkwx1|11,11,11,ˆkkkkkkkkkkxwx预测误差：TkkkkkkkkkkkkkkTkkkkwxwxExxE]~][~[~~11,1|11,11,1|11,1|1|预测误差方差：TkkTkkkkkTkkTkkkkkTkkTkkkkTkkTkkkkkkwxExwEwwExxE1,11|11,1,1|111,1,111,1,1|11|11,]~[]~[][]~~[9卡尔曼滤波的实质•递推估计的过程；•预测+校正的过程。时间更新zKK=k+1测量更新初始状态和方差10111,1ˆˆkkkkkkxx状态预测TkkkkkTkkkkkkkkQPP1,11,1,111,1方差预测111][kTkkkkTkkkkRHPHHPK增益矩阵11|~ˆˆkkkkkkkzKxx状态估值方差估值1][kkkkkkPHKIP预测/时间更新：校正/观测更新：]ˆ[~11kkkkkkxHzz新息序列离散卡尔曼滤波器推算方程11离散卡尔曼滤波公式公式汇总12卡尔曼滤波器结构图延时一步1,kkkz+-1|kkzkKkxˆ++当前估计值上一步估计值1|ˆkkxkH一步预测1ˆkx131.滤波递推实现，必须给定初值（状态和方差）；几点说明：2.当前状态最优估值：11|~ˆˆkkkkkkkzKxx。最优预测值，即则最优估值，即无新息，若1||1ˆˆ0~kkkkkkxxz4.KF是反馈校正过程，其校正项：，比例因子：1~kkkzKkK111][kTkkkkTkkkkRHPHHPK5.增益：。，反比于正比于RPKkkkk1TkkkkkTkkkkkkkkQPP1,11,1,111,16.预测方差：。正比于11kkkQP初始估值可由经验给定，方差需经测量，由统计方法给出；若滤波是稳定的，滤波将不依赖于初值。3.估计方差的作用：计算增益，但给出了误差分析。kkP14•最小二乘估计和卡尔曼滤波估计都是无偏估计，都能在得到系统最优状态估计值的同时，还得到估计误差的方差；均适用于状态为向量的情况。•维纳滤波和卡尔曼滤波都是最小方差估计。维纳滤波要求系统的输入是平稳过程，最小二乘和卡尔曼滤波无此要求。•最小二乘估计法可行的条件是只需要系统的测量方程和测量误差方差的信息，有递推算法，最优解是状态值；•维纳滤波需要知道系统的输入和噪声的功率谱密度，以及输入和噪声的互谱密度，属于批处理方法，最优解是滤波器的传递函数或是冲激响应，对状态为向量的情况不容易实现；•卡尔曼滤波可行的条件是不仅需要系统的测量方程，还需要系统的状态方程以及系统干扰和测量误差的均值和方差信息，递推方法，可以实时实现，最优解是系统的状态。卡尔曼滤波&最小二乘&维纳滤波卡尔曼滤波的直观解释卡尔曼滤波的基本思想与舰船组合导航人员作业中对船位推算的逻辑思维方法是吻合的。基本思想都可以归结为“预测+修正”。卡尔曼滤波过程是：每隔一个滤波周期，通过量测传感器得到量测船位C，同时经过状态转移得到预测船位B，在量测船位和预测船位之间根据增益Kk进行折衷，从而获得最佳估计船位D，依此过程不断循环下去。)()()1|(ˆ)()(kkKBDkkxkzkBC：对预测的修正：新息93332112ˆˆ001xx和滤波误差方差阵。、试求卡尔曼滤波估值，，，，并已知)1(ˆ)1(ˆ100015.0)1(100433)0()0()()()()(2)()1()()()(2)1(110212212211xxRQzPxxEkvkxkzkxkxkxkwkxkxkxkk代入滤波公式，得和、、、将，，解：由给定模型知kkRQPxxH02133)0(ˆ)0(ˆ01102112例6.2已知离散型线性系统的状态方程和观测方程如下8661800012112100421120001TTQPP869111086618101086618][11110|1110|11RHPHHPKTT21993105.0ˆ~0|1111xHzz9/510/3~869193]ˆ[ˆˆ10|11110|11zxHzKxx9/83/23/214866181086911001][0|1111PHKIP18应用实例1——舰船导航•状态向量：•状态方程：7654321sincos,KSVVVVKSVKSVWtXfXNNEEENTENKSVVX43214321vKvSvvzzzz•观测方程：1.系统模型2.初始条件3.仿真仿真结果(1)仿真结果(2)仿真结果(3)236.3带有控制项和测量偏差的卡尔曼滤波器kkkkkkkkkkkkkkkkkvyxHzwuxxyu11,11,11,时，模型为：和测量偏差考虑控制项两项。和的差别主要在于，因此滤波方程与之前状态方程和观测方程中由于这两项分别出现在1|1|~ˆkkkkzx1||1,11,1,1|11,1|11|1|1|1|11,1|11,1|1|1||][][ˆ~ˆˆ~ˆˆkkkkkkTkkkkkTkkkkkkkkkTkkkkTkkkkkkkkkkkkkkkkkkkkkkkkkkkPHKIPQPPRHPHHPKxHyzzuxxzKxx滤波方程：246.4系统噪声和测量噪声相关时的卡尔曼滤波kkkkkkkkkkkkkkvxHzwuxx11,11,11,系统模型：kjkwvSjkR),(相关噪声：其他条件与6.2中相同。相关！条件改变，不符合Kalman基本方程，前面的结果不能直接套用，需要对方程作形式上的修改，化为符合条件的形式，再利用之前结果。方法：引入类似于Lagrange乘子的待定常数矩阵，适当选取这个矩阵，变非约束方称为约束方程，将约束条件纳入方程，变成符合基本假设条件的形式。1111kkkkvxHz01111kkkkvxHzkkkkvxHz已知在动态方程右端增加一个恒为零的项：)(1111111,11,11,kkkkkkkkkkkkkkkvxHzJwuxx1111,1111,1111,)(kkkkkkkkkkkkkkkvJwzJuxHJ1111,kkkkkwux待定矩阵1