• 卡尔曼滤波(Kalman Filtering)是1960年由R.E.Kalman首
次提出的一种估计方法。之所以称为滤波，是因为它是一 种排除随机干扰，提高检测精度的一种手段。
• KF是基于最小方差准则推导出来的一种线性滤波器。 • KF是一种时域递推算法，根据上一状态的估计值和当前
状态的观测值推出当前状态，不需存储大量的历史数据， 便于计算机实现。
xˆk xˆk K( yk yˆk )
Px, k Px, k KPy, k K T
27
Xˆ k|k Xˆ k k1 Kk Z~k k1
测量更新 /修正
方差估值 Pk k [I Kk Hk ]Pk k1
7
3.5 卡尔曼滤波的结构图
上述递推公式，称为卡尔曼滤波器。实际上，卡尔曼 滤波器也是一个系统，其结构框图如下：
Zk + -
+
Kk
+
Z k|k 1
当前估计值
Xˆ k
14
3.7 联邦卡尔曼滤波
• 卡尔曼滤波最成功的工程应用是设计运载体的高精度组合
导航系统。为了与联邦滤波方法相区别，将普通的卡尔曼
滤波称为集中卡尔曼滤波。
• 由于对导航精度要求的提高，导航设备越来越多。另一方
面，现代系统向大系统和复杂系统的方向发展。这种情况
下采用集中式卡尔曼实现组合导航，存在两个问题：
yˆ
k

W (m) i
i
k|k 1
i0
2n
Py, k
Wi
(c)
[
i k|k
1

yˆ k
][
i k|k 1

yˆk ]T
Rk
i0
2n
Pxy, k
Wi(c)
[
i k|k
1

xˆk
][
i k|k 1

yˆ
k
]T
i0
K Pxy, k Py,1k
X (t) — — n 维 状 态 向 量 ； Z (t) — — m 维 观 测 向 量 ； W (t) — — p 维 系 统 随 机 干 扰 ； V (t) — — m 维 随 机 测 量 噪 声 ； f (t), h(t) — — n 维 和 m 维 向 量 函 数 。
3
系统的状态空间描述(续)
3 卡尔曼滤波方法
3.1 卡尔曼滤波的特点及应用领域 3.2 系统的状态空间描述 3.3 卡尔曼滤波的直观推导 3.4 卡尔曼滤波的递推运算方程 3.5 卡尔曼滤波的结构图 3.6 卡尔曼滤波的应用实例 3.7 联邦卡尔曼滤波 3.8 联邦卡尔曼滤波的应用实例 3.9 Unscented卡尔曼滤波
3.1 卡尔曼滤波的特点及应用领域
主滤波器的信息分配系数，满足守恒原则
N
i
m 1
i 1
19
联邦卡尔曼滤波器结构
信息分配
卡尔曼滤波
信息融合
联邦卡尔曼滤波器
20
联邦卡尔曼滤波器结构图
注：组合导航中，一般选择可靠性好的系统作为参考 系统，如惯性导航系统。
21
联邦卡尔曼滤波器的应用
仍然以舰船导航为例，模型与前面相同。为了与集中卡 尔曼滤波器的效果相比较，同时采用两种滤波器滤波。
时间/s
海流北向速度分量估计误差/(m/s)
0.6 联邦卡尔曼滤波算法 集中卡尔曼滤波算法
0.4
0.2
0
-0.2
-0.4
-0.6
-0.8 100 200 300 400 500 600 700 800 900 1000
时间/s
图中可以看出，联邦卡尔曼滤波与集中卡尔曼滤波算法 的估计精度相当，但联邦卡尔曼滤波相对于集中卡尔曼滤 波，不仅有更好的容错性，而且可以并行运算。
得预测测量估计偏差： Z~k|k1 Zk Zˆk|k1 Zk Hk Xˆ k|k1
利用此偏差修正预测估计：
Xˆ k|k Xˆ k k1 Kk [Zk Hk Xˆ k k1]
待定校正增益阵
5
卡尔曼滤波的直观推导(续)
增益阵的求法： 使目标函数E[X~k|k X~T k|k ] 极小化 定义：
• 各子滤波器并行运行，获得建立在局部量测基础上的局部
最优估计。
• 各局部最优估计在第二级滤波器（即主滤波器）内按融合
算法合成，获得建立在所有量测基础上的全局估计。
• 全局估计再按信息守恒原则反馈给各子滤波器。如此反复
递推。
实际设计的联邦滤波器是全局次优的，但是对于自主性要求很高的重 要运载体来说，导航系统的可靠性比精度更重要。采用联邦滤波器设 计组合导航系统，虽然损失了少许精度，但得到的是组合导航系统的 强容错能力。

