Kalman Filter
1、卡尔曼滤波器
首先卡尔曼滤波器所要解决的问题—状态估计出发,状态估计过程即SLAM过程可以由运动方程和观测方程来描述,假设从
t
=
0
t=0
t=0时刻到
t
=
n
t=n
t=n时刻,有系统状态
x
0
,
x
1
,
⋯
,
x
n
\mathbf{x}_{0},\mathbf{x}_{1},\cdots,\mathbf{x}_{n}
x0,x1,⋯,xn,路标
y
1
,
⋯
,
y
m
\mathbf{y}_{1},\cdots,\mathbf{y}_{m}
y1,⋯,ym,则上述两方程的基本形式为
{
x
k
=
f
(
x
k
−
1
,
u
k
,
w
k
)
k
=
1
,
.
.
.
,
n
z
k
,
j
=
h
(
y
j
,
x
k
,
v
k
,
j
)
(
k
,
j
)
∈
O
\left\{ \begin{array}{ll} \mathbf{x}_{k}=\mathbf{f}(\mathbf{x}_{k-1},\mathbf{u}_{k},\mathbf{w}_{k})&\quad k=1,...,n\\ \mathbf{z}_{k,j}=\mathbf{h}(\mathbf{y}_{j},\mathbf{x}_{k},\mathbf{v}_{k,j})&\quad (k,j)\in{O} \end{array}\right.
{xk=f(xk−1,uk,wk)zk,j=h(yj,xk,vk,j)k=1,...,n(k,j)∈O
其中
w
k
\mathbf{w}_{k}
wk为运动过程产生的噪声,
u
k
\mathbf{u}_{k}
uk是传感器读数或输入,
O
O
O是一个集合,记录在哪个时刻观察到了哪些路标(通常不是每个路标在所有时刻都可见,在单个时刻通常只能看到其中一小部分),
v
k
,
j
\mathbf{v}_{k,j}
vk,j是本次观测产生的噪声。由于系统状态和路标点都是待估计量,即要解决定位(估计系统状态)和建图(估计路标点)问题,故可将二者统一为一个未知量
x
k
=
d
e
f
[
x
k
T
,
y
1
T
,
⋯
,
y
m
T
]
T
\mathbf{x}_{k}\overset{\mathrm{def}}{=}[\mathbf{x}_{k}^{T},\mathbf{y}_{1}^{T},\cdots,\mathbf{y}_{m}^{T}]^{T}
xk=def[xkT,y1T,⋯,ymT]T
重新整理运动方程和观测方程,函数符号保持不变,可将二者写得更简洁
{
x
k
=
f
(
x
k
−
1
,
u
k
,
w
k
)
z
k
=
h
(
x
k
,
v
k
)
k
=
1
,
.
.
.
,
n
(1)
\left\{ \begin{array}{ll} \mathbf{x}_{k}=\mathbf{f}(\mathbf{x}_{k-1},\mathbf{u}_{k},\mathbf{w}_{k})\\ \mathbf{z}_{k}=\mathbf{h}(\mathbf{x}_{k},\mathbf{v}_{k}) \end{array} \right.\quad\quad k=1,...,n\tag1
{xk=f(xk−1,uk,wk)zk=h(xk,vk)k=1,...,n(1)
从最简单的线性高斯系统开始,即假设为运动方程与观测方程为线性(等价地,可以忽略常数项,和输入前的与其相乘的矩阵,因为这些量均可归入输入,在数学上没有影响),噪声服从高斯分布,并以加法的形式出现(考虑到方程的线性性)(此时同样称观测方程中不带噪声的部分为观测方程)。此时,该方程组具有如下形式
{
x
k
=
A
k
x
k
−
1
+
u
k
+
w
k
z
k
=
C
k
x
k
+
v
k
k
=
1
,
.
.
.
,
n
(2)
\left\{ \begin{array}{ll} \mathbf{x}_{k}=\mathbf{A}_{k}\mathbf{x}_{k-1}+\mathbf{u}_{k}+\mathbf{w}_{k}\\ \mathbf{z}_{k}=\mathbf{C}_{k}\mathbf{x}_{k}+\mathbf{v}_{k} \end{array} \right.\quad\quad k=1,...,n\tag2
{xk=Akxk−1+uk+wkzk=Ckxk+vkk=1,...,n(2)
严格地写出上式所涉及的量的维数,
x
k
∈
R
N
\mathbf{x}_{k}\in\mathbb{R}^{N}
xk∈RN为系统状态,其初始状态为
x
0
∈
R
N
∼
N
(
x
^
0
,
P
^
0
)
\mathbf{x}_{0}\in\mathbb{R}^{N}\sim\mathcal{N}(\hat{\mathbf{x}}_{0},\hat{\mathbf{P}}_{0})
x0∈RN∼N(x^0,P^0),状态递推的过程噪声为
w
k
∈
R
N
∼
N
(
0
,
R
k
)
\mathbf{w}_{k}\in\mathbb{R}^{N}\sim\mathcal{N}(\mathbf{0},\mathbf{R}_{k})
wk∈RN∼N(0,Rk)。输入
u
k
∈
R
N
\mathbf{u}_{k}\in\mathbb{R}^{N}
uk∈RN为确定性变量,有时也将输入写为
B
k
u
k
\mathbf{B}_{k}\mathbf{u}_{k}
Bkuk,因输入是确定性变量,故二者等价。测量量为
z
k
∈
R
M
k
\mathbf{z}_{k}\in\mathbb{R}^{M_{k}}
zk∈RMk,测量噪声为
v
k
∈
R
M
k
∼
N
(
0
,
Q
k
)
\mathbf{v}_{k}\in\mathbb{R}^{M_{k}}\sim\mathcal{N}(\mathbf{0},\mathbf{Q}_{k})
vk∈RMk∼N(0,Qk)。
A
k
∈
R
N
×
N
\mathbf{A}_{k}\in\mathbb{R}^{N\times N}
Ak∈RN×N称为状态转移矩阵,
C
k
∈
R
M
k
×
N
\mathbf{C}_{k}\in\mathbb{R}^{M_{k}\times N}
Ck∈RMk×N称为观测矩阵。
设计滤波器的目的是求后验概率分布 p ( x k ∣ x 0 , u 1 : k , z 1 : k ) p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k}) p(xk∣x0,u1:k,z1:k)。
欲求此分布,可依据贝叶斯法则对其进行如下转换
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
)
=
p
(
x
k
,
x
0
,
u
1
:
k
,
z
1
:
k
)
p
(
x
0
,
u
1
:
k
,
z
1
:
k
)
=
p
(
z
k
∣
x
k
,
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p
(
x
k
,
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p
(
x
0
,
u
1
:
k
,
z
1
:
k
)
=
p
(
z
k
∣
x
k
,
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p
(
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p
(
z
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p
(
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
=
p
(
z
k
∣
x
k
,
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p
(
z
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
\begin{align*} p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k})&=\frac{p(\mathbf{x}_{k},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k})}{p(\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k})}\\ &=\frac{p(\mathbf{z}_{k}|\mathbf{x}_{k},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})p(\mathbf{x}_{k},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})}{p(\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k})}\\ &=\frac{p(\mathbf{z}_{k}|\mathbf{x}_{k},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})p(\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})}{p(\mathbf{z}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})p(\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})}\\ &=\frac{p(\mathbf{z}_{k}|\mathbf{x}_{k},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})}{p(\mathbf{z}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})}\\ \end{align*}
p(xk∣x0,u1:k,z1:k)=p(x0,u1:k,z1:k)p(xk,x0,u1:k,z1:k)=p(x0,u1:k,z1:k)p(zk∣xk,x0,u1:k,z1:k−1)p(xk,x0,u1:k,z1:k−1)=p(zk∣x0,u1:k,z1:k−1)p(x0,u1:k,z1:k−1)p(zk∣xk,x0,u1:k,z1:k−1)p(xk∣x0,u1:k,z1:k−1)p(x0,u1:k,z1:k−1)=p(zk∣x0,u1:k,z1:k−1)p(zk∣xk,x0,u1:k,z1:k−1)p(xk∣x0,u1:k,z1:k−1)
其中
p
(
z
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p(\mathbf{z}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})
p(zk∣x0,u1:k,z1:k−1)的取值不受
x
k
\mathbf{x}_{k}
xk影响,为定值,因为由
x
0
,
u
1
:
k
,
z
1
:
k
−
1
\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1}
x0,u1:k,z1:k−1可以完全确定出
z
k
\mathbf{z}_{k}
zk的分布。故关于
x
k
\mathbf{x}_{k}
xk的函数
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
)
p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k})
p(xk∣x0,u1:k,z1:k)可以写为
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
)
=
η
∗
p
(
z
k
∣
x
k
,
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
(3)
p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k})=\eta\ast p(\mathbf{z}_{k}|\mathbf{x}_{k},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})\tag3
p(xk∣x0,u1:k,z1:k)=η∗p(zk∣xk,x0,u1:k,z1:k−1)p(xk∣x0,u1:k,z1:k−1)(3)
即其形式完全由似然
p
(
z
k
∣
x
k
,
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p(\mathbf{z}_{k}|\mathbf{x}_{k},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})
p(zk∣xk,x0,u1:k,z1:k−1)和先验置信度
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})
p(xk∣x0,u1:k,z1:k−1)决定,求出二者形式即可得到
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
)
p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k})
p(xk∣x0,u1:k,z1:k)。
对于先验,以
x
k
−
1
\mathbf{x}_{k-1}
xk−1为条件做全概率展开
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
=
∫
p
(
x
k
∣
x
k
−
1
,
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p
(
x
k
−
1
∣
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
d
x
k
−
1
p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})=\int p(\mathbf{x}_{k}|\mathbf{x}_{k-1},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})p(\mathbf{x}_{k-1}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})d\mathbf{x}_{k-1}
p(xk∣x0,u1:k,z1:k−1)=∫p(xk∣xk−1,x0,u1:k,z1:k−1)p(xk−1∣x0,u1:k,z1:k−1)dxk−1
实际上,上述的贝叶斯转换并没有假设系统的具体形式。注意到将上面的积分式第二个因子中条件
u
k
\mathbf{u}_{k}
uk和
x
k
−
1
\mathbf{x}_{k-1}
xk−1无关,故在该条件去掉,并积分式代入
(
3
)
(3)
(3),得到求解后验概率分布的公式
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
)
=
η
∗
p
(
z
k
∣
x
k
,
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
∫
p
(
x
k
∣
x
k
−
1
,
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
p
(
x
k
−
1
∣
x
0
,
u
1
:
k
−
1
,
z
1
:
k
−
1
)
d
x
k
−
1
(4)
p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k})=\eta\ast p(\mathbf{z}_{k}|\mathbf{x}_{k},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})\int p(\mathbf{x}_{k}|\mathbf{x}_{k-1},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})p(\mathbf{x}_{k-1}|\mathbf{x}_{0},\mathbf{u}_{1:k-1},\mathbf{z}_{1:k-1})d\mathbf{x}_{k-1}\tag4
p(xk∣x0,u1:k,z1:k)=η∗p(zk∣xk,x0,u1:k,z1:k−1)∫p(xk∣xk−1,x0,u1:k,z1:k−1)p(xk−1∣x0,u1:k−1,z1:k−1)dxk−1(4)
此公式称贝叶斯滤波器,因为该方法可以减少或剔除数据中的噪声部分,从而保持信号的真实特征,这与传统的滤波器去除信号中某一频率范围内噪声的功能类似,故称为“滤波器”,但二者的主要区别在于贝叶斯滤波器是基于概率和统计理论进行的高级滤波操作。卡尔曼滤波器是贝叶斯滤波器在线性高斯系统下的实现,其它不仅可以滤波(处理当前信号),还可以结合系统动态模型和之前的状态估计对未来状态进行预测。
因为
x
0
∼
N
(
x
^
0
,
P
^
0
)
\mathbf{x}_{0}\sim\mathcal{N}(\hat{\mathbf{x}}_{0},\hat{\mathbf{P}}_{0})
x0∼N(x^0,P^0),故不妨假设从
x
k
−
1
\mathbf{x}_{k-1}
xk−1递推得到的状态
x
k
\mathbf{x}_{k}
xk和根据状态
x
k
\mathbf{x}_{k}
xk得到的观测
z
k
\mathbf{z}_{k}
zk仍服从正态分布,故由观测方程得出
p
(
z
k
∣
x
k
,
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
=
p
(
z
k
∣
x
k
)
=
N
(
C
k
x
k
,
Q
k
)
(5)
p(\mathbf{z}_{k}|\mathbf{x}_{k},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})=p(\mathbf{z}_{k}|\mathbf{x}_{k})=\mathcal{N}(\mathbf{C_{k}}\mathbf{x}_{k},\mathbf{Q}_{k})\tag5
p(zk∣xk,x0,u1:k,z1:k−1)=p(zk∣xk)=N(Ckxk,Qk)(5)
由运动方程得出
p
(
x
k
∣
x
k
−
1
,
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
=
p
(
x
k
∣
x
k
−
1
,
u
k
)
=
N
(
A
k
x
k
−
1
+
u
k
,
R
k
)
\begin{align*} p(\mathbf{x}_{k}|\mathbf{x}_{k-1},\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})&=p(\mathbf{x}_{k}|\mathbf{x}_{k-1},\mathbf{u}_{k})=\mathcal{N}(\mathbf{A}_{k}\mathbf{x}_{k-1}+\mathbf{u}_{k},\mathbf{R_{k}})\\ \end{align*}
p(xk∣xk−1,x0,u1:k,z1:k−1)=p(xk∣xk−1,uk)=N(Akxk−1+uk,Rk)
并定义
p
(
x
k
−
1
∣
x
0
,
u
1
:
k
−
1
,
z
1
:
k
−
1
)
=
N
(
x
^
k
−
1
,
P
^
k
−
1
)
p(\mathbf{x}_{k-1}|\mathbf{x}_{0},\mathbf{u}_{1:k-1},\mathbf{z}_{1:k-1})=\mathcal{N}(\hat{\mathbf{x}}_{k-1},\hat{\mathbf{P}}_{k-1})
p(xk−1∣x0,u1:k−1,z1:k−1)=N(x^k−1,P^k−1)
根据§1,可以得到
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
=
N
(
A
x
^
k
−
1
+
u
k
,
A
k
P
^
k
−
1
A
k
T
+
R
k
)
(6)
p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})=\mathcal{N}(\mathbf{A}\hat{\mathbf{x}}_{k-1}+\mathbf{u}_{k},\mathbf{A}_{k}\hat{\mathbf{P}}_{k-1}\mathbf{A}_{k}^{T}+\mathbf{R}_{k})\tag6
p(xk∣x0,u1:k,z1:k−1)=N(Ax^k−1+uk,AkP^k−1AkT+Rk)(6)
上式也可以看成已知
x
k
−
1
\mathbf{x}_{k-1}
xk−1的后验均值
x
^
k
−
1
\hat{\mathbf{x}}_{k-1}
x^k−1和后验协方差
P
^
k
−
1
\hat{\mathbf{P}}_{k-1}
P^k−1,据此对
x
k
\mathbf{x}_{k}
xk进行先验估计,得到其先验均值
x
ˇ
k
\check{\mathbf{x}}_{k}
xˇk和先验协方差
P
ˇ
k
\check{\mathbf{P}}_{k}
Pˇk,即
x
ˇ
k
=
A
x
^
k
−
1
+
u
k
,
P
ˇ
k
=
A
k
P
^
k
−
1
A
k
T
+
R
k
\check{\mathbf{x}}_{k}=\mathbf{A}\hat{\mathbf{x}}_{k-1}+\mathbf{u}_{k},\quad\check{\mathbf{P}}_{k}=\mathbf{A}_{k}\hat{\mathbf{P}}_{k-1}\mathbf{A}_{k}^{T}+\mathbf{R}_{k}
xˇk=Ax^k−1+uk,Pˇk=AkP^k−1AkT+Rk
故可以
(
4
)
(4)
(4)即先验置信度可以写成
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
=
N
(
x
ˇ
k
,
P
ˇ
k
)
(7)
p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k-1})=\mathcal{N}(\check{\mathbf{x}}_{k},\check{\mathbf{P}}_{k})\tag7
p(xk∣x0,u1:k,z1:k−1)=N(xˇk,Pˇk)(7)
该过程也称为预测。通过
(
3
)
(3)
(3)式可以看到,预测阶段完成后,还须要通过观测数据
z
k
\mathbf{z}_{k}
zk完成更新(或称为校正),得到后验置信度。
将
(
5
)
,
(
7
)
(5),(7)
(5),(7)代入
(
3
)
(3)
(3),并将等号右边展开得到
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
)
=
η
′
∗
exp
(
(
z
k
−
C
k
x
k
)
T
Q
k
(
z
k
−
C
k
x
k
)
)
exp
(
(
x
k
−
x
ˇ
k
)
T
P
ˇ
k
(
x
k
−
x
ˇ
k
)
)
p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k})=\eta^{\prime}\ast\exp\left((\mathbf{z}_{k}-\mathbf{C}_{k}\mathbf{x}_{k})^{T}\mathbf{Q}_{k}(\mathbf{z}_{k}-\mathbf{C}_{k}\mathbf{x}_{k})\right)\exp\left((\mathbf{x}_{k}-\check{\mathbf{x}}_{k})^{T}\check{\mathbf{P}}_{k}(\mathbf{x}_{k}-\check{\mathbf{x}}_{k})\right)
p(xk∣x0,u1:k,z1:k)=η′∗exp((zk−Ckxk)TQk(zk−Ckxk))exp((xk−xˇk)TPˇk(xk−xˇk))
其中
η
′
\eta^{\prime}
η′是归一化常数。
根据§2,可以知道后验置信度服从高斯分布,记为
p
(
x
k
∣
x
0
,
u
1
:
k
,
z
1
:
k
)
=
N
(
x
^
k
,
P
^
k
)
p(\mathbf{x}_{k}|\mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k})=\mathcal{N}(\hat{\mathbf{x}}_{k},\hat{\mathbf{P}}_{k})
p(xk∣x0,u1:k,z1:k)=N(x^k,P^k)
且其后验均值和后验协方差满足
P
^
k
−
1
=
C
k
T
Q
k
−
1
C
k
+
P
ˇ
k
−
1
P
^
k
−
1
x
^
k
=
C
k
T
Q
k
−
1
z
k
+
P
ˇ
k
−
1
x
ˇ
k
\begin{align*} \hat{\mathbf{P}}_{k}^{-1}&=\mathbf{C}_{k}^{T}\mathbf{Q}_{k}^{-1}\mathbf{C}_{k}+\check{\mathbf{P}}_{k}^{-1}\tag8\\ \hat{\mathbf{P}}_{k}^{-1}\hat{\mathbf{x}}_{k}&=\mathbf{C}_{k}^{T}\mathbf{Q}_{k}^{-1}\mathbf{z}_{k}+\check{\mathbf{P}}_{k}^{-1}\check{\mathbf{x}}_{k}\tag9 \end{align*}
P^k−1P^k−1x^k=CkTQk−1Ck+Pˇk−1=CkTQk−1zk+Pˇk−1xˇk(8)(9)
要求后验均值和后验协方差,需要§3的Sherman-Morrison-Woodbury恒等式变形1,将
(
8
)
(8)
(8)等号两侧求逆即可得到恒等式中的形式,注意到多元正态分布要求其协方差矩阵为正定矩阵,可以验证其对应矩阵满足可逆条件。故有
P
^
k
=
P
ˇ
k
−
P
ˇ
k
C
k
T
(
Q
k
+
C
k
P
ˇ
k
C
k
T
)
−
1
C
k
P
ˇ
k
\hat{\mathbf{P}}_{k}=\check{\mathbf{P}}_{k}-\check{\mathbf{P}}_{k}\mathbf{C}_{k}^{T}(\mathbf{Q}_{k}+\mathbf{C}_{k}\check{\mathbf{P}}_{k}\mathbf{C}_{k}^{T})^{-1}\mathbf{C}_{k}\check{\mathbf{P}}_{k}
P^k=Pˇk−PˇkCkT(Qk+CkPˇkCkT)−1CkPˇk
定义中间变量,即卡尔曼增益
K
k
=
P
ˇ
k
C
k
T
(
Q
k
+
C
k
P
ˇ
k
C
k
T
)
−
1
(10)
\mathbf{K}_{k}=\check{\mathbf{P}}_{k}\mathbf{C}_{k}^{T}(\mathbf{Q}_{k}+\mathbf{C}_{k}\check{\mathbf{P}}_{k}\mathbf{C}_{k}^{T})^{-1}\tag{10}
Kk=PˇkCkT(Qk+CkPˇkCkT)−1(10)
故
P
^
k
=
(
I
−
K
k
C
k
)
P
ˇ
k
(11)
\hat{\mathbf{P}}_{k}=(\mathbf{I}-\mathbf{K}_{k}\mathbf{C}_{k})\check{\mathbf{P}}_{k}\tag{11}
P^k=(I−KkCk)Pˇk(11)
为化简
(
9
)
(9)
(9),对
(
10
)
(10)
(10)应用§3的Sherman-Morrison-Woodbury恒等式变形3和
(
8
)
(8)
(8),得到
K
k
=
P
^
k
C
k
T
Q
k
−
1
\begin{align*} \mathbf{K}_{k}=\hat{\mathbf{P}}_{k}\mathbf{C}_{k}^{T}\mathbf{Q}_{k}^{-1}\tag{12} \end{align*}
Kk=P^kCkTQk−1(12)
化简
(
9
)
(9)
(9),并将
(
10
)
,
(
12
)
(10),(12)
(10),(12)代入,得到
x
^
k
=
P
^
k
C
k
T
Q
k
−
1
z
k
+
P
^
k
P
ˇ
k
−
1
x
ˇ
k
=
K
k
z
k
+
(
I
−
K
k
C
k
)
P
ˇ
k
P
ˇ
k
−
1
x
ˇ
k
=
K
k
z
k
+
(
I
−
K
k
C
k
)
x
ˇ
k
=
x
ˇ
k
+
K
k
(
z
k
−
C
k
x
ˇ
k
)
\begin{align*} \hat{\mathbf{x}}_{k}&=\hat{\mathbf{P}}_{k}\mathbf{C}_{k}^{T}\mathbf{Q}_{k}^{-1}\mathbf{z}_{k}+\hat{\mathbf{P}}_{k}\check{\mathbf{P}}_{k}^{-1}\check{\mathbf{x}}_{k}\\ &=\mathbf{K}_{k}\mathbf{z}_{k}+(\mathbf{I}-\mathbf{K}_{k}\mathbf{C}_{k})\check{\mathbf{P}}_{k}\check{\mathbf{P}}_{k}^{-1}\check{\mathbf{x}}_{k}\\ &=\mathbf{K}_{k}\mathbf{z}_{k}+(\mathbf{I}-\mathbf{K}_{k}\mathbf{C}_{k})\check{\mathbf{x}}_{k}\\ &=\check{\mathbf{x}}_{k}+\mathbf{K}_{k}(\mathbf{z}_{k}-\mathbf{C}_{k}\check{\mathbf{x}}_{k})\tag{13} \end{align*}
x^k=P^kCkTQk−1zk+P^kPˇk−1xˇk=Kkzk+(I−KkCk)PˇkPˇk−1xˇk=Kkzk+(I−KkCk)xˇk=xˇk+Kk(zk−Ckxˇk)(13)
至此求出了后验高斯概率分布的具体形式,因此可以继续迭代下去。上面的过程可以归纳为“预测”和“更新”两个步骤:
- 预测:
- x ˇ k = A x ^ k − 1 + u k , P ˇ k = A k P ^ k − 1 A k T + R k \check{\mathbf{x}}_{k}=\mathbf{A}\hat{\mathbf{x}}_{k-1}+\mathbf{u}_{k},\quad\check{\mathbf{P}}_{k}=\mathbf{A}_{k}\hat{\mathbf{P}}_{k-1}\mathbf{A}_{k}^{T}+\mathbf{R}_{k} xˇk=Ax^k−1+uk,Pˇk=AkP^k−1AkT+Rk
- 更新:
- 先计算卡尔曼增益 K k = P ˇ k C k T ( Q k + C k P ˇ k C k T ) − 1 \mathbf{K}_{k}=\check{\mathbf{P}}_{k}\mathbf{C}_{k}^{T}(\mathbf{Q}_{k}+\mathbf{C}_{k}\check{\mathbf{P}}_{k}\mathbf{C}_{k}^{T})^{-1} Kk=PˇkCkT(Qk+CkPˇkCkT)−1
- 然后计算后验均值 x ^ k = x ˇ k + K k ( z k − C k x ˇ k ) \hat{\mathbf{x}}_{k}=\check{\mathbf{x}}_{k}+\mathbf{K}_{k}(\mathbf{z}_{k}-\mathbf{C}_{k}\check{\mathbf{x}}_{k}) x^k=xˇk+Kk(zk−Ckxˇk)和后验协方差 P ^ k = ( I − K k C k ) P ˇ k \hat{\mathbf{P}}_{k}=(\mathbf{I}-\mathbf{K}_{k}\mathbf{C}_{k})\check{\mathbf{P}}_{k} P^k=(I−KkCk)Pˇk,即得到后验高斯概率分布 x k ∼ N ( x ^ k , P ^ k ) \mathbf{x}_{k}\sim\mathcal{N}(\hat{\mathbf{x}}_{k},\hat{\mathbf{P}}_{k}) xk∼N(x^k,P^k)
2、扩展卡尔曼滤波器
回到 ( 1 ) (1) (1)式,实际上大部分的运动方程和观测方程是非线性的,即实际系统是非线性高斯系统。对非线性系统的运动方程和观测方程进行线性化,然后将线性系统中推导出的卡尔曼滤波器的结论应用于非线性系统中的算法,称为扩展卡尔曼滤波器,与此相对,前述在线性高斯系统推出的卡尔曼滤波器也称为经典卡尔曼滤波器。
根据§4,将
(
1
)
(1)
(1)中的运动方程在
(
x
^
k
−
1
,
u
k
,
0
)
(\hat{\mathbf{x}}_{k-1},\mathbf{u}_{k},\mathbf{0})
(x^k−1,uk,0)处做一阶泰勒展开
x
k
=
f
(
x
k
−
1
,
u
k
,
w
k
)
≈
f
(
x
^
k
−
1
,
u
k
,
0
)
+
∂
f
(
x
k
−
1
,
u
k
,
w
k
)
∂
x
k
−
1
∣
x
^
k
−
1
,
u
k
,
0
(
x
k
−
1
−
x
^
k
−
1
)
+
∂
f
(
x
k
−
1
,
u
k
,
w
k
)
∂
w
k
∣
x
^
k
−
1
,
u
k
,
0
w
k
\begin{align*} \mathbf{x}_{k}=\mathbf{f}(\mathbf{x}_{k-1},\mathbf{u}_{k},\mathbf{w}_{k})\approx&\mathbf{f}(\hat{\mathbf{x}}_{k-1},\mathbf{u}_{k},\mathbf{0})+\left.\frac{\partial{\mathbf{f}(\mathbf{x}_{k-1},\mathbf{u}_{k},\mathbf{w}_{k})}}{\partial\mathbf{x}_{k-1}}\right|_{\hat{\mathbf{x}}_{k-1},\mathbf{u}_{k},\mathbf{0}}(\mathbf{x}_{k-1}-\hat{\mathbf{x}}_{k-1})\\ &+\left.\frac{\partial{\mathbf{f}(\mathbf{x}_{k-1},\mathbf{u}_{k},\mathbf{w}_{k})}}{\partial\mathbf{w}_{k}}\right|_{\hat{\mathbf{x}}_{k-1},\mathbf{u}_{k},\mathbf{0}}\mathbf{w}_{k} \end{align*}
xk=f(xk−1,uk,wk)≈f(x^k−1,uk,0)+∂xk−1∂f(xk−1,uk,wk)
x^k−1,uk,0(xk−1−x^k−1)+∂wk∂f(xk−1,uk,wk)
x^k−1,uk,0wk
注意,原运动方程中的输入是自变量,而展开处的输入是具体的值,为简便起见,采用改写发法,后文某些量同理。还应注意到,无论实际输入是什么,均在其上展开,故输入对应的泰勒展开项为
0
0
0,下同。考虑到
x
ˇ
k
=
f
(
x
^
k
−
1
,
u
k
,
0
)
\check{\mathbf{x}}_{k}=\mathbf{f}(\hat{\mathbf{x}}_{k-1},\mathbf{u}_{k},\mathbf{0})
xˇk=f(x^k−1,uk,0),且记第二项中对系统状态的偏导数矩阵为
F
k
\mathbf{F}_{k}
Fk,记第三项中经线性变换后的过程噪声为
w
k
′
∼
N
(
0
,
R
k
′
)
\mathbf{w}_{k}^{\prime}\sim\mathcal{N}(\mathbf{0},\mathbf{R}_{k}^{\prime})
wk′∼N(0,Rk′),故上式可以写为
x
k
≈
x
ˇ
k
+
F
k
(
x
k
−
1
−
x
^
k
−
1
)
+
w
k
′
\begin{align*} \mathbf{x}_{k}&\approx\check{\mathbf{x}}_{k}+\mathbf{F}_{k}(\mathbf{x}_{k-1}-\hat{\mathbf{x}}_{k-1})+\mathbf{w}_{k}^{\prime}\tag{14} \end{align*}
xk≈xˇk+Fk(xk−1−x^k−1)+wk′(14)
注意此式中,
x
k
−
1
,
w
k
′
\mathbf{x}_{k-1},\mathbf{w}_{k}^{\prime}
xk−1,wk′为不确定的变量,其余量为可确定的已知量。
将
(
1
)
(1)
(1)中的观测方程在
(
x
ˇ
k
,
0
)
(\check{\mathbf{x}}_{k},\mathbf{0})
(xˇk,0)处做一阶泰勒展开
z
k
=
h
(
x
k
,
v
k
)
≈
h
(
x
ˇ
k
,
0
)
+
∂
h
(
x
k
,
v
k
)
∂
x
k
∣
x
ˇ
k
,
0
(
x
k
−
x
ˇ
k
)
+
∂
h
(
x
k
,
v
k
)
∂
v
k
∣
x
ˇ
k
,
0
v
k
\mathbf{z}_{k}=\mathbf{h}(\mathbf{x}_{k},\mathbf{v}_{k})\approx\mathbf{h}(\check{\mathbf{x}}_{k},\mathbf{0})+\left.\frac{\partial{\mathbf{h}(\mathbf{x}_{k},\mathbf{v}_{k})}}{\partial\mathbf{x}_{k}}\right|_{\check{\mathbf{x}}_{k},\mathbf{0}}(\mathbf{x}_{k}-\check{\mathbf{x}}_{k})+\left.\frac{\partial{\mathbf{h}(\mathbf{x}_{k},\mathbf{v}_{k})}}{\partial\mathbf{v}_{k}}\right|_{\check{\mathbf{x}}_{k},\mathbf{0}}\mathbf{v}_{k}
zk=h(xk,vk)≈h(xˇk,0)+∂xk∂h(xk,vk)
xˇk,0(xk−xˇk)+∂vk∂h(xk,vk)
xˇk,0vk
记第二项中对系统状态的偏导数矩阵为
H
k
\mathbf{H}_{k}
Hk,记第三项中经线性变换后的过程噪声为
v
k
′
∼
N
(
0
,
Q
k
′
)
\mathbf{v}_{k}^{\prime}\sim\mathcal{N}(\mathbf{0},\mathbf{Q}_{k}^{\prime})
vk′∼N(0,Qk′),故上式可以写为
z
k
≈
h
(
x
ˇ
k
,
0
)
+
H
k
(
x
k
−
x
ˇ
k
)
+
v
k
′
\begin{align*} \mathbf{z}_{k}&\approx\mathbf{h}(\check{\mathbf{x}}_{k},\mathbf{0})+\mathbf{H}_{k}(\mathbf{x}_{k}-\check{\mathbf{x}}_{k})+\mathbf{v}_{k}^{\prime}\tag{15} \end{align*}
zk≈h(xˇk,0)+Hk(xk−xˇk)+vk′(15)
注意此式中,
x
k
,
v
k
′
\mathbf{x}_{k},\mathbf{v}_{k}^{\prime}
xk,vk′为不确定的变量,其余量为可确定的已知量。
得出观测方程和运动方程的线性近似后,仍根据贝叶斯滤波求系统状态的后验概率分布,具体方法是将 ( 14 ) , ( 15 ) (14),(15) (14),(15)代入 ( 3 ) , ( 4 ) (3),(4) (3),(4)求解(不可将 ( 14 ) , ( 15 ) (14),(15) (14),(15)直接代入线性卡尔曼滤波的结论,因为二者不能完全符合 ( 2 ) (2) (2)的形式)。此过程同样可以归纳为“预测”和“更新”两个步骤:
-
预测:
- x ˇ k = f ( x ^ k − 1 , u k , 0 ) , P ˇ k = F k P ^ k − 1 F k T + R k ′ \check{\mathbf{x}}_{k}=\mathbf{f}(\hat{\mathbf{x}}_{k-1},\mathbf{u}_{k},\mathbf{0}),\quad\check{\mathbf{P}}_{k}=\mathbf{F}_{k}\hat{\mathbf{P}}_{k-1}\mathbf{F}_{k}^{T}+\mathbf{R}_{k}^{\prime} xˇk=f(x^k−1,uk,0),Pˇk=FkP^k−1FkT+Rk′
-
更新:
-
先计算卡尔曼增益 K k = P ˇ k H k T ( Q k ′ + H k P ˇ k H k T ) − 1 \mathbf{K}_{k}=\check{\mathbf{P}}_{k}\mathbf{H}_{k}^{T}(\mathbf{Q}_{k}^{\prime}+\mathbf{H}_{k}\check{\mathbf{P}}_{k}\mathbf{H}_{k}^{T})^{-1} Kk=PˇkHkT(Qk′+HkPˇkHkT)−1
-
然后计算后验均值 x ^ k = x ˇ k + K k ( z k − h ( x ˇ k , 0 ) ) \hat{\mathbf{x}}_{k}=\check{\mathbf{x}}_{k}+\mathbf{K}_{k}(\mathbf{z}_{k}-\mathbf{h}(\check{\mathbf{x}}_{k},\mathbf{0})) x^k=xˇk+Kk(zk−h(xˇk,0))和后验协方差 P ^ k = ( I − K k H k ) P ˇ k \hat{\mathbf{P}}_{k}=(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})\check{\mathbf{P}}_{k} P^k=(I−KkHk)Pˇk,即得到后验高斯概率分布 x k ∼ N ( x ^ k , P ^ k ) \mathbf{x}_{k}\sim\mathcal{N}(\hat{\mathbf{x}}_{k},\hat{\mathbf{P}}_{k}) xk∼N(x^k,P^k)
-
附录
§1、高斯分布随机变量的带噪声变换的分布
若
p
(
x
)
=
N
(
μ
x
,
Σ
x
x
)
p(\mathbf{x})=\mathcal{N}(\boldsymbol{\mu}_{x},\boldsymbol{\Sigma}_{xx})
p(x)=N(μx,Σxx),略写维数,其带噪声变换为
y
=
g
(
x
)
+
δ
\mathbf{y}=\mathbf{g}(\mathbf{x})+\boldsymbol{\delta}
y=g(x)+δ,
δ
∼
N
(
0
,
R
)
\boldsymbol{\delta}\sim\mathcal{N}(\mathbf{0},\mathbf{R})
δ∼N(0,R),则在
x
\mathbf{x}
x条件下
y
\mathbf{y}
y的分布为
p
(
y
∣
x
)
=
N
(
g
(
x
)
,
R
)
(A1)
p(\mathbf{y}|\mathbf{x})=\mathcal{N}(\mathbf{g}(\mathbf{x}),\mathbf{R})\tag{A1}
p(y∣x)=N(g(x),R)(A1)
在该条件下做全概率展开
p
(
y
)
=
∫
p
(
y
∣
x
)
p
(
x
)
d
x
(A2)
p(\mathbf{y})=\int p(\mathbf{y}|\mathbf{x})p(\mathbf{x})d\mathbf{x}\tag{A2}
p(y)=∫p(y∣x)p(x)dx(A2)
然而当
g
(
⋅
)
\mathbf{g}(\cdot)
g(⋅)为非线性时变换,难以得到上式的闭式解,故需要对非线性变换做线性化处理(见附录§4),
g
(
x
)
≈
μ
y
+
G
(
x
−
μ
x
)
(A3)
\mathbf{g}(\mathbf{x})\approx\boldsymbol{\mu}_{y}+\mathbf{G}(\mathbf{x}-\boldsymbol{\mu}_{x})\tag{A3}
g(x)≈μy+G(x−μx)(A3)
其中
G
i
j
=
∂
f
i
(
x
)
∂
x
j
∣
μ
x
,
μ
y
=
g
(
μ
x
)
\mathbf{G}_{ij}=\left.\frac{\partial{f_{i}(\mathbf{x})}}{\partial{x_{j}}}\right|_{\boldsymbol{\mu}_{x}},\boldsymbol{\mu}_{y}=\mathbf{g}(\boldsymbol{\mu}_{x})
Gij=∂xj∂fi(x)
μx,μy=g(μx)。将
(
A
3
)
(\mathrm{A}3)
(A3)代入
(
A
1
)
(\mathrm{A}1)
(A1),再将
(
A
1
)
(\mathrm{A}1)
(A1)代入
(
A
2
)
(\mathrm{A}2)
(A2),可得
p
(
y
)
=
N
(
μ
y
,
Σ
y
y
)
=
N
(
g
(
μ
x
)
,
R
+
G
Σ
x
x
G
T
)
p(\mathbf{y})=\mathcal{N}(\boldsymbol{\mu}_{y},\boldsymbol{\Sigma}_{yy})=\mathcal{N}(\mathbf{g}(\boldsymbol{\mu}_{x}),\mathbf{R}+\mathbf{G}\boldsymbol{\Sigma}_{xx}\mathbf{G}^{T})
p(y)=N(μy,Σyy)=N(g(μx),R+GΣxxGT)
§2、高斯概率密度函数的归一化积
η
1
exp
(
−
1
2
(
x
−
μ
)
Σ
−
1
(
x
−
μ
)
)
≡
η
2
∏
k
=
1
K
exp
(
−
1
2
(
A
k
x
−
μ
k
)
Σ
k
−
1
(
A
k
x
−
μ
k
)
)
\eta_{1}\exp\left(-\frac{1}{2}(\mathbf{x}-\boldsymbol{\mu})\boldsymbol{\Sigma}^{-1}(\mathbf{x}-\boldsymbol{\mu})\right)\equiv\eta_{2}\prod_{k=1}^{K}\exp\left(-\frac{1}{2}(\mathbf{A}_{k}\mathbf{x}-\boldsymbol{\mu}_{k})\boldsymbol{\Sigma}^{-1}_{k}(\mathbf{A}_{k}\mathbf{x}-\boldsymbol{\mu}_{k})\right)
η1exp(−21(x−μ)Σ−1(x−μ))≡η2k=1∏Kexp(−21(Akx−μk)Σk−1(Akx−μk))
其中
Σ
−
1
=
∑
k
=
1
K
A
k
T
Σ
k
−
1
A
k
Σ
−
1
μ
=
∑
k
=
1
K
A
k
T
Σ
k
−
1
μ
k
\begin{align*} \mathbf{\Sigma}^{-1}&=\sum_{k=1}^{K}\mathbf{A}_{k}^{T}\boldsymbol{\Sigma}_{k}^{-1}\mathbf{A}_{k}\\ \mathbf{\Sigma}^{-1}\boldsymbol{\mu}&=\sum_{k=1}^{K}\mathbf{A}_{k}^{T}\boldsymbol{\Sigma}_{k}^{-1}\boldsymbol{\mu}_{k} \end{align*}
Σ−1Σ−1μ=k=1∑KAkTΣk−1Ak=k=1∑KAkTΣk−1μk
A
k
∈
R
M
k
×
N
\mathbf{A}_{k}\in\mathbb{R}^{M_{k}\times N}
Ak∈RMk×N,
η
1
,
η
2
\eta_{1},\eta_{2}
η1,η2为归一化常数,保证等号两侧积分为
1
1
1
§3、Sherman-Morrison-Woodbury恒等式(矩阵求逆引理)
若
A
\mathbf{A}
A可逆,
D
\mathbf{D}
D可逆,
(
D
+
C
A
B
)
(\mathbf{D}+\mathbf{C}\mathbf{A}\mathbf{B})
(D+CAB)可逆,
(
A
−
1
+
B
D
−
1
C
)
(\mathbf{A}^{-1}+\mathbf{B}\mathbf{D}^{-1}\mathbf{C})
(A−1+BD−1C)可逆,则
(
A
−
1
+
B
D
−
1
C
)
−
1
≡
A
−
A
B
(
D
+
C
A
B
)
−
1
C
A
(
D
+
C
A
B
)
−
1
≡
D
−
1
−
D
−
1
C
(
A
−
1
+
B
D
−
1
C
)
−
1
B
D
−
1
A
B
(
D
+
C
A
B
)
−
1
≡
(
A
−
1
+
B
D
−
1
C
)
−
1
B
D
−
1
(
D
+
C
A
B
)
−
1
C
A
≡
D
−
1
C
(
A
−
1
+
B
D
−
1
C
)
−
1
\begin{align*} (\mathbf{A}^{-1}+\mathbf{B}\mathbf{D}^{-1}\mathbf{C})^{-1}&\equiv\mathbf{A}-\mathbf{A}\mathbf{B}(\mathbf{D}+\mathbf{C}\mathbf{A}\mathbf{B})^{-1}\mathbf{C}\mathbf{A}\\ (\mathbf{D}+\mathbf{C}\mathbf{A}\mathbf{B})^{-1}&\equiv\mathbf{D}^{-1}-\mathbf{D}^{-1}\mathbf{C}(\mathbf{A}^{-1}+\mathbf{B}\mathbf{D}^{-1}\mathbf{C})^{-1}\mathbf{B}\mathbf{D}^{-1}\\ \mathbf{A}\mathbf{B}(\mathbf{D}+\mathbf{C}\mathbf{A}\mathbf{B})^{-1}&\equiv(\mathbf{A}^{-1}+\mathbf{B}\mathbf{D}^{-1}\mathbf{C})^{-1}\mathbf{B}\mathbf{D}^{-1}\\ (\mathbf{D}+\mathbf{C}\mathbf{A}\mathbf{B})^{-1}\mathbf{C}\mathbf{A}&\equiv\mathbf{D}^{-1}\mathbf{C}(\mathbf{A}^{-1}+\mathbf{B}\mathbf{D}^{-1}\mathbf{C})^{-1} \end{align*}
(A−1+BD−1C)−1(D+CAB)−1AB(D+CAB)−1(D+CAB)−1CA≡A−AB(D+CAB)−1CA≡D−1−D−1C(A−1+BD−1C)−1BD−1≡(A−1+BD−1C)−1BD−1≡D−1C(A−1+BD−1C)−1
参考:《机器人学中的状态估计》,《视觉SLAM十四讲:从理论到实践》