《卡尔曼滤波》ppt课件

1、卡尔曼滤波算法及运用.目录一. 概述二. 规范卡尔曼滤波卡尔曼滤波方程闭环卡尔曼滤波卡尔曼滤波特性及实现中的问题三. 扩展卡尔曼滤波非线性系统线性化卡尔曼滤波扩展卡尔曼滤波四. Schmidt 卡尔曼滤波五. 自顺应卡尔曼滤波六. 平滑算法2.一、概述一、概述2021-12-143.1.1 Rudolf Emil Kalman4uBorn 1930 in Hungary uBS and MS from MIT uPhD 1957 from Columbia uFilter developed in 1960-61目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 K

2、F平滑算法平滑算法Kalman R E. A new approach to linear filtering and prediction problems J. Journal of Fluids Engineering, 1960, 82(1): 35-45. 援用：18083.5uKalman滤波是一种最优估计算法，而非滤波器u可以实时估计系统中的参数如延续变化的位置、速度等信息。u估计量经过一系列受噪声污染的观丈量来更新，u观丈量必需是待估参数的函数，但是在给定的时辰，不要求观丈量可以独一确定当时的参数值。1.2 概述目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF

3、自顺应自顺应 KF平滑算法平滑算法.6uKalman滤波是一种递推线性最小方差估计u在提供的初始估计根底上，卡尔曼滤波经过递归运算，用先验值和最新观测数据的加权平均来更新形状估计老息+新息。u非递归算法如规范最小二乘中没有先验估计，估计结果由全部观测数据计算而来新息 。u卡尔曼滤波是一种贝叶斯估计目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法最小方差估计线性最小方差估计递推线性最小方差估计.71.3 卡尔曼滤波的要素和流程实践系统系统模型观测模型观测向量及其协方差形状向量及其协方差卡尔曼滤波算法实线表示数据流不断有，虚线表示只在某些运用中有

4、，Ref：Paul Groves目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.8u 形状向量形状u是一组描画系统的参数。u可以是常量，也可是时变量，是估计对象。u与之相关联的是误差协方差矩阵，描画了形状估计的不确定度及估计误差间的相关度。目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.91.4 卡尔曼滤波的要素4个要素：2个模型、1组观丈量、1个算法 2个模型系统模型也称过程模型或者时间传送模型，描画了形状与误差协方差矩阵随时间的变化特性。对于选定形状量，系统模型是确定的。观测模型描画

5、了观测向量与形状向量间的函数关系。目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.10p 1 1组观测向量组观测向量p是一组针对同一时辰的系统特性的丈量值，例如观丈量是一组针对同一时辰的系统特性的丈量值，例如观丈量可以包括可以包括GNSSGNSS系统的位置丈量值，或者系统的位置丈量值，或者INSINS与与GNSSGNSS位置位置结果的差值。结果的差值。p 1 1个算法：个算法：p卡尔曼滤波算法卡尔曼滤波算法p运用观测向量、观测模型和系统模型来获得形状向量的运用观测向量、观测模型和系统模型来获得形状向量的最优估计，分为系统传送和丈量更新两个部

6、分。最优估计，分为系统传送和丈量更新两个部分。目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.目目 录录111.5 卡尔曼滤波的导航运用p惯性导航系统INS的精对准和标定p单一导航GNSS, 无线电、水声学、匹配p组合导航pINS/GNSS组合导航及多传感器组合导航pINS/水声组合导航pINS/匹配导航p 概述 经典KF EKF LKF.二、二、KalmanKalman滤波滤波2021-12-1412.132.1 卡尔曼滤波方程1. 离散系统的数学描画 设离散化后的系统形状方程和量测方程分别为： kk,k 1k 1k 1k 1kkkkXX

7、WZH XV目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.14p要求Wk和Vk是互不相关的、零均值白噪声序列：TkjkkjTkjkkjE W WQE V VRp Qk和Rk分别称为系统噪声和量测噪声的方差矩阵，分别是知值的非负定阵和正定阵； kj0(kj)1(kj)p k j k j 是是Kronecker Kronecker 函数，即：函数，即：目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.15u 初始形状的一、二阶统计特性为：0 x0E Xm0 x0Var XCu Var 为对求

