卡尔曼滤波专题培训课件.ppt

1、：卡尔曼滤波：卡尔曼滤波2目录一. 概述二. 标准卡尔曼滤波n卡尔曼滤波方程n闭环卡尔曼滤波n卡尔曼滤波特性及实现中的问题三. 扩展卡尔曼滤波n非线性系统n线性化卡尔曼滤波n扩展卡尔曼滤波四. Schmidt 卡尔曼滤波五. 自适应卡尔曼滤波六. 平滑算法2022-5-173一、概述一、概述41.1 Rudolf Emil KalmanuBorn 1930 in Hungary uBS and MS from MIT uPhD 1957 from Columbia uFilter developed in 1960-61目目 录录 概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算

2、法Kalman R E. A new approach to linear filtering and prediction problems J. Journal of Fluids Engineering, 1960, 82(1): 35-45. （引用：18083）5uKalman滤波是一种最优估计估计算法，而非滤波器 能够实时估计估计系统中的参数参数（如连续变化的位置、速度等信息）。 估计量估计量通过一系列受噪声污染的观测量观测量来更新， 观测量观测量必须是待估参数的函数待估参数的函数，但是在给定的时刻，不要求观测量能够唯一确定当时的参数值。1.2 概述目目 录录概述标准 KF扩展 K

3、FSchmidt KF自适应 KF平滑算法6uKalman滤波是一种递推线性最小方差估计递推线性最小方差估计在提供的初始估计基础上，卡尔曼滤波通过递归递归运算，用先验值先验值和最最新观测数据新观测数据的加权平均来更新状态估计（老息老息+ +新息新息）。非递归算法（如标准最小二乘）中没有先验估计，估计结果由全部观测数据计算而来（新息新息） 。u卡尔曼滤波是一种贝叶斯估计目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法最小方差估计线性最小方差估计递推线性最小方差估计71.3 卡尔曼滤波的要素和流程实际系统系统模型观测模型观测向量及其协方差状态向量及其协方差卡尔曼滤波算法（

4、实线表示数据流一直有，虚线表示只在某些应用中有，Ref：Paul Groves）目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法8u 状态向量（状态） 是一组描述系统的参数。 可以是常量，也可是时变量，是估计对象。 与之相关联的是误差协方差矩阵，描述了状态估计的不确定度及估计误差间的相关度。目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法91.4 卡尔曼滤波的要素4个要素：2个模型、1组观测量、1个算法 p 2 2个模型个模型u系统模型系统模型也称过程模型或者时间传递模型，描述了状态与误差协方差矩阵随时间的变化特性。对于选定状态量，系统模型是

5、确定的。u观测模型描述了观测向量与状态向量间的函数关系。目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法10p 1 1组观测向量组观测向量是一组针对同一时刻的系统特性的测量值，例如观测量可以包括GNSS系统的位置测量值，或者INS与GNSS位置结果的差值。p 1 1个算法：个算法：卡尔曼滤波算法使用观测向量、观测模型和系统模型来获得状态向量的最优估计，分为系统传递和测量更新两个部分。目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法11目目 录录1.5 卡尔曼滤波的导航应用p惯性导航系统（INS）的精对准和标定p单一导航（GNSS, 无线电、水

6、声学、匹配）p组合导航INS/GNSS组合导航及多传感器组合导航INS/水声组合导航INS/匹配导航p 概述 经典KF EKF LKF2022-5-1712二、二、KalmanKalman滤波滤波132.1 卡尔曼滤波方程1. 离散系统的数学描述u 设离散化后的系统状态方程和量测方程分别为： kk,k 1k 1k 1k 1kkkkXXWZH XV目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法14p要求WWk k和V Vk k是互不相关的、零均值白噪声序列：TkjkkjTkjkkjE W WQE V VRp Q Qk k和R Rk k分别称为系统噪声和量测噪声的方差矩

7、阵，分别是已知值的非负定阵和正定阵； kj0(kj)1(kj)p k j k j 是Kronecker 函数，即：目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法15u 初始状态的一、二阶统计特性为：0 x0E Xm0 x0Var XCu Var 为对求方差的符号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 状态估值计算

8、方程状态估值计算方程TT1kk/k 1kkk/k 1kkKPH (H PHR )p 滤波增益方程滤波增益方程TTk/k 1k,k 1 k 1k,k 1k 1k 1k 1PPQ-=+ GGp 一步预测均方差方程一步预测均方差方程TTkkkk/k 1kkkkkP(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 P

9、HR )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自适应 KF平滑算法18TT1kk/k 1kk k/k 1kkKPH (H PHR )TTk/k 1k,k 1 k 1 k,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 1X

