后端要做什么？

前端能够根据相邻的两幅图像判断出此时此刻的位姿，是暂时的；那么后端需要对前端测量以及计算的结果进行矫正，不仅用过去的信息，也用未来的信息更新自己，希望能够得到一个长时间的正确状态。

前面有讲过，在运动方程和观测方程中，如果把位姿和路标看成随机变量，就可以把问题变为已知观测数据和运动数据情况下，如何确定状态量的分布问题，是一个状态估计问题。假设噪声和状态量服从高斯分布，只需要估计它的均值和方差，即可确定状态量的分布。

完成后端优化可以使用滤波器，也可以使用非线性优化，本讲对两种方法都做了推导。

概述

状态估计的概率解释

SLAM过程可以由运动方程与观测方程来描述, 那么假设在 $t=0$ 到 $t=N$ 的时间段内, 有位姿 $x_0$ 到 $x_N$, 并且有路标 $y_1, \cdots , y_M$, 那么运动方程与观测方程写为:

$$ \left\{ \begin{array} { l } \boldsymbol { x } _ { k } = f \left( \boldsymbol { x } _ { k - 1 } , \boldsymbol { u } _ { k } \right) + \boldsymbol { w } _ { k } \\ \boldsymbol { z } _ { k, j } = h \left( \boldsymbol { y } _ { j },\boldsymbol { x } _ { k } \right) + \boldsymbol { v } _ { k, j } \end{array} \quad k = 1 , \ldots , N, j=1 \cdots M \right. $$

其中, $\boldsymbol u$ 为运动数据, $\boldsymbol z$ 为观测数据. 注意以下几点:

1. 观测方程中, 只有 $x_k$ 能看到 $y_i$ 才有观测数据, 否则就没有. 一般情况下, 由于视觉SLAM的特征点数量 众多, 实际的观测方程数量会远远大于运动方程.
2. 由于可能没有能够测量运动方程的设备, 因此没有运动方程. 这时候, 我们会假设相机保持静止或者相机保持匀速直线运动.
3. 当只有运动方程的时候, 误差是逐渐累积的, 我们对位置方差的估计将会越来越大. 而如果有观测方程, 那么位置估计的不确定性会变小.

我们可以将位姿和路标写在一起, 将 $\boldsymbol x_k$ 记为 $\boldsymbol { x } _ { k } \triangleq \left\{ \boldsymbol { x } _ { k } , \boldsymbol { y } _ { 1 } , \ldots , \boldsymbol { y } _ { m } \right\}$, 其为 $k$ 时刻的所有未知量, 包含了当前时刻的相机位姿与 $m$ 个路标点. 将 $k$ 时刻的所有观测记为 $z_k$. 那么, 原方程可以写为:

$$ \begin{equation}\left\{ \begin{array} { l } \boldsymbol { x } _ { k } = f \left( \boldsymbol { x } _ { k - 1 } , \boldsymbol { u } _ { k } \right) + \boldsymbol { w } _ { k } \\ \boldsymbol { z } _ { k } = h \left( \boldsymbol { x } _ { k } \right) + \boldsymbol { v } _ { k } \end{array} \quad k = 1 , \ldots , N \right.\end{equation} $$

在已知 $0$ 时刻的状态、$1:k$ 时刻的观测下，估计 $k$ 现在的状态分布 $P \left( \boldsymbol { x } _ { k } \mid \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k } \right)$, 则可以得到

$$ \begin{equation}P \left( \boldsymbol { x } _ { k } \mid \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k } \right) \propto P \left( \boldsymbol { z } _ { k } \mid \boldsymbol { x } _ { k } \right) P \left( \boldsymbol { x } _ { k } \mid \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right)\end{equation} $$

记 $P(A) := P(\boldsymbol x_k), P(B):=P(\boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 }), P(C) := P(\boldsymbol z_k)$. 则 $P \left( \boldsymbol { x } _ { k } \mid \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k } \right) = P(A|BC).$

$$ P(A|BC)= \frac{P(ABC)}{P(BC)} = \frac{P(BC|A)P(A)}{P(BC)} $$

假设 $B, C$ 之间相互独立, 则 $P(BC|A) = P(B|A) \cdot P(C|A)$, $P(BC)=P(B)\cdot P(C)$带入得:

$$ P(A|BC) = \frac{P(B|A)P(C|A)P(A)}{P(B)P(C)} \\= P(AB)/P(A) \cdot P(AC)/P(A) \cdot P(A) / P(B)/P(C) \\ = P(A|B)P(B)/P(A) \cdot P(A|C)P(C) \cdot P(A)/P(B)/P(C) \\ = P(A|B) P(A|C) / P(A) $$

另外也可以得到 $P(A|BC) = P(A|B)P(C|A)P(C)$.

即 $P \left( \boldsymbol { x } _ { k } \mid \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k } \right) = P(\boldsymbol { x } _ { k } \mid \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k-1 } ) P(z_k|x_k) / P(z_k)$

其中, 第一项为由观测方程确定得似然, 第二项为先验部分.

先验概率中 $k$ 时刻的状态 $P \left( \boldsymbol { x } _ { k } \mid \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right)$ 受到前面 $0:k-1$时刻状态的影响，对先验概率进行以 $x_{k-1}$ 时刻为条件展开，可以写为