8、方差的符号u卡尔曼滤波要求mx0和Cx0为知量，u且要求X0与Wk和Vk都不相关目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.162. 离散卡尔曼滤波方程k/k 1k,k 1k 1XX-= p 形状一步预测方程形状一步预测方程kk/k 1kkkk/k 1XXK (ZH X)p 形状估值计算方程形状估值计算方程TT1kk/k 1kkk/k 1kkKPH (H PHR )p 滤波增益方程滤波增益方程TTk/k 1k,k 1 k 1k,k 1k 1k 1k 1PPQ-=+ GGp 一步预测均方差方程一步预测均方差方程TTkkkk/k 1kkkk

9、kP(IK H )P(IK H )K R Kp 估计均方差方程估计均方差方程kkkk/k 1P(IK H )P或 目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.17时间更新方程量测修正方程k/k 1k,k 1k 1XXkk/k 1kkkk/k 1XXK (ZH X)TT1kk/k 1kkk/k 1kkKPH (H PHR )TTk/k 1k,k 1k 1 k,k 1k 1k 1k 1PPQTTkkkk/k 1kkkkkP(IK H )P(IK H )K R K目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应

10、KF平滑算法平滑算法.18TT1kk/k 1kk k/k 1kkKPH (H PHR )TTk/k 1k,k 1 k 1k,k 1k 1k 1k 1PPQTTkkkk/k 1kkkkkP(IK H )P(IK H )K R Kk 1Pkk1k,k 1Tk 1k 1k 1QkRkHkRkHk/k 1PkKk,k 1k/k 1k,k 1k 1XXkk/k 1kkkk/k 1XXK (ZH X)k 1Xkk1kXkZkP滤波计算回路增益计算回路目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.193. 卡尔曼滤波例如目目 录录概述概述规范规范 KF

11、扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法有一个质点，沿X轴正方向运动，质点从X = 0 开场匀速直线运动, 速度为V = 10m/s，那么每一时辰质点的真实位置参考真值为：X = X0+V * t；实践上，我们每隔 0.1s 可以丈量一次质点的位置，但位置丈量值存在误差假设是均值为0的白噪声序列根据我们对质点的位置观丈量，用卡尔曼滤波方法计算每一时辰质点的位置和速度Xv = 10 m/sXi-1XiXi+1位置观测值.20目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法l形状量 x = X, V，即以质点的位置和速

12、度作为卡尔曼滤波形状量；l系统形状方程为 Xk = Xk-1 + Vk-1 * dt ；l形状转移矩阵 Phi = 1 dt; 0 1；l系统噪声协方差阵Q： 即系统模型的不确定度，由于假设模型即质点运动模型，因此可以为模型的不确定度为0，即 Q = 0 0; 0 0l观测矩阵 H：由于只观测了质点位置，未观测速度，因此观测矩阵 H = 1 0;l观测噪声矩阵R：位置观丈量的方差为 m2，即 R = 1l观丈量向量Z：在真实形状真实位置加上均值为零，方差为 m2的白噪声;l卡尔曼滤波初始形状：X0 = 0, V0 = 5 m/s，初始形状误差协方差矩阵P = 1 0; 0 1p 设计卡尔曼滤波

13、.21目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法卡尔曼滤波位置估计012345678910020406080100120时间 (s)位置 (m) 参考真值测量值卡尔曼滤波估值.22目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法卡尔曼滤波速度估计0246810-30-20-1001020304050时间 (s)速度 (m/s) 参考真值位置观测量微分滑动平均法卡尔曼滤波.232.2 闭环卡尔曼滤波1. 全形状滤波和误差形状滤波根据卡尔曼滤波形状向量的选取不同，卡尔曼滤波可分为:全形状卡

14、尔曼滤波Total State Implementation和误差形状卡尔曼滤波Error State Implementation组合导航系统采用卡尔曼滤波进展估计的主要对象位置，L 速度VX ,VY ,VZ姿态,导航参数用X表示目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.24Total State：以系统的固有属性，如位置、速度和姿态等，为形状向量的卡尔曼滤波，称为全形状滤波或直接卡尔曼滤波Error State：以系统丈量误差值，如INS位置、速度和姿态等，为形状向量的卡尔曼滤波称为误差形状滤波或间接卡尔曼滤波直接法直接法间接法间