VVEN
bVE bVN
3 4
S 5
i (i 1,2,,7)为独立的零均值高
K 6 7
斯白噪声, b为海流相关时间常数。
• 观测方程
z1 v1

z2





v2

i k|k 1

f
k
(

i k
1
)
2n
xˆk
W (m) i
i
k|k 1
i0
2n
P x,k

W [ (c) i
i
k|k 1

xˆ k
][

i k|k
1

xˆ
k
]T

Qk
26
i0
UKF的具体应用过程(续)
• 测量更新
2n

i k|k 1

h(ki |k 1 )，
构 造 一 组 采 样 点i
x (
(n )Px )i ,
i x ( (n )Px )i ,

x,
i 1, , n i n 1, , 2n i0
0 为可调尺度参数，调整它可以提高逼近精度。
2. 利用Sigma点进行非线性传播
将上面构造的Sigma点直接按照(3-1)的关系作非线性 变换，则产生一组变换样本点
延时 一步
Hk
k ,k 1
一步预测
上一步估计值
Xˆ k|k 1
Xˆ k1
8
3.6 卡尔曼滤波的应用实例(舰船导航)
卡尔曼滤波最成功的工程应用是设计运载体的高精度组合导航 系统。下面以舰船导航问题为例，介绍其具体应用。
1、系统模型
• 状态变量
X (k) VN VE S K T
方差预测
Pk
k 1

P T k,k 1 k 1 k 1 k,k 1

Q T k,k 1 k 1 k ,k 1
时间更新 /预测
增益矩阵 新息序列 状态估值
Kk

Pk
HT
k 1 k
[Hk
Pk
HT
k 1 k
Rk ]1
Z~k k1 [Zk Hk Xˆ k k1]
– 计算负担重。滤波器计算量以状态维数的三次方剧增，无法满足导 航的实时性要求；
– 容错性能差，不利于故障诊断。当任一导航子系统发生故障，且没
有及时检测出并隔离时，则整个导航系统都会被污染，使输出的信
息不可靠。
• 为解决上述问题，出现了分散化滤波的思想和方法。其中，
Carlson在1988年提出的联邦滤波由于设计灵活、计算量小、
4
3.3 卡尔曼滤波的直观推导
假 设 在k 时 刻 已 获 得k 次 测 量 结 果{Z1, Z2,, Zk1, Zk },
且
已
得
到X
k
1
的
最
优
线
性
估
计Xˆ
k
1|k
，则
1
设
想
：
Xˆ k|k 1 Xˆ k , k 1 k 1|k 1
Zˆk|k 1 H k Xˆ k|k 1
X~k|k1 X k Xˆ k k1 X~k|k X k Xˆ k k
使 Pk|k E[ X~k|k X~kT|k ] 极小，求得增益阵，进而求得卡尔曼 滤 波 器 的 递 推 运 算 方 程：
6
3.4 卡尔曼滤波的递推运算方程
初始条件 状态预测
Xˆ 0 0 Xˆ 0 P0 0 P0 Xˆ k k1 k,k1Xˆ k1 k1
对于如下的非线性变换：
y f (x)
----------------- (3-1)
假设已知n维随机向量 x 的均值 x 和协方差 Px ，则对上述 非线性系统进行滤波的目的是计算 y 的均值 y 和 方差 Py 。
24
1. 构造 Sigma 点
根据随机向量x 的均值x 和协方差Px ，在 x 的附近

z3 z4


S K

v3 v4

量测系统由GPS和航位推算系统(DR) 组成，GPS输出舰船的经纬度φ和λ。
DR由罗经和计程仪组成，分别输出 航向K和船相对于水流的速度S。
10
2、初始条件和参数选取
11
3、仿真结果(a)
12
3、仿真结果(b)
13
3、仿真结果(c)
23
3.9 Unscented卡尔曼滤波
UKF方法的建立是基于如下的事实：逼近任意的分布比 逼近它的任何非线性函数更容易。UKF的基础是Unscented 变换(Unscented Transformation, UT)，其基本思想是用一组 确定的离散采样点(称为Sigma点)来近似状态变量的分布。 UKF假定状态满足高斯分布，因此只需逼近其均值和方差。