滤波算法与SLAM
第三章：MAP/MLE问题和贝叶斯网络
文章目录
- 滤波算法与SLAM
- 前言
- 一、最大后验问题（MAP）与最大似然问题（MLE）
- 二、贝叶斯网络与SLAM问题
- 三、因子图与SLAM问题
- [四、从概率角度理解Kalman Filter与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
这里就是贝叶斯滤波公式的推导了，能够参考的内容很多，推荐这里