一、概述
1. 什么是后端（Backend）
- 从前端给予的带噪声的数据估计物体真实状态
- Estimated the inner state from noisy data
graph LR
%%这是一条注释，在渲染图中不可见
A[后端] --> C(渐进式)
C -->|通过新数据更新估计| F[滤波]
A --> D(批量式)
D -->|通过批量数据优化| E[BA优化]
SLAM过程可以由运动方程和观测方程来描述。那么在\(t=0\) 到\(t=N\)时刻，有位姿\(x_0\)到\(x_N\)，路标为\(y_1,···,y_M\)
\[\begin{equation}
\left\{ \begin{array}{l}
{\mathbf{x}_k} = f\left( {{\mathbf{x}_{k - 1}},{\mathbf{u}_k}} \right) + \mathbf{w}_k \\
{\mathbf{z}_{k,j}} = h\left( {{ \mathbf{y}_j},{ \mathbf{x}_k}} \right)+ \mathbf{v}_{k,j}
\end{array} \right. \quad k=1, \ldots, N, \ j=1, \ldots, M.
\end{equation}
\]
- 即使观测仅部分点可以得到，但特征点众多，观测方程数量远大于运动方程；
- 运动方程可以没有，即不知道相机的位置移动，那么演变为类似带图片先后顺序的SfM问题；
如果把位姿\(x\)和路标\(y\)看成服从某种概率分布的随机变量，那么问题可以描述为通过运动数据\(u\)和观测数据\(z\)来估计\(x,y\)的分布。
2. 后端的概率表示
将k时刻的所有观测量记为\(z_k\)，所有未知量记为\(x_k\)。这里的\(x_k\)包含第k时刻的相机位姿x_k和路标点位置\(y_1,y_2···y_m\)。即记为：
\[\mathbf{x}_k \xlongequal{def} \{\mathbf{x}_k, \mathbf{y}_1, \ldots, \mathbf{y}_m \}
\]
那么运动方程和观测方程可以写为：
\[\begin{equation} \left\{ \begin{array}{l} {\mathbf{x}_k} = f\left( {{\mathbf{x}_{k - 1}},{\mathbf{u}_k}} \right) + \mathbf{w}_k \\ {\mathbf{z}_{k}} = h\left( \mathbf{x}_k \right)+ \mathbf{v}_{k} \end{array} \right. \quad k=1, \ldots, N .\end{equation}
\]
我们的目标是通过0到k时刻的数据来估计k时刻的未知量\(x_k\)，即需要估计状态分布:
\[P(\mathbf{x}_k | \mathbf{x}_0, \mathbf{u}_{1:k}, \mathbf{z}_{1:k})
\]
那么由Bayes法则有：
至此有两种分歧：
① 具有马尔科夫性，仅仅和k-1时刻状态有关；
② 与先前所有的时刻均有关；
二、滤波方法
\[P\left( {{\mathbf{x}_k}|{\mathbf{x}_0},{\mathbf{u}_{1:k}},{\mathbf{z}_{1:k}}} \right) \propto \underbrace{P\left( {{\mathbf{z}_k}|{\mathbf{x}_k}} \right)}_{\text{似然}} \underbrace{P\left( {{\mathbf{x}_k}|{\mathbf{x}_0},{\mathbf{u}_{1:k}},{\mathbf{z}_{1:k - 1}}} \right)}_{\text{先验}}.
\]
似然由观测方程得出，先验由运动方程得出。那么先验部分关于\(x_{k-1}\)为条件概率展开：
\[\small
\underbrace{P\left( {{\mathbf{x}_k}|{\mathbf{x}_0},{\mathbf{u}_{1:k}},{\mathbf{z}_{1:k - 1}}} \right)}_{k时刻的位姿估计} = \int { \underbrace{P\left( {{\mathbf{x}_k}|{\mathbf{x}_{k - 1}},{\mathbf{x}_0},{\mathbf{u}_{1:k}},{\mathbf{z}_{1:k - 1}}} \right) }_{k时刻如何受k-1时刻状态的影响} \underbrace {P\left( {{\mathbf{x}_{k - 1}}|{\mathbf{x}_0},{\mathbf{u}_{1:k}},{\mathbf{z}_{1:k - 1}}} \right) }_{k-1时刻的位姿估计}\mathrm{d}\mathbf{x}_{k-1} }
\]
假设相机位姿具有马尔可夫性，可以做如下简化：
- 第一项：k时刻如何受k-1时刻状态的影响即为运动方程：
\[P\left( {{\mathbf{x}_k}|{\mathbf{x}_{k - 1}},{\mathbf{x}_0},{\mathbf{u}_{1:k}},{\mathbf{z}_{1:k - 1}}} \right) = P\left( {{\mathbf{x}_k}|{\mathbf{x}_{k - 1}},{\mathbf{u}_k}} \right),
\]
- 第二项：k-1时刻的位姿状态分布
\[P\left( {{\mathbf{x}_{k - 1}}|{\mathbf{x}_0},{\mathbf{u}_{1:k}},{\mathbf{z}_{1:k - 1}}} \right) = P\left( {{\mathbf{x}_{k - 1}}|{\mathbf{x}_0},{\mathbf{u}_{1:k - 1}},{\mathbf{z}_{1:k - 1}}} \right)
\]
因此改工时指出了如何从k-1时刻的状态分布地推到k时刻的状态分布。
1. 线性系统和KF（线性系统+高斯噪声）
线性高斯系统是指，运动方程\(f\)和观测方程\(h\)可以由线性方程来描述，
\[\left\{ \begin{array}{l} {\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 k=1, \ldots, N \]并且所有的噪声均满足高斯分布(通常假设为白噪声)
\[\mathbf{w}_k \sim N(\mathbf{0}, \mathbf{R}). \quad \mathbf{v}_k \sim N( \mathbf{0}, \mathbf{Q}), \]
卡尔曼滤波推导：
在k-1时刻，记位姿的后验状态估计为\(\mathbf{\hat x}_{k-1}\)，方差为\(\mathbf{\hat P}_{k-1}\)，那么k-1时刻的后验概率满足分布：
\[N(\mathbf{\hat{x}}_k, \mathbf{\hat{P}}_k ) = \eta N\left( {{\mathbf{C}_k}{\mathbf{x}_k},\mathbf{Q}} \right) \cdot N( \mathbf{\check{x}}_k, \mathbf{\check{P}}_k)
\]
\[P(\hat x_{k-1}) =P\left( {{\mathbf{x}_{k - 1}}|{\mathbf{x}_0},{\mathbf{u}_{1:k-1}},{\mathbf{z}_{1:k - 1}}} \right)= N\left( {\mathbf{\hat x_{k-1}}, \mathbf{\hat P}_{k-1} } \right)
\]
结合运动方程，\(\check{\mathbf{x}}_k = {\mathbf{A}_k {{\hat{\mathbf{x}}}_{k - 1}} + {\mathbf{u}_k}}\)，在\(k\)时刻的先验概率分布\(\check x_k\)满足：
\[\check P( x_{k}) = P\left( {{\mathbf{x}_k}|{\mathbf{x}_0},{\mathbf{u}_{1:k}},{\mathbf{z}_{1:k - 1}}} \right) = N\left( {\mathbf{A}_k {{\hat{\mathbf{x}}}_{k - 1}} + {\mathbf{u}_k}, \mathbf{A}_k\hat{\mathbf{P}}_{k-1} {\mathbf{A}_k^T} + \mathbf{R}} \right)
\]
\[\left \{ \begin{array}{l}\check{\mathbf{x}}_k = {\mathbf{A}_k {{\hat{\mathbf{x}}}_{k - 1}} + {\mathbf{u}_k}} \\ \check{\mathbf{P}}_k = {\mathbf{A}_k \hat{\mathbf{P}}_{k-1} { \mathbf{A}^T_k} + \mathbf{R}}\end{array} \right.
\]
根据观测方程，可以计算在 \(\mathbf{ x}_{k}\)状态下，观测的分布为：
\[P\left( {{\mathbf{z}_k}|{\mathbf{x}_k}} \right) = N\left( {{\mathbf{C}_k}{\mathbf{x}_k},\mathbf{Q}} \right)
\]
根据：
假设 \(\mathbf{x}_k \sim N(\mathbf{\hat{x}}_k, \mathbf{\hat{P}}_k )\)， 那么有：
这里为了求解\(\mathbf{\hat{x}}_k, \mathbf{\hat{P}}_k\)， 将分布的指数部分提出来，分别比较二次项和一次项即可（省略\(\eta\) 影响）展开为：
\[\begin{equation}
\small
{\left( {{\mathbf{x}_k} - {{\hat{\mathbf{x}}}_k}} \right)^T}\hat{\mathbf{P}}_k^{ - 1}\left( {{\mathbf{x}_k} - {{\hat{\mathbf{x}}}_k}} \right) = {\left( {{\mathbf{z}_k} - {\mathbf{C}_k} {\mathbf{x}_k}} \right)^T}{\mathbf{Q}^{ - 1}}\left( {{\mathbf{z}_k} - {\mathbf{C}_k}{\mathbf{x}_k}} \right) + {\left( {{\mathbf{x}_k} - {{\check{\mathbf{x}}}_k}} \right)^T}\check{\mathbf{P}}_k^{ - 1}\left( {\mathbf{x}_k - {{\check{\mathbf{x}}}_k}} \right)
\end{equation}
\]
那么对于次项二和一次项分别有：
\[\left \{ \begin{array}{l} \hat{\mathbf{P}}_k^{ - 1} = \mathbf{C}_k^T{\mathbf{Q}^{ - 1}}{\mathbf{C}_k} + \check {\mathbf{P}}_k^{ - 1} \\ - 2\hat {\mathbf{x}}_k^T \hat{\mathbf{P}}_k^{ - 1}{\mathbf{x}_k} = - 2\mathbf{z}_k^T {\mathbf{Q}^{ - 1}}{\mathbf{C}_k}{\mathbf{x}_k} - 2\mathbf{\check {x}}_k^T \mathbf{\check {P}}_k^{ - 1}{\mathbf{x}_k} \end{array} \right.
\]
引入变量 \(\mathbf{K} = \mathbf{\hat{P}}_k \mathbf{C}_k^T \mathbf{Q}^{-1}\) （卡尔曼增益），二次项可以简化为：
\[\mathbf{\hat{P}}_k = ( \mathbf{I} - \mathbf{K} \mathbf{C}_k ) \mathbf{\check{P}}_k
\]
带入一次项，可以得到卡尔曼滤波更新式：
\[\begin{align*}
\begin{array}{ll} {{\mathbf{\hat {x}}}_k} &= {{\hat {\mathbf{P}}}_k} \mathbf{C}_k^T { \mathbf{Q}^{ - 1}}{\mathbf{z}_k} + {{\mathbf{\hat{ P}}}_k}\check {\mathbf{P}}_k^{ - 1}{{\check {\mathbf{x}}}_k}\\ \\ &= \mathbf{K} {\mathbf{z}_k} + \left( {\mathbf{I} - \mathbf{K}{\mathbf{C}_k}} \right){{\mathbf{\check {x}}}_k} \\ \\ &= {{\check {\mathbf{x}}}_k} + \underbrace {\mathbf{K} }_{卡尔曼滤波增益}\underbrace {\left( {\mathbf{z}_k - {\mathbf{C}_k} {\mathbf{\check{x}}_k}} \right)}_{\text 观测方程误差} \end{array} \end{align*}
\]
KF结论：
卡尔曼滤波分为两步，预测和跟新
- 预测，（使用k-1时刻的后验分布估计k时刻的
先验分布）：\[\begin{equation} \left \{ \begin{array}{l} \check{\mathbf{x}}_k = {\mathbf{A}_k {{\hat{\mathbf{x}}}_{k - 1}} + {\mathbf{u}_k}}, \\ \check{\mathbf{P}}_k = {\mathbf{A}_k \hat{\mathbf{P}}_{k-1} { \mathbf{A}^T_k} + \mathbf{R}}. \end{array} \right . \end{equation} \]
更新（使用观测结果更新估计，对于结果进行修正，缩小不确定性）
计算
卡尔曼增益：\[\mathbf{K} = {{\check {\mathbf{P}}}_k} \mathbf{C}_k^T {\left( {{\mathbf{C}_k}{{\check {\mathbf{P}}}_k}\mathbf{C}_k^T + {\mathbf{Q}_k}} \right)^{ - 1}} \]计算
后验概率：\[\left \{ \begin{array}{l} \hat {\mathbf{x}}_k = {{\check {\mathbf{x}}}_k} + \mathbf{K} \left( {\mathbf{z}_k - {\mathbf{C}_k}{\mathbf{\check{x}}_k}} \right)\\ {{\mathbf{\hat {P}}}_k} = \left( {\mathbf{I} - \mathbf{K}{\mathbf{C}_k}} \right) \check{\mathbf{P}}_k. \end{array} \right . \]
2. 非线性系统和EKF
SLAM中运动方程和观测方程通常为非线性的。
假设在k-1时刻，\(P(\hat x_{k-1}) = N\left( {\mathbf{\hat x_{k-1}}, \mathbf{\hat P}_{k-1} } \right)\)，
那么k时刻，运动方程和观测方程在k-1时刻后验概率和k时刻先验概率附近一阶泰勒展开：
\[
\left \{
\begin{array}{l}
{\mathbf{x}_k} \approx f\left( {{{\mathbf{\hat x}}_{k - 1}},{\mathbf{u}_k}} \right) + {\left. {\frac{{\partial f}}{{\partial {\mathbf{x}_{k - 1}}}}} \right|_{{{\mathbf{\hat x}}_{k - 1}}}}\left( {{\mathbf{x}_{k - 1}} - {{\mathbf{\hat x}}_{k - 1}}} \right) + {\mathbf{w}_k} \\
{\mathbf{z}_k} \approx h\left( {{{\mathbf{\check x}}_k}} \right) + {\left. {\frac{{\partial h}}{{\partial {\mathbf{x}_k}}}} \right|_{{{\mathbf{\check x}}_k}}}\left( {\mathbf{x}_k - {{\mathbf{\check x}}_k}} \right) + {\mathbf{n}_k}
\end{array}
\right .
\]
其中两个偏导数分别标记为：
\[\mathbf{F} = \left. {\frac{{\partial f}}{{\partial {\mathbf{x}_{k - 1}}}}} \right|_{{{\mathbf{\hat x}}_{k - 1}}}, \quad \mathbf{H} = \left. {\frac{{\partial h}}{{\partial {\mathbf{x}_k}}}} \right|_{{{\mathbf{\check x}}_k}}
\]
\[\left \{ \begin{array}{l}
{\mathbf{x}_k} = f\left( {{{\mathbf{\hat x}}_{k - 1}},{\mathbf{u}_k}} \right) + {\left. F\right .}
\left( {{\mathbf{x}_{k - 1}} - {{\mathbf{\hat x}}_{k - 1}}} \right) + {\mathbf{w}_k} \\
{\mathbf{z}_k} = h\left( {{{\mathbf{\check x}}_k}} \right) + {\left. H \right.}\left( {\mathbf{x}_k - {{\mathbf{\check x}}_k}} \right) + {\mathbf{v}_k}
\end{array} \right .
\]
那么k时刻的先验和协方差为：
\[\mathbf{\check {x}}_k = f\left( {{{\mathbf{\hat x}}_{k - 1}},{\mathbf{u}_k}} \right), \quad \mathbf{\check{P}}_k = \mathbf{F}\mathbf{\hat{P}}_{k-1} \mathbf{F}^T + \mathbf{R}_k
\]
先验和似然可以描述为：
\[\begin{align*}
\begin{array}{ll} \check P( x_{k}) &= P\left( {{\mathbf{x}_k}|{\mathbf{x}_0},{\mathbf{u}_{1:k}},{\mathbf{z}_{1:k - 1}}} \right)\\ &= N\left( {\check {x}_k, \mathbf{\check{P}}_k} \right) \\ &= N(f\left( {{{\mathbf{\hat x}}_{k - 1}},{\mathbf{u}_k}} \right), \quad \mathbf{F}\mathbf{\hat{P}}_{k-1} \mathbf{F}^T + \mathbf{R}_k) \end{array} \end{align*}
\]
\[ P\left( {{\mathbf{z}_k}|{\mathbf{x}_k}} \right) = N( h\left( {{{\mathbf{\check x}}_k}} \right) + \mathbf{H} \left( {\mathbf{x}_k - {{\mathbf{\check x}}_k}} \right), \mathbf{Q}_k ).
\]
剩余部分按照KF相同步轴推理，可以得出下面结论：
EKF结论：
卡尔曼滤波分为两步，预测和跟新
- 预测，（使用k-1时刻的后验分布估计k时刻的
先验分布）：\[\begin{equation} \left \{ \begin{array}{l} \mathbf{\check {x}}_k = f\left( {{{\mathbf{\hat x}}_{k - 1}},{\mathbf{u}_k}} \right), \\ \mathbf{\check{P}}_k = \mathbf{F}\mathbf{\hat{P}}_{k-1} \mathbf{F}^T + \mathbf{R}_k \end{array} \right . \end{equation} \]
更新（使用观测结果更新估计，对于结果进行修正，缩小不确定性）
计算
卡尔曼增益：\[ \mathbf{K}_k = {\mathbf{\check{P}}_{k}}{\mathbf{H}^T}{\left( {\mathbf{H}{\mathbf{\check P}_k}{\mathbf{H}^T} + {\mathbf{Q}_k}} \right)^{ - 1}} \]计算
后验概率：\[\left \{\begin{array}{l} {{\mathbf{\hat x}}_k} = {{\mathbf{\check x}}_k} + {\mathbf{K}_k}\left( {{\mathbf{z}_k} - h\left( {{\mathbf{\check x}_k}} \right)} \right) \\ {\mathbf{\hat P}_k} = \left( {\mathbf{I} - {\mathbf{K}_k}{\mathbf{H}}} \right) \mathbf{\check{P}}_k. \end{array} \right . \]
EKF缺点：
- 一阶马尔科夫性过于简单，缺少了相对较远帧的关联；
- 可能会发散（要求数据不能有离群点）
- 非线性误差，一次线性化仅仅在后验概率局部有效