滤波算法与SLAM：从概率角度理解SLAM问题
滤波算法与SLAM
第三章：MAP/MLE问题和贝叶斯网络
文章目录
- 滤波算法与SLAM
- 前言
- 一、最大后验问题（MAP）与最大似然问题（MLE）
- 二、贝叶斯网络与SLAM问题
- 三、因子图与SLAM问题
- 四、从概率角度理解Kalman Filter与SLAM
前言
考虑到滤波与优化不分家，都是基于贝叶斯理论发展出来的，后续章节中必然会介绍因子图
所以本章大致介绍如何从概率角度理解SLAM问题
以及贝叶斯网络、因子图、Kalman Filter之间的关联
一、最大后验问题（MAP）与最大似然问题（MLE）
（本项目不面向完全的小白，所以会默认读者至少接触过本科的概率学，了解分布和协方差等内容）
正如第二章中提到过的，后验概率由似然概率与先验概率所决定，而在SLAM问题中可以理解为：
当我们有机器人的观测： Z n Z_n Zn，以及机器人的先验： X n − 1 X_{n-1} Xn−1
那么在n时刻，机器人的位姿就应该使最大后验 P ( X n ∣ Z n ) P(X_n|Z_n) P(Xn∣Zn)最大
也就是 X n = a r g m a x P ( X n ∣ Z n ) = a r g m a x P ( Z n ∣ X n ) P ( X n − 1 ) X_n=argmax\; P(X_n|Z_n) = argmax\;P(Z_n|X_n)P(X_{n-1}) Xn=argmaxP(Xn∣Zn)=argmaxP(Zn∣Xn)P(Xn−1)
此时SLAM问题就变成了求某个
X
X
X使得后验概率
P
(
X
n
∣
Z
n
)
P(X_n|Z_n)
P(Xn∣Zn)最大
即最大后验概率问题（Maximum A Posteriori, MAP）
那么最大似然问题呢（Maximum Likelihood Estimation, MLE）
注意最大后验概率中存在一个先验项
P
(
X
n
−
1
)
P(X_{n-1})
P(Xn−1)
如果这个先验项无法获得，或者属于均匀分布时，最大后验概率问题就退化成最大似然问题
简单解释为，最大似然问题不考虑先验概率，而最大后验问题会考虑
这里只需要理解最大后验概率问题是似然和先验的共同结果即可
（但是最好不要认为两者是一个东西，MLE更像频率派的想法，MAP更像贝叶斯派的想法，即便是当数据量极大时两者的结果差不多，但是为了不发生争吵，还是分开讲比较好，以和为贵）
二、贝叶斯网络与SLAM问题
想要深入理解的话，参考：贝叶斯网络介绍，这里只需要理解：
贝叶斯网络是一个有向图，用于表述概率分布之间的联系
比如一个概率分布满足：
P
(
a
,
b
,
c
)
=
P
(
c
∣
a
,
b
)
P
(
c
∣
a
)
p
(
a
)
P(a,b,c)=P(c|a,b)P(c|a)p(a)
P(a,b,c)=P(c∣a,b)P(c∣a)p(a)
那么就可以用贝叶斯网络表述为：
回到SLAM问题中：
机器人的状态记为 X 0 : n = ( x 0 , x 1 , ⋯ , x n ) X_{0:n}=(x_0, x_1,\cdots,x_n) X0:n=(x0,x1,⋯,xn)
机器人的观测记为 Z 0 : n = ( z 0 , z 1 , ⋯ , z n ) Z_{0:n}=(z_0, z_1,\cdots,z_n) Z0:n=(z0,z1,⋯,zn)
那么最大后验概率问题就是令 P ( X 0 : n ∣ Z 0 : n ) P(X_{0:n}|Z_{0:n}) P(X0:n∣Z0:n)最大，换言之，令 P ( Z 0 : n ∣ X 0 : n ) P ( X 0 : n ) P(Z_{0:n}|X_{0:n})P(X_{0:n}) P(Z0:n∣X0:n)P(X0:n)最大
但是考虑到一般只需要求当前时刻的位姿，所以上述问题可以简化为
x
∗
=
arg
max
x
n
P
(
Z
0
:
n
∣
x
k
)
P
(
x
k
)
x^*=\arg \mathop{\max}\limits_{x_n}P(Z_{0:n}|x_k)P(x_k)
x∗=argxnmaxP(Z0:n∣xk)P(xk)
假设观测都是独立的，那么上述公式可以写成
x
∗
=
arg
max
x
n
∏
i
=
0
n
P
(
z
i
∣
x
k
)
P
(
x
k
)
x^*=\arg \mathop{\max}\limits_{x_n}\prod_{i=0}^nP(z_{i}|x_k)P(x_k)
x∗=argxnmaxi=0∏nP(zi∣xk)P(xk)
画成贝叶斯网络如下
再进一步的，考虑多个时刻的状态进来，就可以将贝叶斯网络画成：
理解了贝叶斯网络与SLAM最大后验概率问题的关系后，就可以介绍因子图了
三、因子图与SLAM问题
因子图的原理是对概率图进行因式分解
比如这里的贝叶斯网络中，
P
(
∗
)
P(*)
P(∗)代表的是概率密度函数
那么因子图就使用一个通用的公式来表示这些关系
∏
i
=
0
n
P
(
z
i
∣
x
k
)
P
(
x
k
)
→
∏
i
=
0
n
f
i
(
z
i
,
x
k
)
f
x
(
x
k
)
\prod_{i=0}^nP(z_{i}|x_k)P(x_k)\rightarrow\prod_{i=0}^nf_i(z_i,x_k)f_x(x_k)
i=0∏nP(zi∣xk)P(xk)→i=0∏nfi(zi,xk)fx(xk)
这样就可以基于这个公式画一个无向图
圆形是状态量，不可观测，只能计算，方形是观测变量
用因子图就可以很直观地看出来每个变量之间的关系
再进阶一些，看LIO-SAM的因子图表示
雷达观测需要比较前后帧点云来得出轨迹变化，所以绿色部分会链接两个状态，IMU的预积分同理
而GPS与上下文均无关系，是直接获得的，所以用起来也不需要和谁链接取先验
至于因子图的求解与使用，参考GTSAM使用教程即可
四、从概率角度理解Kalman Filter与SLAM
这里就是贝叶斯滤波公式的推导了，能够参考的内容很多，推荐这里