第五讲：卡尔曼滤波.ppt

卡尔曼滤波算法及应用 目 录 一. 概述 二. 标准卡尔曼滤波 卡尔曼滤波方程 闭环卡尔曼滤波 卡尔曼滤波特性及实现中的问题 三. 扩展卡尔曼滤波 非线性系统 线性化卡尔曼滤波 扩展卡尔曼滤波 四. Schmidt 卡尔曼滤波 五. 自适应卡尔曼滤波 六. 平滑算法 2 一、概述 2019-3-4 3 1.1 Rudolf Emil Kalman 4 Born 1930 in Hungary BS and MS from MIT PhD 1957 from Columbia Filter developed in 1960-61 目 录 概述 标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法 Kalman R E. A new approach to linear filtering and prediction problems [J]. Journal of Fluids Engineering, 1960, 82(1): 35-45. （引用：18083） 5 Kalman滤波是一种最优估计算法，而非滤波器 能够实时估计系统中的参数（如连续变化的位置、速度等信息）。 估计量通过一系列受噪声污染的观测量来更新， 观测量必须是待估参数的函数，但是在给定的时刻，不要求观测量能够唯一确定当时的参数值。 1.2 概述 目 录 概述 标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法 6 Kalman滤波是一种递推线性最小方差估计 在提供的初始估计基础上，卡尔曼滤波通过递归运算，用先验值和必威体育精装版观测数据的加权平均来更新状态估计（老息+新息）。 非递归算法（如标准最小二乘）中没有先验估计，估计结果由全部观测数据计算而来（新息） 。 卡尔曼滤波是一种贝叶斯估计 目 录 概述 标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法 7 1.3 卡尔曼滤波的要素和流程 （实线表示数据流一直有，虚线表示只在某些应用中有，Ref：Paul Groves） 目 录 概述 标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法 8 状态向量（状态） 是一组描述系统的参数。 可以是常量，也可是时变量，是估计对象。 与之相关联的是误差协方差矩阵，描述了状态估计的不确定度及估计误差间的相关度。 目 录 概述 标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法 9 1.4 卡尔曼滤波的要素 4个要素：2个模型、1组观测量、1个算法 2个模型 系统模型 也称过程模型或者时间传递模型，描述了状态与误差协方差矩阵随时间的变化特性。 对于选定状态量，系统模型是确定的。 观测模型 描述了观测向量与状态向量间的函数关系。 目 录 概述 标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法 10 1组观测向量 是一组针对同一时刻的系统特性的测量值，例如观测量可以包括GNSS系统的位置测量值，或者INS与GNSS位置结果的差值。 1个算法： 卡尔曼滤波算法 使用观测向量、观测模型和系统模型来获得状态向量的最优估计，分为系统传递和测量更新两个部分。 目 录 概述 标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法 目 录 11 1.5 卡尔曼滤波的导航应用 惯性导航系统（INS）的精对准和标定 单一导航（GNSS, 无线电、水声学、匹配） 组合导航 INS/GNSS组合导航及多传感器组合导航 INS/水声组合导航 INS/匹配导航 … 概述 经典KF EKF LKF 二、Kalman滤波 2019-3-4 12 13 2.1 卡尔曼滤波方程 1. 离散系统的数学描述 设离散化后的系统状态方程和量测方程分别为： Xk为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 扩展 KF Schmidt KF 自适应 KF 平滑算法 14 要求{Wk}和{Vk}是互不相关的、零均值白噪声序列： Qk和Rk分别称为系统噪声和量测噪声的方差矩阵，分别是已知值的非负定阵和正定阵； δk j 是Kronecker δ函数，即： 目 录 概述 标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法 15 初始状态的一、二阶统计特性为： Var{·} 为对{·}求方差的符号 卡尔曼滤波要求mx0和Cx0为已知量， 且要求X0与{W