15、接法以各种导航参数X为主要形状滤波器估值的主要部分即是导航参数的估值以某种导航系统输出导航参数的误差为主要形状滤波器估值的主要部分即是导航参数误差的估值目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法模型能够是线性的，也能够是非线性的模型普通都是线性的.252. 开环卡尔曼滤波25惯性系统卡尔曼滤波器其他导航系统+-XIXINXXD- DNXIX用导航参数误差的估值 去校正系统输出的导航参数，得到综合导航系统的导航参数估值XX目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法开环输出校正的卡尔

16、曼滤波器.263. 闭环卡尔曼滤波26惯性系统卡尔曼滤波器其他导航系统IXINXXD- DNXIXIXIX采用反响校正的间接法估计，是将惯导系统导航参数误差 的估值 反响到惯导系统内，对误差形状进展校正。IXIX目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法闭环反响校正的卡尔曼滤波器.2727 开环滤波仅校正系统输出量，闭环滤波那么是校正系统内部的形状。两种校正方法的性质是一样的，具有同样的精度。 闭环滤波的反响校正使得卡尔曼滤波形状值为小量；开环因无反响，形状值会随时间不断变大。形状方程都是经过一阶近似的线性方程，形状的数值越小，那么近似

17、的准确性越高，因此，利用形状反响校正的系统形状方程，更能接近真实地反映系统误差形状的动态过程。 卡尔曼滤波算法中，反响形状估计的最正确时机是在丈量更新后立刻进展。 卡尔曼滤波的闭环和开环可以混合运用，即一些形状估计作为校正值被反响，而另外一些不反响。目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法4. 开环与闭环卡尔曼滤波对比.285. 混合卡尔曼滤波例如28在松组合算法中，21维向量，其中：位置、速度和姿态只做开环修正；而IMU误差，如陀螺和加速度计零偏，比例因子误差进展反响，修正IMU的原始观测值。目目 录录概述概述规范规范 KF扩展扩展

18、 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法GINS软件松组合算法架构表示图混合滤波.292.3 滤波特性及滤波实现中的问题1. 滤波收敛特性初始不确定度平衡不确定度时间形状不确定度收敛过程中的卡尔曼滤波形状不确定度* 注：形状不确定度是误差协方差矩阵P对角元素的平方根 当卡尔曼滤波形状不确定度接近平衡点，每次丈量更新后形状不确定度的降低量与系统噪声呵斥的不确定度的添加量是匹配的； 在平衡点，不确定度所反映出的估计置信度程度根本固定.目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.30 形状估计的收敛速度根本上取决于该形状的

19、可观测性。 假设观测矩阵随时间变化或者形状之间经过形状转移矩阵存在时间依存关系，那么随着迭代次数的添加，更多的形状量变得可观测。 例：导航中用位置的变化率来确定速度 许多参数的可观测性依赖于系统的动态性 例：姿态不变时，INS姿态误差和加速度计偏向不是独立可观测的； 陀螺仪误差那么需求载体有更高的动态性，方可观测 假设两个形状对观丈量有同样的影响，以一样方式随时间变化，并且具有一样的动态特性，那么它们非独立可观量，在滤波设计时，应将其组合在一同，以免浪费计算资源目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.312. 滤波参数调整形状估计误

20、差P/R太小形状估计误差形状估计误差P/R适中P/R太大时间时间时间目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法不同P/R比率下的卡尔曼滤波误差传送Paul Groves为抑制卡尔曼滤波模型的限制，噪声建模必需足以囊括实践系统的行为，笼统地说，要将真实世界的“方形楔子放到卡尔曼滤波模型的“圆洞中，因此这个洞要被扩宽 。 例：假设忽略1 Hz GPS定位结果误差的时间相关性，应将其放大普通为2-3倍以建模为白噪声.32 P阵非正定 P阵的非正定容易导致滤波发散。 运用高精度变量存储如double类型，减小舍入误差； 缩放卡尔曼滤波标度，使一

21、切形状不确定度在数值上具有一样量级； P阵非对称 运用式 计算P阵，容易导致P阵非对称； 每次系统传送及丈量更新后经过 P=(P+P)/2来坚持对称性； 建议采用Joseph式P阵更新： 平方根滤波 传送 而非P,可把动态范围减小两个量级，从而减小舍入误差影响。kkkk/k 1P(IK H )P目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法 TTkkkk/k 1kkkkkP(IK H )P(IK H )K R K3. 数值计算问题.三、扩展三、扩展KalmanKalman滤波滤波EKFEKF2021-12-1433.34u 规范卡尔曼滤波的

