第 3 章 高斯滤波器
3.1 卡尔曼滤波: The Kalman Filter (KF)
3.2 扩展卡尔曼滤波: The Extended Kalman Filter (EKF)
3.3 无迹卡尔曼滤波: The Unscented Kalman Filter (UKF)
基于 Talor 展开进行局部线性化，仅是对高斯分布的非线性变换进行线性近似
的一种方式。另一种常用的近似方法是 “无迹 Kalman 滤波 (Unsented Kalman Filter,
UKF)”。无迹 Kalman 滤波通过一种 “Sigma Point” 采样方法，我们称为特征点采样。
这种方法，通过在特征点 {X [i] } 对函数 g(ut , µt−1 ) 进行采样，利用采样的结果进行
加权计算，获得对变换后分布的均值和方差的估计值，用于近似原高斯分布。
3.3.1 基于特征点采样的近似方法
UKF 采用基于特征点采样的方法。具体的，对具有均值为 µ，方差为 Σ 的 n-维
高斯分布，可确定 2n + 1 个特征点，这些特征点表示为：
χ [0] = µ
χ [i ] = µ +
χ [i ] = µ −

(√
(√
(n + λ) ∑
(n + λ) ∑
)
)i
i
for i = 1, ..., n
(3.1)
for i = n + 1, ..., 2n
Note: 注意，式 (3.1) 中：
� · 其中每个采样点是一个 n 维向量；下标 i 代表矩阵
√
(n + λ) ∑ 的第 i 列向量。
� · λ = α2 (n + κ ) − n 是一个微调参数，其中 α 和 κ 是缩放因子。 λ 决定每个特
征点到均值的距离。
� · 对矩阵平方根
√
(n + λ) ∑ 的计算，一般采用 Cholesky 分解 [] 的方法，这样可
以使得计算更有效和稳定。
设待近似的函数为 g(·)，UKF 的基本思想就是在特征点上对要近似的函数进行
采样，然后，基于采样的结果，基于两个权值进行加权计算，计算近似均值和近似的
方差。
第 3 章 高斯滤波器
–4/18–
Y [i ] = g ( χ [i ] )
(3.2)
µ ′ = ∑ ω m Y [i ]
i =0
(
)(
)T
2n
[i ]
′
[i ] − µ ′
[i ] − µ ′
=
ω
Y
Y
∑
∑ c
(3.3)
2n
[i ]
i =0
其中，
λ
1
[i ]
, ωm =
, for i = 1, ..., 2n
n+λ
2( n + λ )
λ
1
[i ]
+ (1 − α2 + β ), ω c =
, for i = 1, ..., 2n
=
n+λ
2( n + λ )
[0]
ωm =
[0]
ωc
(3.4)
(3.5)
参数 β 可以用于编码其它附加知识，例如对分布的更高阶信息的知识。在待近似
分布为准确的高斯分布时， β = 2 为最优值。
Lineariza*on via Unscented Transform
3.3.2 UKF 近似方法示意
EKF UKF 图 3.1: UKF 基于特征点采样的近似方法示例。UKF 首先提取 2n + 1 个特征点，代入
g(x) 进行计算，获得采样值。然后基于加权计算，近似计算高斯分布的均值和方差。
原图摘自 [].
图 (3.1) 给出了 UKF 方法对高斯分布进行近似的示意图。图中，UKF 在 3 个特征
点上的采样，获得 3 个采样值后，对采样值进行加权求和计算，获得近似的高斯分布
的均值和方差。
3.3 无迹卡尔曼滤波: e Unscented Kalman Filter (UKF)
–5/18–
图 (3.1) 中也比较了 EKF 和 UKF 的近似的特点。左边两幅图中，灰色曲线为实际
分布 p(y); 蓝色曲线代表对 p(y) 通过密集采样计算均值和方差的方法，获得的高斯
近似。左 1 图中红色曲线是 EKF 的近似结果；左二图中红色曲线是 UKF 的近似结果；
我们可见通过在多个特征点上进行采样，UKF 相比 EKF 具有更好的近似准确性。
3.3.3 UKF 算法
Algorithm 1 Algorithm Unscented Kalman Filter(µt−1 , Σt−1 , ut , zt )
)
(
√
√
1. χt−1 = µt−1 , µt−1 + (n + λ)∑t−1 , µt−1 − (n + λ)∑t−1
2. χ̄∗t = g(ut , χt−1 )
2n
[i ] ∗[i ]
3. µ̄t = ∑ ωm χ̄t
i =0
(
)(
)T
2n
¯ t = ∑ ωc[i] χ̄∗ [i] − µ̄t χ̄∗ [i] − µ̄t + Rt
4. ∑
t
t
i =0
(
5. χ̄t = µ̄t , µ̄t +
√
¯ t , µ̄t −
(n + λ)∑
√
¯t
(n + λ)∑
)
6. Z̄t = h (χ̄t )
2n
[i ]
[i ]
7. ẑt = ∑ ωm Z̄t
i =0
2n
8. St = ∑
i =0
[i ]
ωc
(
[i ]
Z̄t
− ẑt
)(
[i ]
Z̄t
− ẑt
)T
+ Qt
(
)(
)T
2n
¯ tx,z = ∑ ωc[i] χ̄[i] − µ̄t Z̄ [i] − ẑt
9. ∑
t
t
i =0
¯ tx,z S−1
10. Kt = ∑
t
11. µt = µ̄t + Kt (zt − ẑt )
¯ t − K t St K T
12. ∑t = ∑
t
13. return µt , Σt
算法 (1) 给出了 UKF 的算法流程。UKF 的输入和输出同 EKF 相同。第 1 行 χt−1
为基于 t − 1 时刻的状态确定的特征点；第 2 行，将特征点代入非线性函数；第 3，4
行计算预测的 t 时刻的分布均值和方差；第 5 行基于预测分布确定特征点；第 6 行将
特征点代入观测函数进行采样；第 7，8 行计算预期观测的均值和方差，其中 Qt 是观
第 3 章 高斯滤波器
–6/18–
测噪声的方差矩阵, 注意 St 对应于 EKF 中的 Ht Σ̄t HtT + Qt ; 第 9 行计算状态和观测之
间的协方差矩阵，对应于 EKF 算法中的 Σ̄t HtT ; 第 10 行计算 Kalman 增益矩阵; 第 11，
12 行计算更新之后的状态均值和方差矩阵。
3.3.4 UKF 算法特点以及与 EKF 的比较
在计算速度方面，UKF 与 EKF 具有相似的计算复杂度，EKF 相比 UKF 仅有常数
级的速度优势，在实际中，UKF 也是非常高效的。
在准确性方面，对于纯线性系统 UKF 的近似，可达到同 KF 一致。对于非线性系
统，UKF 可产生比 EKF 更好的结果，相比 EKF 的改善效果取决于状态概率分布的特
点和模型的非线性程度。
图 (3.1), (3.2) 给出了在不同的状态分布，系统模型下 UKF 和 EKF 的直观比较结
果。可见在不同方差的情况下，UKF 对 p(y) 的高斯分布的近似更准确。但在很多实
际系统中，EKF 和 UKF 之间的差异难以察觉，其主要原因是, p(y) 的高斯分布仅是对
UKF Sigma-­‐Point Es2mate (2) p(y) 原分布的近似，虽然 UKF 通过采样方法与 p(y) 的高斯近似更接近，但并不代表
对原 p(y) 分布近似的更好。
EKF UKF 图 3.2: 相比 EKF，UKF 对原分布具有更好的近似准确性，从图中实线对虚线的近似
效果可见。原图摘自 [].
UKF 的采样方法从思路上与后面要讲的粒子滤波方法略有类似，两者都是通过
采样的方法来对非线性变换的结果进行近似，但是关键的不同点是：UKF 的采样点
是固定的，粒子滤波的采样点是随机生成的。这是关键的不同，当待近似的概率分布
是接近高斯分布时，UKF 相比粒子滤波更加有效，但当待近似的概率分布严重的非
3.4 信息滤波器: Information Filter (IF)
–7/18–
高斯时，UKF 的固定点采样的近似效果，相比与粒子滤波就可能会很差。
3.4 信息滤波器: Information Filter (IF)
信息滤波器是 Kalman 滤波器的对偶滤波器。
• 相同点：同 KF, EKF, UKF 相类似，IF 也假设状态的置信度分布符合高斯分布。
• 不同点：IF 同 KF 类方法的主要不同是对高斯置信度分布的表达方法不同。在
KF 类的滤波算法中，高斯分布由其一阶矩 (Mean) 和二阶矩 (Variance) 描述。在
IF 中，高斯分布由其典型参数 (canonical parameterization) 描述，典型参数由信
息矩阵 (information matrix)和信息向量 (information vector)组成。不同的描述方
法，使得算法的更新公式不同，特别是在一种表达方式下复杂的计算过程，对
于在另一种表达方式下可能简单，反之亦然。
典型参数描述和矩描述，常被认为是对偶的描述方法，IF 和 KF 也因此被看作是
互为对偶的滤波器。
3.4.1 高斯分布的典型参数描述
在典型参数描述中，一个多元高斯分布由信息矩阵Ω 和信息向量ξ 描述。信息矩
阵 Ω 是协方差矩阵 Σ 的逆矩阵；信息向量 ξ 是协方差矩阵的逆与均值向量的乘积。
Ω = Σ −1
ξ = Σ −1 µ
(3.6)
因为 Σ 和 µ 可由 Σ = Ω−1 , µ = Ω−1 ξ 直接计算，所以高斯分布由 Ω 和 ξ 完全确
定。
回顾多元高斯分布的表达式：
}
{
1
1
T −1
p( x ) = √
exp − ( x − µ) Σ ( x − µ)
2
|2πΣ|
典型参数表达方式可以通过将指数部分展开得到：
}
{
p( x ) = √ 1 exp − 12 x T Σ−1 x + x T Σ−1 µ − 12 µ T Σ−1 µ
|2πΣ| {
}
}
{
1
1 T −1
=√
exp − µ Σ µ exp − 12 x T Ωx + x T ξ
2
|2πΣ|
|
{z
}
const
{
}
= η exp − 21 x T Ωx + x T ξ
(3.7)
(3.8)
注意式 (3.8) 中，由于 “const” 部分不依赖于变量 x，所以将其写为归一化系数 η。
第 3 章 高斯滤波器
–8/18–
对式 (3.8) 两侧求导数，可以进一步得到：
1
− log ( p( x )) = const + x T Ωx − x T ξ
2
所以， − log ( p( x )) 是 x 的二次函数，二次项系数为
Ω
2 ，一次项系数为
(3.9)
−ξ，常
数项为 − log η。注意 − log ( p( x )) 的取值范围不再是 0-1，所以常数项部分不能在归

一化为 η。
Note: 对于高斯分布， Ω 是半正定矩阵，所以， − log ( p( x )) 表示的是一个二次距离
函数。矩阵 Ω 决定着距离值随 x 的不同维度变化的变化速率。由半正定矩阵 Ω 加权
的距离函数，称之为马氏距离 (Mahalanobis Distance)。
3.4.2 信息滤波算法
对于满足如下线性状态方程和线性观测的系统：
xt = At xt−1 + Bt ut + ε t
zt = Ht xt + δt
(3.10)
设 Rt 和 Qt 分别为系统噪声向量 ε t 和测量噪声向量 δt 的方差矩阵，则信息滤波算法
如算法 (2) 所示：
Algorithm 2 Algorithm Information Filter(ξ t−1 , Ωt−1 , ut , zt )
1. Ω̄t =
(
1
T
At Ω−
t −1 A t
+ Rt
) −1
(
)
1
2. ξ̄ t = Ω̄t At Ω−
ξ
+
B
u
t
t
t
−
1
t −1
1
3. Ωt = HtT Q−
t Ht + Ω̄t
1
4. ξ t = HtT Q−
t zt + ξ̄ t
5. return ξ t , Ωt
同 KF 算法相同，IF 算法包括预测和更新两步。算法中，第 1 行根据 Ωt−1 和 Rt
对 Ω̄t 进行预测；第 2 行根据控制 ut ，预测 ξ̄ t ; 第 3 行根据观测对 Ωt 进行更新；第 4

行更新状态 ξ t 。
Note:[基于复杂性分析 IF 和 KF 的对偶性] 对比 KF 和 IF，可以从计算复杂性上分析
和验证两者的对偶性质。在 KF 中，预测步骤的复杂度是 O(n2 )，更新步骤包括矩阵
求逆，复杂度是 O(n2.4 )。在 IF 中，预测步骤包含矩阵求逆过程，复杂度为 O(n2.4 )；
更新部分不包括矩阵求逆，复杂度为 O(n2 )。
从分析可见，IF 同 KF 同样具有较高的计算效率，但二者的计算难点不同。KF
计算的难点在状态更新部分，预测部分计算较为简单，仅为一个线性方程，所以适用
3.4 信息滤波器: Information Filter (IF)
–9/18–
于控制更为频繁的情况。相反的，IF 的计算难点在于预测部分，状态更新较为简单，
所以更适用于观测比较频繁的情况。两者的这一特点也印证了 IF 和 KF 的对偶性。
3.4.3 扩展信息滤波算法:EIF
扩展信息滤波（Extended Information Filter: EIF）将 IF 扩展到非线性情况，扩展
的方式同样采用基于 Talyor 展开的局部线性化方法。对于如 (3.11) 状态函数和观测函
数所描述的动态系统：
x t = g ( u t , x t −1 ) + ε t
zt = h ( xt ) + δt
(3.11)
如 Rt , Qt 分别为过程噪声方差矩阵和观测噪声方差矩阵，如 Gt 和 Ht 分别是
g (ut , µt−1 ,) 和 h (µt ) 的雅克比矩阵，则 EIF 的算法流程如算法3所示。
Algorithm 3 Extended Information Filter(ξ t−1 , Ωt−1 , ut , zt )
1
1. µt−1 = Ω−
t −1 ξ t −1
(
) −1
1
T+R
2. Ω̄t = At Ω−
A
t
t −1 t
3. ξ̄ t = Ω̄t g (ut , µt−1 )
4. µ̄t = g (ut , µt−1 )
1
5. Ωt = HtT Q−
t Ht + Ω̄t
1
6. ξ t = HtT Q−
t [ zt − h ( µ̄ t ) + Ht µ̄ ] + ξ̄ t
7. return ξ t , Ωt
其中，第 2，3 行是信息矩阵和信息向量的预测；第 5，6 行是信息矩阵和信息向
量的更新。同 IF 的不同之处仅在于：1）信息向量的预测是直接将 µt−1 代入到非线
性函数 g(·) 进行计算；2）信息向量的更新中采用 “观测 − 预测的观测 + 线性化后的
预测的观测” 来计算。
算法中在典型参数模型的迭代更新同时，还需要计算 µt−1 和 µ̄t ，这部分将在基
于信息滤波的地图构建过程问题中进一步讨论。
3.4.4 Kalman 滤波器与信息滤波器的区别
在机器人定位导航问题中，IF 类方法相比 KF 类方法具有一些优点。
例如，表示全部信息均不确定，在 IF 中，仅需设 Ω = 0，但在 KF 中，需要将方
差矩阵中的元素设为无穷大。
–10/18–
第 3 章 高斯滤波器
在传感器的测量只能观测到部分状态的信息时，在 EKF 中需要进行特殊处理，
而这种情况在机器人定位导航中非常普遍。信息滤波器相比 Kalman 滤波器在数值计
算上更稳定。
在后续章节中我们将看到，信息滤波类方法可以在不恢复概率分布的情况下，将
新的信息直接融入到信息滤波器中，这在大型状态估计问题中，即当状态个数成千上
万时，尤其重要。Kalman 类的滤波器需要将所有状态更新以融入新的信息，而信息
滤波器可以近通过信息矩阵和信息向量的局部修改引入新的信息。当然这种融入方法
需要设计更高级的分布式信息滤波器，将在后面的章节中介绍。
信息滤波器的另一优势来自于对多机器系统的天然适配。多机器人系统常常涉及
到分布式检测的多传感信息的融合问题，这一融合常通过贝叶斯滤波进行。当采用指
数形势描述贝叶斯滤波时，贝叶斯滤波器的乘法变为加法形式。由典型参数描述的信
息滤波器即是指数形式的贝叶斯滤波，所以信息融合可以通过将多机器人的信息相
加进行计算。因为加法的特点，采用信息滤波其的信息融合可以以任意顺序，任意延
迟进行，并且可以完全分布式实现。虽然在 KF 类滤波器中也可以同样实现信息的融
合，毕竟两者是对偶的，但是计算的成本会更高。
信息滤波器也有一些重要的局限性。一个主要的不足是在 EIF 中，在状态的预测
和更新过程中需要计算 µt−1 和 µ̄t 。其中 µt−1 的计算需要计算信息矩阵的逆矩阵。而
在 EKF 计算中，无需这种附加的逆矩阵计算。所以，在状态空间维度较高时，EIF 比
EKF 具有更高的计算成本，这也是 EKF 相比 EIF 被更广泛使用的一个重要原因。
在后续章节中我们可以看到，这一局限性在信息矩阵存在特定结构时也可以避
免。在很多机器人定位导航的实际问题中，状态变量的相互影响是局部的，因而信息
矩阵是稀疏的。在信息矩阵是稀疏的情况下，协方差矩阵并不是稀疏的。
由于其稀疏性，信息矩阵可以看做图 (Graph)，在信息矩阵的非对角线元素不为
零的状态向量之间存在边。稀疏的信息矩阵对应着稀疏图，常可通过 Markov 随机场
(Markov Random Field) 进行描述。针对这类问题存在一系列的算法进行状态的估计和
更新，比如 loopy belief propagation 等。在后续章节中，我们将在地图构建问题中，介
绍一个稀疏的信息矩阵。基于稀疏矩阵的 EIF 算法相比 EKF 和 IF 都将更为高效。
3.5 参考文献