10、kk1kXkZkP滤波计算回路增益计算回路目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法193. 卡尔曼滤波示例目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法有一个质点，沿X轴正方向运动，质点从X = 0 开始匀速直线运动, 速度为V = 10m/s，则每一时刻质点的真实位置（参考真值）为：X = X0+V * t；实际上，我们每隔 0.1s 可以测量一次质点的位置，但位置测量值存在误差（假设是均值为0的白噪声序列）根据我们对质点的位置观测量，用卡尔曼滤波方法计算每一时刻质点的位置和速度Xv = 10 m/sXi-1XiXi+1位置观测值

11、20目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法l状态量 x = X, V，即以质点的位置和速度作为卡尔曼滤波状态量；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的白噪

12、声;l卡尔曼滤波初始状态：X0 = 0, V0 = 5 m/s，初始状态误差协方差矩阵P = 1 0; 0 1p 设计卡尔曼滤波21目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法卡尔曼滤波位置估计012345678910020406080100120时间 (s)位置 (m) 参考真值测量值卡尔曼滤波估值22目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法卡尔曼滤波速度估计0246810-30-20-1001020304050时间 (s)速度 (m/s) 参考真值位置观测量微分滑动平均法卡尔曼滤波232.2 闭环卡尔曼滤波1. 全状态滤波和误

13、差状态滤波根据卡尔曼滤波状态向量的选取不同，卡尔曼滤波可分为:全状态卡尔曼滤波（Total State Implementation）和误差状态卡尔曼滤波（Error State Implementation）组合导航系统采用卡尔曼滤波进行估计的主要对象位置，L 速度VX ,VY ,VZ姿态,导航参数用X表示目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法24Total State：以系统的固有属性，如位置、速度和姿态等，为状态向量的卡尔曼滤波，称为全状态滤波或直接卡尔曼滤波Error State：以系统测量误差值，如INS位置、速度和姿态等，为状态向量的卡尔曼滤波称

14、为误差状态滤波或间接卡尔曼滤波直接法直接法间接法间接法以各种导航参数X为主要状态滤波器估值的主要部分即是导航参数的估值以某种导航系统输出导航参数的误差为主要状态滤波器估值的主要部分即是导航参数误差的估值目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法模型可能是线性的，也可能是非线性的模型一般都是线性的2022-5-17252. 开环卡尔曼滤波惯性系统卡尔曼滤波器其他导航系统+-XIXINXXD- DNXIX用导航参数误差的估值 去校正系统输出的导航参数，得到综合导航系统的导航参数估值XX目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法开环（输

15、出校正）的卡尔曼滤波器2022-5-17263. 闭环卡尔曼滤波惯性系统卡尔曼滤波器其他导航系统IXINXXD- DNXIXIXIX采用反馈校正的间接法估计，是将惯导系统导航参数误差 的估值 反馈到惯导系统内，对误差状态进行校正。IXIX目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法闭环（反馈校正）的卡尔曼滤波器2022-5-1727 开环滤波仅校正系统输出量，闭环滤波则是校正系统内部的状态。两种校正方法的性质是一样的，具有同样的精度。 闭环滤波的反馈校正使得卡尔曼滤波状态值为小量；开环因无反馈，状态值会随时间不断变大。状态方程都是经过一阶近似的线性方程，状态的数值

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

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

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

19、R太小状态估计误差状态估计误差P/R适中P/R太大时间时间时间目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法不同P/R比率下的卡尔曼滤波误差传递（Paul Groves）为克服卡尔曼滤波模型的限制，噪声建模必须足以囊括实际系统的行为，形象地说，要将真实世界的“方形楔子”放到卡尔曼滤波模型的“圆洞”中，因此这个洞要被扩宽 。 例：例：若忽略1 Hz GPS定位结果误差的时间相关性，应将其放大（一般为2-3倍）以建模为白噪声32 P阵非正定P阵的非正定容易导致滤波发散。l使用高精度变量存储（如double类型），减小舍入误差；l缩放卡尔曼滤波标度，使所有状态不确定度在数