22、线性假设u在规范的卡尔曼滤波中，观测模型假设为线性Z是X的线性函数，但实践情况往往并非如此如GNSS导航滤波器中，观测模型是强非线性的u在规范卡尔曼滤波中，系统模型也被假设为线性的X的时间导数是X的线性函数u问题：在全形状INS/GNSS组合导航中，形状量为绝对位置、速度和姿态，由于难以不断坚持系统的线性近似，完成一切的系统反响并不总是可行的；u 扩展/线性化卡尔曼滤波u 为卡尔曼滤波的非线性方式目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法1. 问题描画3.1 非线性系统.35目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt

23、KF自顺应自顺应 KF平滑算法平滑算法2. 非线性系统描画u普通非线性系统延续和离散系统可由以下方程来表示：u扩展卡尔曼滤波研讨的非线性系统可由以下方程来表示( )( ),( ), ( )( ), ( ), X tf X t W t tZ th X t V t t= &或11,1, kkkkkkXf XWkZh X V k-=-= 为简化问题，需对噪声的统计特性给以符合实践又便于处置的假定或( )( ), ( )( ),( )( )X tf X t tZ thG t W tV tX t t=+&111,1, kkkkkkkXf XkZh XkWV-=-=+ G+ Wt或Wk-1

24、，Vt或Vk 均为彼此不相关的零均值白噪声序列，与初始形状X(0)或X0也不相关 .36根本假设：非线性微分方程的实际解一定存在；实际解与实践解差可以用一个线性微分方程表示，即线性扰动方程。目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法3. 处理方案.373.2 非线性系统的线性化t实践形状 X标称形状 X*n围绕X*线性化： 线性化卡尔曼滤波Linearized Kalman Filter， LKFn围绕实践形状X滤波估计形状，或实践轨迹进展线性化：扩展卡尔曼滤波Extended Kalman Filter， EKF目目 录录概述概述规范

25、规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法泰勒展开法进展线性化形状.38n线性化卡尔曼滤波：围绕标称形状对非线性系统进展线性化或n当Wt或Wk-1，Vt或Vk恒为0时，系统模型的解称为非线性方程的实际解，又称“标称轨迹或标称形状。通常记为Xn(t)或Xkn,和Zn(t)或Zkn，那么有：( )( ), ( )( ), nnnnXtf Xt tZth Xt t1,1, nnkknkkXf XkZh Xk目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.393.3 线性化卡尔曼滤波或u 定义：u非线性系统的真轨

