第五讲：卡尔曼滤波

7
目
录

状态向量（状态）
是一组描述系统的参数。
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
可以是常量，也可是时变量，是估计对象。
与之相关联的是误差协方差矩阵，描述了状态估计的不确定度 及估计误差间的相关度。
8
1.4 卡尔曼滤波的要素
目 录
4个要素：2个模型、1组观测量、1个算法
目
录
1.1 Rudolf Emil Kalman

Born 1930 in Hungary BS and MS from MIT PhD 1957 from Columbia Filter developed in 1960-61
• 概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
E Vk VjT R k kj
Qk和Rk分别称为系统噪声和量测噪声的方差矩阵，分别 是已知值的非负定阵和正定阵； δk j 是Kronecker δ函数，即：
0 (k j) kj 1 (k j)
1

初始状态的一、二阶统计特性为：
目
录
E X0 m x 0
5
目
录

Kalman滤波是一种递推线性最小方差估计
在提供的初始估计基础上，卡尔曼滤波通过递归运算，用先验值和
最新观测数据的加权平均来更新状态估计（老息+新息）。
•
概述
非递归算法（如标准最小二乘）中没有先验估计，估计结果由全部 观测数据计算而来（新息） 。 最小方差估计 线性最小方差估计 递推线性最小 方差估计
T k 1 k 1

k,k1
Pk 1
T P k /k1 k,k1Pk 1 k,k 1 k 1Q k
T 1 k 1

