SLAM基础——如何从最大后验概率到最小二乘（理论篇）
最大后验概率
符号说明
本文的标记没有用粗体表示，但是要记住本文的变量都是任意多维变量
定义
最大后验概率（MAP:Maximum a Posterior）具有以下逻辑
- 我们的最终目标是想要估计一个未知量\(x\)，令其概率分布为\(p(x)\).
- 如果我们能够知道\(p(x)\)的具体表达，则\(x\)的估计值应该使得概率最大
- 当我们知道一些与\(x\)有关的观测结果\(z\)，\(p(x|z)\)称做\(x\)的后验概率
- 于是我们的估计可以变成使得\(p(x|z)\)最大化的\(x\)
上述问题称作最大后验概率，也就是在已知观测值的情况下如何求取未知量。 从形式上可以做如下定义：
\[ \hat{x}_{MAP}(z)=\underset{x}{\operatorname{argmax\ }}p(x|z)\tag{1} \]
将上述定义代入SLAM问题
- \(x\)对应机器人的位姿
- \(z\)对应SLAM系统的观测，比如路标点、激光里程计的输出、视觉里程计的输出等
- SLAM的后端优化最终目标就是求取在给定的观测下，机器人的位姿估计值，也就是使得后验概率\(p(x|z)\)最大化的位姿\(x\)
最大似然估计
贝叶斯定理
为了让没有基础的读者更容易理顺推理的过程，我们先简单介绍一下贝叶斯定理（Bayes's theorem）
贝叶斯定义描述随机事件A和B的条件概率的一个关系： \[ P(A|B) = \frac{P(A)P(B|A)}{P(B)}\tag{Bayes's Theorem} \]
- \(P(A)\)是\(A\)的先验概率，也就是事件\(A\)的概率分布。\(P(B)\)同理
- \(P(A|B)\)是事件\(B\)发生后\(A\)的条件概率，也叫\(B\)发生后\(A\)的后验概率
- \(P(A|B)\)表示的是事件\(B\)是结果，A是因，就是在知道结果的情况下求原因的概率
- \(P(B|A)\)是事件\(A\)发生后\(B\)的条件概率。但是应该理解当\(B\)固定后\(A\)的似然，也就是\(P(B|A)=L(A|B)\)
- \(P(B|A)\)或者\(L(A|B)\)表示已知有事件B发生，运用似然函数\(L(A|B)\)，我们估计或猜测\(A\)的不同值的可能性
注意：在上面中\(A、B\)是有区分的，虽然\(P(B|A)\)的形式与条件概率函数的形式相同，但是我们关注的变量是\(A\)
参考： 1. 关于贝叶斯定理的推导wiki 2. 关于似然函数的理解wiki
从最大后验概率（MAP）到最大似然估计（MLE）
回顾：（对理解很重要，所以回顾一下）在公式（1）中，\(x\)代表我们要估计的机器人位姿，\(z\)代表我们的观测。这里的观测是一个宽泛的定义，路标（landmark）可以是一种观测，图像特征点也可以是一种观测，激光里程计或者视觉里程计的结果也可以是一种观测。 注意：应该是现有了机器人当前的位姿，然后在当前位姿下对周围环境进行观测。也就是当前位姿是 因，观测是 果
根据贝叶斯定理，有：
\[ \begin{split} p(x|z) &= \frac{p(z|x)p(x)}{p(z)}\\ &\propto p(z|x)p(x) \end{split} \]
上述公式结果的第一项称为 似然（likelihood），第二项是是 先验（prior） 用文字解释一下就是，直接求位姿的最大后验概率是很困难的，但是根据贝叶斯法则，我们可以转换为求取似然和先验的乘积的最大，即下式
\[ \begin{split} \hat{x}_{MAP} &= \underset{x}{\operatorname{argmax\ }}p(x|z)\\ &= \underset{x}{\operatorname{argmax\ }}p(z|x)p(x)\\ &\approx \underset{x}{\operatorname{argmax\ }}p(z|x) \end{split}\tag{2} \]
当然，我们也没有机器人位姿的先验\(p(x)\)，于是，公式（2）可以近似成只求取最大似然(MLE,Maximum LIkelihood Estimation)
\[ \hat{x}_{MLE} = \underset{x}{\operatorname{argmax\ }}p(z|x)\tag{3} \]
MAP与MLE的联系
在上文公式（2）中我们直接忽略了先验项\(p(x)\)，并在最大后验概率和最大似然两者间用了约等号，那么这两者间是否有什么其他关系呢？
\[ \begin{split} \hat{x}_{MAP} &= \underset{x}{\operatorname{argmax\ }}p(x|z)\\ &= \underset{x}{\operatorname{argmax\ }} \ln p(x|z)\\ &= \underset{x}{\operatorname{argmax\ }} \ln p(z|x)p(x)\\ &= \underset{x}{\operatorname{argmax\ }} \ln p(z|x) + \ln p(x)\\ \end{split} \]
如果当先验概率\(p(x)\)是一个均分分布的时候，上式就会变成：
\[ \begin{split} \hat{x}_{MAP} &= \underset{x}{\operatorname{argmax\ }} \ln p(z|x) + \ln p(x)\\ &= \underset{x}{\operatorname{argmax\ }} \ln p(z|x) + constant\\ &= \underset{x}{\operatorname{argmax\ }} \ln p(z|x) \end{split} \]
也就是说MLP实际上是MAP中，将先验概率做均匀分布假设的特殊情况 上面这句话就是说，在没有任何其他条件下，我们认为\(x\)取任何值的概率都一样。
当然，其实我们也可以直接认为SLAM后端的问题就是在给定观测的情况下，求取位姿的似然
最大似然的负对数形式
公式（3）中，可以对优化目标取负对数（对数函数是单调函数，最大化单调递增函数等价于最小化递减函数），于是，我们有如下形式：
\[ \begin{split} \hat{x}_{MLE} &= \underset{x}{\operatorname{argmax\ }}p(z|x)\\ &= \underset{x}{\operatorname{argmin\ }} - \ln p(z|x) \end{split}\tag{4} \]
我们后文都会采用公式（4）这种形式
从最大似然估计到非线性最小二乘问题
高斯分布的最大似然估计形式
因为高斯分布有很多优美的性质，所以我们在SLAM中大量地假设各种变量符合高斯分布，这一小节先推导高斯分布的最大似然估计的形式，方便后面直接使用。 PS:这部分的标记没有用粗体表示，但是要记住这里的变量都是多维变量
对于一个任意高维高斯分布：\(x \sim \mathcal{N}(\mu,\Sigma)\)，概率密度函数为：
\[ p(x) = \frac{1}{\sqrt{(2\pi)^N det(\Sigma)}}\exp\big(-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)\big) \]
对上述取负对数，可得：
\[ -\ln(p(x)) = \frac{1}{2}\ln\big((2\pi)^N\det(\Sigma)\big) + \frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)\tag{5} \]
由于上述第一项与\(x\)无关，于是在求最小化的时候第一项可以忽略。上述第二项中，\(\Sigma^{-1}\)叫做 信息矩阵 ,为协方差的倒数
\[ 信息矩阵 := \Sigma^{-1} \]
SLAM优化目标的最小二乘形式
令机器人当前时刻的姿态为\(x\),观测于姿态之间的关系使用函数\(h(x)\)表达。则，对于观测\(z\)，有如下观测模型：
\[ z = h(x) + v \]
其中\(v\)为噪声项，我们假设噪声项服从均为为0，方差为\(Q\)的高斯分布，也就是
\[ v \sim \mathcal{N}(h(x), Q) \]
于是，而可以得到，在状态\(x\)即已知道的情况下，观测\(z\)的条件概率（也就是似然）也是一个高斯分布
\[ z \sim \mathcal{N}(h(x), Q) \]
为了求得\(x\)，求最大似然，根据公式（5），并忽略第一项和第二项的\(\frac{1}{2}\)系数，则：
\[ \begin{split} \hat{x}_{MLE} &=\underset{x}{\operatorname{argmax\ }} p(z|x)\\ &=\underset{x}{\operatorname{argmin\ }} -\ln p(z|x)\\ &=\underset{x}{\operatorname{argmin\ }} (z-h(x))^T\Sigma^{-1}(z-h(x)) \end{split}\tag{6} \]
可以看出，公式（6）是一个二次型，可以看成是信息矩阵\(\Sigma^{-1}\)加权后的欧式距离（二范数）。其中的\(h(x)\)我们可以称为观测模型，描述机器人位姿和观测之间的关系。我们可以定义观测与模型之间的误差如下：
\[ e_{z} = z - h(x) \]
则，公式（6）可以写成
\[ \begin{split} \hat{x}_{MLE} &= \underset{x}{\operatorname{argmin\ }}\big(z-h(x)\big)^T\Sigma^{-1}\big(z-h(x)\big)\\ &= \underset{x}{\operatorname{argmin\ }}||e_z||^2_{\Sigma^{-1}} \end{split}\tag{7} \]
公式（7）的\(\Sigma_{-1}\)下标指使用信息矩阵进行加权