26、迹运动与标称轨迹运动的偏向为：( )( )( )( )( )( )nnX tX tXtZ tZ tZtnkkknkkkXXXZZZn假设这些偏向足够小，那么，可以围绕标称形状把X(t和Z(t展开成泰勒(Taylor)级数，并且可取一次近似值。( )( )( )( )( )( )( )( )( ), ( )( ), ( )( )( )( )( ), ( )( ), ( )( )( )nnx txtx txtnnx txtx txtf X t tX tf X t tX tG t W tX th X t tZ th X t tX tV tX t目目 录录概述概述规范规范 KF扩展扩展 KFSchmi

27、dt KF自顺应自顺应 KF平滑算法平滑算法.403.3 线性化卡尔曼滤波或n那么有 ：( )( )( )( )( )( )( )( )( )( )( )nnnnX tXtFtX tG t W tZ tZtHtX tV t=+ D+=+ D+ &( )( )( )( )( )( )( )( )( )nnX tFtX tG t W tZ tHtX tV tD= D+D= D+ &目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法( )( )( )( )( )( )nnX tX tXtZ tZ tZtD=-D=- .n由于线性化卡尔

28、曼滤波存在上述问题，改用另一种近似方法，即在最优化形状估计 或 附近进展泰勒展开，线性化。413.4 扩展卡尔曼滤波n标称解难以解算n真轨迹与标称轨迹之间的形状差X(t)或Xk不能确保其足够小，从而导致线性化误差较大，模型的线性近似度变弱。uLKF的缺陷kX( )X t目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.目目 录录42 概述 KF方程 EKF LKF或u 定义u非线性系统的真轨迹运动与实践轨迹运动的偏向为：( )( )( )( )( )( )X tX tX tZ tZ tZ t=-=-kkkkkkXXXZZZ=- =- n假设偏

29、向足够小，那么可以围绕最优化形状估计把X(t和Z(t展开成泰勒(Taylor)级数，并且可取一次近似值。( )( )( )( )( )( )( )( )( ), ( )( ), ( )( )( )( )( ), ( )( ), ( )( )( )x tX tx tX tx tX tx tX tf X t tX tf X t tX tG t W tX th X t tZ th X t tX tV tX t=+=+&.43或n那么有 ：( )( )( )( )( )( )( )( )( )( )( )X tX tF tX tG t W tZ tZ tH tX tV t=+=+ &(

30、)()( )( )( )( )( )( )( )( )( )X tF tX tG t W tZ tH tX tV t=+=+nEKF假设形状向量估计的误差远比形状向量本身小，因此可用线性系统模型计算形状向量残差。n规范的误差协方差传送公式可采用在最优形状估计附近进展线性化的形状转移矩阵，即()1111( )expkkkxxkf xFFx-=-=目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.四、四、SchmidtSchmidt卡尔曼滤波卡尔曼滤波2021-12-1444.454.1 时间相关噪声目目 录录概述概述规范规范 KF扩展扩展 KF

31、Schmidt KF自顺应自顺应 KF平滑算法平滑算法1. 观测噪声u 规范卡尔曼滤波的观测噪声假设u普通假设一切的观测噪声是时间不相关的，即观测噪声是白噪声。u 问题：u该假设常不成立；卡尔曼滤波强迫把新息中的时间相关部分归因形状，因此，时间相关的观测噪声能够会破坏形状估计。u 处置观测噪声时间相关性问题的方法u 将时间相关噪声扩展至卡尔曼滤波的形状向量中，进展估计；u 放大观测噪声协方差矩阵R，进而减小卡尔曼滤波的增益；u利用Schmidt卡尔曼滤波.46目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法2. 系统噪声u 规范卡尔曼滤波的系

32、统噪声假设u规范的卡尔曼滤波中，普通假设一切的系统噪声是时间不相关的，即观测噪声是白噪声。u 问题：u系统常含有显著的系统性噪声和其它时间相关噪声；u这些噪声能够由于可观测性较差未被选为形状量；u但会影响被估计的形状u 处置系统噪声时间相关性问题的方法u 当相关时间较短时：建模为白噪声，但需覆盖会影响卡尔曼滤波收敛的相关噪声；u 当相关时间超越1min：采用带不确定参数的Schmidt卡尔曼滤波；.474.2 Schmidt卡尔曼滤波目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法Schmidt 卡尔曼滤波有效地增大误差协方差阵P,以此来对时

33、间相关噪声进展建模,到达与卡尔曼滤波增益类似的效果.Schmidt卡尔曼滤波符号: - 时间相关系统噪声和时间相关观测噪声的未估计参数的协方差阵.U -相关矩阵,是对未估计参数和形状量之间相关性的建模 - 相关噪声参数的转移矩阵 -系统噪声协方差阵 - 转移矩阵,描画形状如何随未知参数变化的模型.48目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法Schmidt 卡尔曼率滤波对应的误差协方差系统传送式为:将误差协方差P, 相关噪声协方差W, 相关矩阵U分解为不同传送方程:.49目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF

34、自顺应自顺应 KF平滑算法平滑算法类似于观测矩阵H, 定义J 为待估计参数到观测向量的耦合矩阵,那么卡尔曼滤波增益变为:Schmidt 卡尔曼滤波误差协方差, 相关噪声和相关矩阵更新为:.50目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法从Schmidt卡尔曼滤波方程中可以看出:假设待估参数与观测向量无直接相关时, 那么 J 为零矩阵, 增益矩阵的方式与规范卡尔曼滤波一样,但是误差协方差变大了.假设没有形状随待估计参数的变化而变化时，即形状量不是待估参数的函数，那么零矩阵,此时误差协方差阵的方式与规范卡尔曼滤波方式一样;留意: 待估参数可像

35、形状量一样，影响系统模型和观测模型.五、自顺应卡尔曼滤波五、自顺应卡尔曼滤波2021-12-1451.525. 自顺应卡尔曼滤波目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法1. 问题描画多数运用中，Kalman滤波中的系统噪声协方差阵Q和观测噪声协方差阵R可事先经过系统丈量、仿真和实验等方法确定。例：在INS/GNSS组合导航中，Q阵常由IMU的性能参数如VRW, ARW，零偏的一阶高斯马尔可夫过程参数确定GNSS噪声由位置的点位中误差或误差建模 问题：有些情况上述参数是无法获取的，例如：1MEMS IMU在出厂时未经过严厉标定；2假设没

36、有飞机武器挂仓的先验振动环境信息，那么无法获得观测噪声协方差阵； 处理方法Kalman滤波自行估计矩阵Q或/和R，即自顺应卡尔曼滤波.基于新息的自顺应估计Innovation Based Adaptive Estimation, IAE，要从丈量新息统计中计算Q、R。计算最后n个丈量信息的协方差上述协方差矩阵C用于计算R和/或Q当处置第一组观测新息统计值时，必需提供R和Q初始值，初始值的选定须谨慎53目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法2. 基于新息的自顺应估计,1kTjjz kjknCzzn-=-=%,/1,Tkkkz kTkk

37、k kkz kQK CKRCH PH-=-%.多模型自顺应估计Multiple Model Adaptive Estimation, MMAE利用一组并行的卡尔曼滤波器进展计算，每一个滤波器对应于不同的系统噪声和/或观测噪声协方差矩阵Q和R。第i个卡尔曼滤波器被分配的概率为随时间推移，最优滤波假设的概率会接近1，而其它的接近0；为充分利用计算处置才干，可剔除弱的滤波假设，并周期性地细分最强的假设用于精化滤波器参数。54目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法3. 多模型自顺应估计.完好的形状向量估计和误差协方差计算公式如下： IAE新

38、息 和 MMAE多模型方法比较MMAE计算量大IAE中，Q,R,P和滤波增益均能够是形状估计的函数；而他们在MMAE滤波器组规范卡尔曼滤波而非EKF中是相互独立的，因此MMAE滤波器更趋于稳定55目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法.六、平滑算法六、平滑算法2021-12-1456.576. 6. 平滑算法平滑算法目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法1. 问题描画u 实时处置/后处置差别u卡尔曼滤波器是为实时运用而设计的，用于估计给定时辰的系统特性，所用新息是这一时辰

39、之前的系统可以观丈量。u当需求在某时间发生后获得系统特性时，卡尔曼滤波仍旧只用1/2的观测新息，由于卡尔曼滤波器没有运用所关注时间点之后的观丈量u 卡尔曼平滑器u是卡尔曼滤波器的扩展，不仅适用所关注时间点之前的观测新息，且适用之后的新息；非实时运用中，平滑处置可以得到更高的精度。u 常用平滑算法u包含两类主要方法：1正向-方向滤波；2RTS平滑算法.58目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法2. 正向-反向滤波算法经过正向滤波和反向滤波两个相互独立的滤波器结果的加权平均，平滑结果由下式给定：下标f和b分别表示正向和反向；k表示两个滤

40、波器中的一样时间点正向-反向卡尔曼平滑算法的形状不确定度正向滤波反向滤波形状不确定度时间前向滤波.59目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法3. RTS平滑算法：RTSRauch,Tung and Striebel, RTS传统的卡尔曼滤波随时间向前运转，在每次系统传送和丈量更新后记录形状向量X、误差协方差阵P及形状转移矩阵 ，一旦运算到数据的结尾，那么开场反向从末尾到起始点进展数据平滑每次迭代的平滑增益为：平滑后的形状向量和误差协方差阵为：当需对每个点进展平滑时，RTS算法更有效，假设只需对单点进展平滑时，正向-反向算法更有效.6

41、0附录：卡尔曼滤波例如MATLAB源代码目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法参考参考:Alex Blekhman, mathworks/matlabcentral/fileexchange% Set true trajectory Nsamples=100;dt = .1;t=0:dt:dt*Nsamples;Vtrue = 10; % Xtrue is a vector of true positions of the train Xinitial = 0;Xtrue = Xinitial + Vtrue * t; % Moti

42、on equations% Previous state (initial guess): Our guess is that the train starts at 0 with velocitythat equals to 50% of the real velocityXk_prev = 0; 0.5*Vtrue; % Current state estimateXk=;.61附录：卡尔曼滤波例如MATLAB源代码目目 录录概述概述规范规范 KF扩展扩展 KFSchmidt KF自顺应自顺应 KF平滑算法平滑算法Phi = 1 dt; 0 1;sigma_model = 1;P = sigma_model2 0; 0 sigma_model2;Q = 0 0; 0 0; % M is the measurement matrix. M = 1 0;sigma_meas = 1; % 1 m/secR = sigm