20、值上具有相同量级； P阵非对称使用式 计算P阵，容易导致P阵非对称；l每次系统传递及测量更新后通过 P=(P+P)/2来保持对称性；l建议采用Joseph式P阵更新： 平方根滤波传递 而非P,可把动态范围减小两个量级，从而减小舍入误差影响。kkkk/k 1P(IK H )P目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法 TTkkkk/k 1kkkkkP(IK H )P(IK H )K R K3. 数值计算问题2022-5-1733三、扩展三、扩展KalmanKalman滤波滤波（EKFEKF）34u 标准卡尔曼滤波的线性假设 在标准的卡尔曼滤波中，观测模型观测模型假

21、设为线性（Z Z是X X的线性函数），但实际情况往往并非如此（如GNSS导航滤波器中，观测模型是强非线性的） 在标准卡尔曼滤波中，系统模型系统模型也被假设为线性的（X的时间导数是X的线性函数）问题：问题：在全状态INS/GNSS组合导航中，状态量为绝对位置、速度和姿态，因为难以一直保持系统的线性近似，完成所有的系统反馈并不总是可行的；u 扩展扩展/ /线性化线性化卡尔曼滤波 为卡尔曼滤波的非线性形式目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法1. 问题描述3.1 非线性系统35目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法2. 非线性系

22、统描述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+ 2022-5-1736如何解决非线性问题如何解决非线性问题采用近似的方法采用近

23、似的方法通常通常线性化方法线性化方法目前应用比较广泛的是目前应用比较广泛的是对非线性模型的线性化对非线性模型的线性化基本假设： 非线性微分方程的理论解一定存在； 理论解与实际解差能够用一个线性微分方程表示，即线性线性扰动扰动方程方程。目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法3. 解决方案373.2 非线性系统的线性化t实际状态 X标称状态 X*n围绕X*线性化： 线性化卡尔曼滤波（Linearized Kalman Filter， ）n围绕实际状态X（滤波估计状态，或实际轨迹）进行线性化：扩展卡尔曼滤波（Extended Kalman Filter）目目 录录

24、概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法泰勒展开法进行线性化状态38n线性化卡尔曼滤波：围绕标称状态对非线性系统进行线性化或n当WWt t或WWk-1k-1，V Vt t或V Vk k恒为0时，系统模型的解称为非线性方程的理论解理论解，又称“标称轨迹”或标称状态。通常记为X Xn n(t)(t)或X Xk kn n, ,和Z Zn n(t)(t)或Z Zk kn n，则有：( )( ), ( )( ), nnnnXtf Xt tZth Xt t1,1, nnkknkkXf XkZh Xk目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法393.3

25、 线性化卡尔曼滤波或u 定义：非线性系统的真轨迹运动与标称轨迹运动的偏差为：( )( )( )( )( )( )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 t

26、X t目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法2022-5-17403.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平滑算法( )( )( )( )(

27、)( )nnX tX tXtZ tZ tZtD=-D=- 2022-5-1741n由于线性化卡尔曼滤波存在上述问题，改用另一种近似方法，即在最优化状态估计最优化状态估计 或 附近进行泰勒展开，线性化。3.4 扩展卡尔曼滤波n标称解难以解算n真轨迹与标称轨迹之间的状态差X(t)或Xk不能确保其足够小，从而导致线性化误差较大，模型的线性近似度变弱。uLKF的缺陷kX( )X t目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法扩展卡尔曼滤波扩展卡尔曼滤波2022-5-1742目目 录录 概述 KF方程 EKF LKF或u 定义非线性系统的真轨迹运动与实际轨迹运动的偏差为：(

28、 )( )( )( )( )( )X tX tX tZ tZ tZ t=-=-kkkkkkXXXZZZ=- =- n若偏差足够小，则可以围绕最优化状态估计最优化状态估计把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则有 ：( )( )( )( )( )(

29、 )( )( )( )( )( )X tX tF tX tG t W tZ tZ tH tX tV t=+=+ &()()( )( )( )( )( )( )( )( )( )X tF tX tG t W tZ tH tX tV t=+=+nEKF假设状态向量估计的误差远比状态向量本身小，因此可用线性系统模型计算状态向量残差。n标准的误差协方差传递公式可采用在最优状态估计最优状态估计附近进行线性化的状态转移矩阵，即()1111( )expkkkxxkf xFFx-=-=目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法2022-5-1744四、四、SchmidtSchm

