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