第五讲：卡尔曼滤波

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
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
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
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
卡尔曼滤波算法及应用
目 录
一. 概述
二. 标准卡尔曼滤波

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


扩展卡尔曼滤波
四. Schmidt 卡尔曼滤波 五. 自适应卡尔曼滤波 六. 平滑算法
2
3
一、概述
2017/4/5
概述
GNSS系统的位臵测量值，或者INS与GNSS位臵结果的差值。
标准 KF
扩展 KF Schmidt KF 自适应 KF 平滑算法
1个算法：
卡尔曼滤波算法
使用观测向量、观测模型和系统模型来获得状态向量的最优估计，
分为系统传递和测量更新两个部分。
1
目
录
1.5 卡尔曼滤波的导航应用

• 概述 • 经典KF • EKF • LKF
目 录

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

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

Var X0 C x 0

Var{·} 为对{·}求方差的符号
卡尔曼滤波要求mx0和Cx0为已知量，
且要求X0与{Wk}和{Vk}都不相关
1
目
录
2. 离散卡尔曼滤波方程
状态一步预测方程
ˆ ˆ X k/k- 1 = k,状态向量（状态）
是一组描述系统的参数。
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
可以是常量，也可是时变量，是估计对象。
与之相关联的是误差协方差矩阵，描述了状态估计的不确定度 及估计误差间的相关度。
8
1.4 卡尔曼滤波的要素
目 录
4个要素：2个模型、1组观测量、1个算法
ˆ 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
滤波计算回路 增益计算回路
位置观测值
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

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 平滑算法
惯性导航系统（INS）的精对准和标定 单一导航（GNSS, 无线电、水声学、匹配）

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

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

设离散化后的系统状态方程和量测方程分别为：
也可能是非线性的
参数的估值
2
目
录
2. 开环卡尔曼滤波
ˆ 去校正系统输出的导航参数，得到 用导航参数误差的估值 X
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
ˆ 综合导航系统的导航参数估值 X
惯性系统
XI
ˆ X
D XI - D X N
+
其他导航系统
XN
-
卡尔曼滤波器
ˆ X I

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

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

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

1组观测向量
是一组针对同一时刻的系统特性的测量值，例如观测量可以包括
25
开环（输出校正）的卡尔曼滤波器
2
目
录
3. 闭环卡尔曼滤波
采用反馈校正的间接法估计，是将惯导系统导航参数误差 X I
概述
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法
ˆ 反馈到惯导系统内，对误差状态进行校正。 的估值 X I
ˆ X I
惯性系统
XI
XI
D XI - D X N
标准 KF 扩展 KF Schmidt KF 自适应 KF 平滑算法