30、idt卡尔曼滤波卡尔曼滤波2022-5-17454.1 时间相关噪声目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法1. 观测噪声u 标准卡尔曼滤波的观测噪声假设一般假设所有的观测噪声是时间不相关的，即观测噪声是白噪声。u 问题：该假设常不成立；卡尔曼滤波强制把新息中的时间相关部分归因状态，因此，时间相关的观测噪声可能会破坏状态估计。u 处理观测噪声时间相关性问题的方法 将时间相关噪声扩展至卡尔曼滤波的状态向量中，进行估计； 放大观测噪声协方差矩阵R，进而减小卡尔曼滤波的增益； 利用Schmidt卡尔曼滤波2022-5-1746目目 录录概述标准 KF扩展 KFSch

31、midt KF自适应 KF平滑算法2. 系统噪声u 标准卡尔曼滤波的系统噪声假设标准的卡尔曼滤波中，一般假设所有的系统噪声是时间不相关的，即观测噪声是白噪声。u 问题：系统常含有显著的系统性噪声和其它时间相关噪声；这些噪声可能由于可观测性较差未被选为状态量；但会影响被估计的状态u 处理系统噪声时间相关性问题的方法 当相关时间较短时：建模为白噪声，但需覆盖会影响卡尔曼滤波收敛的相关噪声； 当相关时间超过1min：采用带不确定参数的Schmidt卡尔曼滤波；2022-5-17474.2 Schmidt卡尔曼滤波目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法 2022-5

32、-1748目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法Schmidt 卡尔曼率滤波对应的误差协方差系统传递式为:将误差协方差P, 相关噪声协方差W, 相关矩阵U分解为不同传递方程:2022-5-1749目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法类似于观测矩阵H, 定义J 为待估计参数到观测向量的耦合矩阵,则卡尔曼滤波增益变为:Schmidt 卡尔曼滤波误差协方差, 相关噪声和相关矩阵更新为:2022-5-1750目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法 2022-5-1751五、自适应卡尔曼滤波五、

33、自适应卡尔曼滤波2022-5-17525. 自适应卡尔曼滤波目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法1. 问题描述多数应用中，Kalman滤波中的系统噪声协方差阵Q和观测噪声协方差阵R可事先通过系统测量、仿真和实验等方法确定。例： 在INS/GNSS组合导航中，Q阵常由IMU的性能参数如VRW, ARW，零偏的一阶高斯马尔可夫过程参数确定 GNSS噪声由位置的点位中误差或误差建模u 问题：有些情况上述参数是无法获取的，例如：1）MEMS IMU在出厂时未经过严格标定；2）如果没有飞机武器挂仓的先验振动环境信息，则无法获得观测噪声协方差阵；u 解决办法Kalma

34、n滤波自行估计矩阵Q或/和R，即自适应卡尔曼滤波2022-5-1753基于新息的自适应估计（Innovation Based Adaptive Estimation, IAE），要从测量新息统计中计算Q、R。 计算最后n个测量信息的协方差 上述协方差矩阵C用于计算R和/或Q当处理第一组观测新息统计值时，必须提供R和Q初始值，初始值的选定须谨慎目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法2. 基于新息的自适应估计,1kTjjz kjknCzzn-=-=%,/1,Tkkkz kTkkk kkz kQK CKRCH PH-=-%2022-5-1754多模型自适应估计（M

35、ultiple Model Adaptive Estimation, MMAE）利用一组并行的卡尔曼滤波器进行计算，每一个滤波器对应于不同的系统噪声和/或观测噪声协方差矩阵Q和R。 第i个卡尔曼滤波器被分配的概率为 随时间推移，最优滤波假设的概率会接近1，而其它的接近0； 为充分利用计算处理能力，可剔除弱的滤波假设，并周期性地细分最强的假设用于精化滤波器参数。目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法3. 多模型自适应估计2022-5-1755完整的状态向量估计和误差协方差计算公式如下：u IAE（新息） 和 MMAE（多模型）方法比较MMAE计算量大IAE中，

36、Q,R,P和滤波增益均可能是状态估计的函数；而他们在MMAE滤波器组（标准卡尔曼滤波而非EKF）中是相互独立的，因此MMAE滤波器更趋于稳定目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法2022-5-1756六、平滑算法六、平滑算法576. 6. 平滑算法平滑算法目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法1. 问题描述u 实时处理/后处理差异卡尔曼滤波器是为实时应用而设计的，用于估计给定时刻的系统特性，所用新息是这一时刻之前的系统能够观测量。当需要在某时间发生后获得系统特性时，卡尔曼滤波仍旧只用1/2的观测新息，因为卡尔曼滤波器没有使