概述
标准 KF 扩展 KF
ˆ ˆ X k /k 1 k,k 1X k 1
P k/k1
Zk
Rk Hk
T K k P k/k1 H T R k )1 k (H k Pk/k 1H k
1
目
录
ˆ ˆ X k / k 1 k ,k 1X k 1
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
时间更新 方程 量测修正 方程
ˆ X ˆ ˆ X K k (Z k HkX k k / k 1 k/k 1)
T K k P k / k 1 H T R k )1 k (H k Pk / k 1H k
概述
标准 KF 扩展 KF Schmidt KF
状态估值计算方程 滤波增益方程 一步预测均方差方程
自适应 KF 估计均方差方程 平滑算法
P k (I K k H k )Pk / k 1 (I K k H k )T KkR kKT k
或 P k (I K k H k )Pk / k 1
8
10
卡尔曼滤波速度估计
2
2.2 闭环卡尔曼滤波
目 录
1. 全状态滤波和误差状态滤波
根据卡尔曼滤波状态向量的选取不同，卡尔曼滤波可分为: 全状态卡尔曼滤波（Total State Implementation）和
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
误差状态卡尔曼滤波（Error State Implementation）
1
3. 卡尔曼滤波示例
目 录 有一个质点，沿X轴正方向运动，质点从X = 0 开始匀速直线运动, 速度 为V = 10m/s，则每一时刻质点的真实位臵（参考真值）为：
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
X = X0+V * t；
实际上，我们每隔 0.1s 可以测量一次质点的位臵，但位臵测量值存在 误差（假设是均值为0的白噪声序列） 根据我们对质点的位臵观测量，用卡尔曼滤波方法计算每一时刻质点 的位臵和速度

系统噪声协方差阵Q： 即系统模型的不确定度，由于假设模型即质
点运动模型，因此可认为模型的不确定度为0，即 Q = [0 0; 0 0] 观测矩阵 H：由于只观测了质点位臵，未观测速度，因此观测矩阵 H = [1 0]; 观测噪声矩阵R：位臵观测量的方差为 m2，即 R = 1 观测量向量Z：在真实状态（真实位臵）加上均值为零，方差为 m2 的白噪声;
位置观测值
Xi-1
Xi
Xi+1
X
v = 10 m/s
1
目
录
设计卡尔曼滤波

状态量 x = [X, V]，即以质点的位臵和速度作为卡尔曼滤波状态量； 系统状态方程为 Xk = Xk-1 + Vk-1 * dt ； 状态转移矩阵 Phi = [1 dt; 0 1]；
概述
标准 KF 扩展 KF

ˆ X ˆ ˆ X K k (Z k HkX k k / k 1 k/k 1)
Kk
kk 1
Schmidt KF 自适应 KF 平滑算法
Rk Hk
P k (I K k H k )Pk /k1 (I K k H k )T KkR kKT k
ˆ X k
Pk
滤波计算回路 增益计算回路
25
开环（输出校正）的卡尔曼滤波器
2
目
录
3. 闭环卡尔曼滤波
采用反馈校正的间接法估计，是将惯导系统导航参数误差 X I
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
ˆ 反馈到惯导系统内，对误差状态进行校正。 的估值 X I
ˆ X I
惯性系统
XI
XI
D XI - D X N
0 0 1 2 3 4 5 时间 (s) 6 7 8 9 10
卡尔曼滤波位臵估计
2
目
录
50 40 30 参考真值 位置观测量微分 滑动平均法 卡尔曼滤波
概述
20
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
速度 (m/s)
10 0 -10 -20 -30
0
2
4 时间 (s)
6
概述
GNSS系统的位臵测量值，或者INS与GNSS位臵结果的差值。
标准 KF
扩展 KF Schmidt KF 自适应 KF 平滑算法
1个算法：
卡尔曼滤波算法
使用观测向量、观测模型和系统模型来获得状态向量的最优估计，
分为系统传递和测量更新两个部分。
1
目
录
1.5 卡尔曼滤波的导航应用

• 概述 • 经典KF • EKF • LKF
卡尔曼滤波算法及应用
目 录
一. 概述
二. 标准卡尔曼滤波

卡尔曼滤波方程 闭环卡尔曼滤波 卡尔曼滤波特性及实现中的问题 非线性系统 线性化卡尔曼滤波
三. 扩展卡尔曼滤波


扩展卡尔曼滤波
四. Schmidt 卡尔曼滤波 五. 自适应卡尔曼滤波 六. 平滑算法
2
3
一、概述
2017/4/5
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
向量的卡尔曼滤波称为误差状态滤波或间接卡尔曼滤波
直接法
间接法
滤波器估值的 主要部分即是 模型一般都是线性的 导航参数误差 以某种导航系 的估值 统输出导航参 数的误差为主 要状态
以各种导 航参数X为 滤波器估值的主 模型可能是线性的， 主要状态 要部分即是导航
目 录

Kalman滤波是一种最优估计算法，而非滤波器
能够实时估计系统中的参数（如连续变化的位臵、速度等信息）。
估计量通过一系列受噪声污染的观测量来更新，
•
概述
观测量必须是待估参数的函数，但是在给定的时刻，不要求观测量 能够唯一确定当时的参数值。
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法

概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
2个模型

系统模型
也称过程模型或者时间传递模型，描述了状态与误差协方差矩阵随
时间的变化特性。
对于选定状态量，系统模型是确定的。

观测模型
描述了观测向量与状态向量间的函数关系。
9
目
录

1组观测向量
是一组针对同一时刻的系统特性的测量值，例如观测量可以包括

Schmidt KF 自适应 KF 平滑算法


卡尔曼滤波初始状态：X0 = 0, V0 = 5 m/s，初始状态误差协方差矩
阵P = [1 0; 0 1]
2
目
录
120 参考真值 100 测量值 卡尔曼滤波估值
概述
标准 KF 扩展 KF
位置 (m)
80
60
40
Schmidt KF
20
自适应 KF 平滑算法
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法

Var X0 C x 0

Var{·} 为对{·}求方差的符号
卡尔曼滤波要求mx0和Cx0为已知量，
且要求X0与{Wk}和{Vk}都不相关
1
目
录
2. 离散卡尔曼滤波方程
状态一步预测方程
ˆ ˆ X k/k- 1 = k,k- 1Xk- 1
ˆ X ˆ ˆ X K k (Z k HkX k k / k 1 k/k 1)
T K k P k / k 1 H T R k )1 k (H k Pk / k 1H k
T T Pk/k- 1 = k,k- 1Pk- 1k,k - 1 + Gk- 1Qk- 1Gk - 1

Kalman R E. A new approach to linear filtering and prediction problems [J]. Journal of Fluids Engineering, 1960, 82(1): 35-45. （引用：18083）
4
1.2 概述
T P k / k 1 k ,k 1 Pk 1k ,k 1 k 1Q k
T 1 k 1

P k (I K k H k )Pk / k 1 (I K k H k )T KkR kKT k
1 7
目
录
ˆ X k 1
k,k1
kk 1
k 1Q
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法

卡尔曼滤波是一种贝叶斯估计
6
1.3 卡尔曼滤波的要素和流程
目 录
实际系统
概述
标准 KF 扩展 KF
卡尔曼滤波算法 系统模型 观测模型 观测向量及 其协方差 状态向量及 其协方差
Schmidt KF 自适应 KF 平滑算法 （实线表示数据流一直有，虚线表示只在某些应用中有，Ref：Paul Groves）
也可能是非线性的
参数的估值
2
目
录
2. 开环卡尔曼滤波
ˆ 去校正系统输出的导航参数，得到 用导航参数误差的估值 X
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
ˆ 综合导航系统的导航参数估值 X
惯性系统
XI
ˆ X
D XI - D X N
+
其他导航系统
XN
-
卡尔曼滤波器
ˆ X I
Zk为 kH 时刻的 m Vk 维量测向量 为kn 时刻 m维量测噪声 （被估计量） 转移矩阵（ × （r维） k为k时刻系统量测矩阵 （n n阶） × r阶）
（m×n阶）
1
目
录
要求{Wk}和{Vk}是互不相关的、零均值白噪声序列：
E Wk WjT Q k kj
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
Xk k ,k 1X k 1 k 1 Wk Z k H k X k Vk
1
Xk为k时刻的 k-1到k时刻的系统一步状态 n维状态向量 ΓW 为系统噪声矩阵 为k-1时刻的系统噪声 k-1 k-1
惯性导航系统（INS）的精对准和标定 单一导航（GNSS, 无线电、水声学、匹配）

组合导航
INS/GNSS组合导航及多传感器组合导航 INS/水声组合导航 INS/匹配导航

…
1
wk.baidu.com
12
二、Kalman滤波
2017/4/5
2.1 卡尔曼滤波方程
目 录
1. 离散系统的数学描述

设离散化后的系统状态方程和量测方程分别为：
其他导航系统
+ XN
卡尔曼滤波器
ˆ X I
26
闭环（反馈校正）的卡尔曼滤波器
组合导航系统采用卡尔曼滤波 进行估计的主要对象
导航参数 导航参数
位臵λ，L
速度VX ,VY ,VZ
姿态ψ,θ,γ
导航参数用X表示
2
目
录
Total State：以系统的固有属性，如位臵、速度和姿态等，为状态向
量的卡尔曼滤波，称为全状态滤波或直接卡尔曼滤波
Error State：以系统测量误差值，如INS位臵、速度和姿态等，为状态