第五讲：卡尔曼滤波

卡尔曼滤波算法及应用目录2一.概述二.标准卡尔曼滤波卡尔曼滤波方程闭环卡尔曼滤波卡尔曼滤波特性及实现中的问题三.扩展卡尔曼滤波非线性系统线性化卡尔曼滤波扩展卡尔曼滤波四.Schmidt卡尔曼滤波五.自适应卡尔曼滤波六.平滑算法一、概述2020/5/2431.1RudolfEmilKalman4Born1930inHungaryBSandMSfromMITPhD1957fromColumbiaFilterdevelopedin1960-61目录•概述标准KF扩展KFSchmidtKF自适应KF平滑算法KalmanRE.Anewapproachtolinearfilteringandpredictionproblems[J].JournalofFluidsEngineering,1960,82(1):35-45.（引用：18083）1.2概述5Kalman滤波是一种最优估计算法，而非滤波器能够实时估计系统中的参数（如连续变化的位置、速度等信息）。估计量通过一系列受噪声污染的观测量来更新，观测量必须是待估参数的函数，但是在给定的时刻，不要求观测量能够唯一确定当时的参数值。目录•概述标准KF扩展KFSchmidtKF自适应KF平滑算法6Kalman滤波是一种递推线性最小方差估计在提供的初始估计基础上，卡尔曼滤波通过递归运算，用先验值和最新观测数据的加权平均来更新状态估计（老息+新息）。非递归算法（如标准最小二乘）中没有先验估计，估计结果由全部观测数据计算而来（新息）。卡尔曼滤波是一种贝叶斯估计目录•概述标准KF扩展KFSchmidtKF自适应KF平滑算法最小方差估计线性最小方差估计递推线性最小方差估计1.3卡尔曼滤波的要素和流程7实际系统系统模型观测模型观测向量及其协方差状态向量及其协方差卡尔曼滤波算法（实线表示数据流一直有，虚线表示只在某些应用中有，Ref：PaulGroves）目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法8状态向量（状态）是一组描述系统的参数。可以是常量，也可是时变量，是估计对象。与之相关联的是误差协方差矩阵，描述了状态估计的不确定度及估计误差间的相关度。目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法1.4卡尔曼滤波的要素94个要素：2个模型、1组观测量、1个算法2个模型系统模型也称过程模型或者时间传递模型，描述了状态与误差协方差矩阵随时间的变化特性。对于选定状态量，系统模型是确定的。观测模型描述了观测向量与状态向量间的函数关系。目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法101组观测向量是一组针对同一时刻的系统特性的测量值，例如观测量可以包括GNSS系统的位置测量值，或者INS与GNSS位置结果的差值。1个算法：卡尔曼滤波算法使用观测向量、观测模型和系统模型来获得状态向量的最优估计，分为系统传递和测量更新两个部分。目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法1.5卡尔曼滤波的导航应用11目录惯性导航系统（INS）的精对准和标定单一导航（GNSS,无线电、水声学、匹配）组合导航INS/GNSS组合导航及多传感器组合导航INS/水声组合导航INS/匹配导航…•概述•经典KF•EKF•LKF二、Kalman滤波2020/5/24122.1卡尔曼滤波方程131.离散系统的数学描述设离散化后的系统状态方程和量测方程分别为：kk,k1k1k1k1kkkkXXWZHXVXk为k时刻的n维状态向量（被估计量）Zk为k时刻的m维量测向量k-1到k时刻的系统一步状态转移矩阵（n×n阶）Wk-1为k-1时刻的系统噪声（r维）Γk-1为系统噪声矩阵（n×r阶）Hk为k时刻系统量测矩阵（m×n阶）Vk为k时刻m维量测噪声目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法14要求{Wk}和{Vk}是互不相关的、零均值白噪声序列：TkjkkjTkjkkjEWWQEVVRQk和Rk分别称为系统噪声和量测噪声的方差矩阵，分别是已知值的非负定阵和正定阵；kj0(kj)1(kj)δkj是Kroneckerδ函数，即：目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法15初始状态的一、二阶统计特性为：0x0EXm0x0VarXCVar{·}为对{·}求方差的符号卡尔曼滤波要求mx0和Cx0为已知量，且要求X0与{Wk}和{Vk}都不相关目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法162.离散卡尔曼滤波方程k/k1k,k1k1ˆˆXX---=状态一步预测方程kk/k1kkkk/k1ˆˆˆXXK(ZHX)状态估值计算方程TT1kk/k1kkk/k1kkKPH(HPHR)滤波增益方程TTk/k1k,k1k1k,k1k1k1k1PPQ-------=+GG一步预测均方差方程TTkkkk/k1kkkkkP(IKH)P(IKH)KRK估计均方差方程kkkk/k1P(IKH)P或目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法17时间更新方程量测修正方程k/k1k,k1k1ˆˆXXkk/k1kkkk/k1ˆˆˆXXK(ZHX)TT1kk/k1kkk/k1kkKPH(HPHR)TTk/k1k,k1k1k,k1k1k1k1PPQTTkkkk/k1kkkkkP(IKH)P(IKH)KRK目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法18TT1kk/k1kkk/k1kkKPH(HPHR)TTk/k1k,k1k1k,k1k1k1k1PPQTTkkkk/k1kkkkkP(IKH)P(IKH)KRKk1Pkk1k,k1Tk1k1k1QkRkHkRkHk/k1PkKk,k1k/k1k,k1k1ˆˆXXkk/k1kkkk/k1ˆˆˆXXK(ZHX)k1ˆXkk1kˆXkZkP滤波计算回路增益计算回路目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法193.卡尔曼滤波示例目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法有一个质点，沿X轴正方向运动，质点从X=0开始匀速直线运动,速度为V=10m/s，则每一时刻质点的真实位置（参考真值）为：X=X0+V*t；实际上，我们每隔0.1s可以测量一次质点的位置，但位置测量值存在误差（假设是均值为0的白噪声序列）根据我们对质点的位置观测量，用卡尔曼滤波方法计算每一时刻质点的位置和速度Xv=10m/sXi-1XiXi+1位置观测值20目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法状态量x=[X,V]，即以质点的位置和速度作为卡尔曼滤波状态量；系统状态方程为Xk=Xk-1+Vk-1*dt；状态转移矩阵Phi=[1dt;01]；系统噪声协方差阵Q：即系统模型的不确定度，由于假设模型即质点运动模型，因此可认为模型的不确定度为0，即Q=[00;00]观测矩阵H：由于只观测了质点位置，未观测速度，因此观测矩阵H=[10];观测噪声矩阵R：位置观测量的方差为m2，即R=1观测量向量Z：在真实状态（真实位置）加上均值为零，方差为m2的白噪声;卡尔曼滤波初始状态：X0=0,V0=5m/s，初始状态误差协方差矩阵P=[10;01]设计卡尔曼滤波21目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法卡尔曼滤波位置估计012345678910020406080100120时间(s)位置(m)参考真值测量值卡尔曼滤波估值22目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法卡尔曼滤波速度估计0246810-30-20-1001020304050时间(s)速度(m/s)参考真值位置观测量微分滑动平均法卡尔曼滤波2.2闭环卡尔曼滤波231.全状态滤波和误差状态滤波根据卡尔曼滤波状态向量的选取不同，卡尔曼滤波可分为:全状态卡尔曼滤波（TotalStateImplementation）和误差状态卡尔曼滤波（ErrorStateImplementation）组合导航系统采用卡尔曼滤波进行估计的主要对象导航参数导航参数位置λ，L速度VX,VY,VZ姿态ψ,θ,γ导航参数用X表示目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法24TotalState：以系统的固有属性，如位置、速度和姿态等，为状态向量的卡尔曼滤波，称为全状态滤波或直接卡尔曼滤波ErrorState：以系统测量误差值，如INS位置、速度和姿态等，为状态向量的卡尔曼滤波称为误差状态滤波或间接卡尔曼滤波直接法间接法以各种导航参数X为主要状态滤波器估值的主要部分即是导航参数的估值以某种导航系统输出导航参数的误差为主要状态滤波器估值的主要部分即是导航参数误差的估值目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法模型可能是线性的，也可能是非线性的模型一般都是线性的25252.开环卡尔曼滤波惯性系统卡尔曼滤波器其他导航系统+-ˆXIˆXINXXD-DNXIX用导航参数误差的估值去校正系统输出的导航参数，得到综合导航系统的导航参数估值ˆXˆX目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法开环（输出校正）的卡尔曼滤波器26263.闭环卡尔曼滤波惯性系统卡尔曼滤波器其他导航系统+-IXINXXD-DNXIXIˆXIˆX采用反馈校正的间接法估计，是将惯导系统导航参数误差的估值反馈到惯导系统内，对误差状态进行校正。IXIˆX目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法闭环（反馈校正）的卡尔曼滤波器2727开环滤波仅校正系统输出量，闭环滤波则是校正系统内部的状态。两种校正方法的性质是一样的，具有同样的精度。闭环滤波的反馈校正使得卡尔曼滤波状态值为小量；开环因无反馈，状态值会随时间不断变大。状态方程都是经过一阶近似的线性方程，状态的数值越小，则近似的准确性越高，因此，利用状态反馈校正的系统状态方程，更能接近真实地反映系统误差状态的动态过程。卡尔曼滤波算法中，反馈状态估计的最佳时机是在测量更新后立即进行。卡尔曼滤波的闭环和开环可以混合使用，即一些状态估计作为校正值被反馈，而另外一些不反馈。目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法4.开环与闭环卡尔曼滤波对比28285.混合卡尔曼滤波示例在松组合算法中，21维向量，其中：位置、速度和姿态只做开环修正；而IMU误差，如陀螺和加速度计零偏，比例因子误差进行反馈，修正IMU的原始观测值。目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法GINS软件松组合算法架构示意图（混合滤波）2.3滤波特性及滤波实现中的问题291.滤波收敛特性初始不确定度平衡不确定度时间状态不确定度收敛过程中的卡尔曼滤波状态不确定度*注：状态不确定度是误差协方差矩阵P对角元素的平方根当卡尔曼滤波状态不确定度接近平衡点，每次测量更新后状态不确定度的降低量与系统噪声造成的不确定度的增加量是匹配的；在平衡点，不确定度所反映出的估计置信度水平基本固定.目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法30状态估计的收敛速度基本上取决于该状态的可观测性。如果观测矩阵随时间变化或者状态之间通过状态转移矩阵存在时间依存关系，那么随着迭代次数的增加，更多的状态量变得可观测。例：导航中用位置的变化率来确定速度许多参数的可观测性依赖于系统的动态性例：姿态不变时，INS姿态误差和加速度计偏差不是独立可观测的；陀螺仪误差则需要载体有更高的动态性，方可观测如果两个状态对观测量有同样的影响，以相同方式随时间变化，并且具有相同的动态特性，则它们非独立可观量，在滤波设计时，应将其组合在一起，以免浪费计算资源目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法312.滤波参数调整状态估计误差P/R太小状态估计误差状态估计误差P/R适中P/R太大时间时间时间目录概述标准KF扩展KFSchmidtKF自适应KF平滑算法不同P/