37、用所关注时间点之后的观测量u 卡尔曼平滑器是卡尔曼滤波器的扩展，不仅适用所关注时间点之前的观测新息，且适用之后的新息；非实时应用中，平滑处理能够得到更高的精度。u 常用平滑算法包含两类主要方法：1）正向-方向滤波；2）RTS平滑算法2022-5-1758目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法2. 正向-反向滤波算法通过正向滤波和反向滤波（两个相互独立的滤波器）结果的加权平均，平滑结果由下式给定：下标f和b分别表示正向和反向；k表示两个滤波器中的相同时间点正向-反向卡尔曼平滑算法的状态不确定度正向滤波反向滤波状态不确定度时间前向滤波2022-5-1759目目

38、录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法3. RTS平滑算法：RTS（Rauch,Tung and Striebel, RTS）传统的卡尔曼滤波随时间向前运行，在每次系统传递和测量更新后记录状态向量X、误差协方差阵P及状态转移矩阵 ，一旦运算到数据的结尾，则开始反向从末尾到起始点进行数据平滑 每次迭代的平滑增益为： 平滑后的状态向量和误差协方差阵为：当需对每个点进行平滑每个点进行平滑时，RTS算法更有效，若只需对单点单点进行平滑时，正向-反向算法更有效 2022-5-17陈起金 | 卡尔曼滤波60附录：卡尔曼滤波示例MATLAB源代码目目 录录概述标准 KF扩展 K

39、FSchmidt KF自适应 KF平滑算法参考参考:Alex Blekhman, http:/ 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; % Motion equations% Previous state (initial guess): Our guess is that the train starts

40、 at 0 with velocitythat equals to 50% of the real velocityXk_prev = 0; 0.5*Vtrue; % Current state estimateXk=;2022-5-17陈起金 | 卡尔曼滤波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.

41、M = 1 0;sigma_meas = 1; % 1 m/secR = sigma_meas2; % = Kalman iteration% Buffers for later displayXk_buffer = zeros(2,Nsamples+1);Xk_buffer(:,1) = Xk_prev;Z_buffer = zeros(1,Nsamples+1);2022-5-17陈起金 | 卡尔曼滤波62附录：卡尔曼滤波示例MATLAB源代码目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法for k=1:Nsamples Z = Xtrue(k+1)+sigma

42、_meas*randn; Z_buffer(k+1) = Z; % Kalman iteration P1 = Phi*P*Phi + Q; S = M*P1*M + R;goes to the measurement. % If K is low, more weight goes to the model prediction. K = P1*M*inv(S); P = P1 - K*M*P1; Xk = Phi*Xk_prev + K*(Z-M*Phi*Xk_prev); Xk_buffer(:,k+1) = Xk; % For the next iteration Xk_prev =

43、Xk; end;2022-5-17陈起金 | 卡尔曼滤波63附录：卡尔曼滤波示例MATLAB源代码目目 录录概述标准 KF扩展 KFSchmidt KF自适应 KF平滑算法figure;plot(t, Xtrue,k-., t, Z_buffer,r., t, Xk_buffer(1,:),b); xlabel(时间 (s),fontsize, 15);ylabel(位置 (m),fontsize, 15);legend(参考真值,测量值,卡尔曼滤波估值,fontsize, 15);grid on 2022-5-17陈起金 | 卡尔曼滤波64附录：卡尔曼滤波示例MATLAB源代码目目 录录概述

44、标准 KF扩展 KFSchmidt KF自适应 KF平滑算法% = Velocity analysisInstantaneousVelocity = 0 (Z_buffer(2:Nsamples+1)-Z_buffer(1:Nsamples)/dt;% 滑动平均WindowSize = 5;InstantaneousVelocityRunningAverage = filter(ones(1,WindowSize)/WindowSize,1,InstantaneousVelocity);figure, plot(t,ones(size(t)*Vtrue,g,LineWidth, 2); hold on;plot(t,InstantaneousVelocity,r-*,LineWidth, 2);plot(t,InstantaneousVelocityRunningAverage,k.,LineWidth, 2);plot(t,Xk_buffer(2,:),b,LineWidth, 2);xlabel(时间 (s),fontsize,15);ylabel(速度 (m/s),fontsize,15);legend(参考真值,位置观测量微分,滑动平均法,卡尔曼滤波,15);set(gca, fontsize, 15)grid on