《视觉SLAM十四讲》学习笔记-状态估计问题

最大后验与似然

经典slam模型可表示为：

{\begin{cases} {\vec{x}}_{k} = f ({\vec{x}}_{k - 1}, {\vec{u}}_{k}) + {\vec{w}}_{k} \\ {\vec{z}}_{k, j} = h ({\vec{y}}_{j}, {\vec{x}}_{k}) + {\vec{v}}_{k, j} \end{cases}

$\begin{cases} \vec{x}_k = f(\vec{x}_{k-1}, \vec{u}_k) + \vec{w}_k\\ \vec{z}_{k,j} = h(\vec{y}_j, \vec{x}_k) + \vec{v}_{k, j} \end{cases}$
注意这里的

\vec{x}

$\vec{x}$ 为相机的位姿，可以用李群

T_{k}

$\mathbf{T}_k$ 或 李代数

\exp (ξ_{k}^{\land})

$\exp(\xi^\wedge_k)$ 表示。假设在

{\vec{x}}_{k}

$\vec{x}_k$ 对路标

{\vec{y}}_{j}

$\vec{y}_j$ 进行一次观测，对应到图像的位置为

{\vec{z}}_{k, j}

$\vec{z}_{k,j}$ ,则观测方程为：

s {\vec{z}}_{k, j} = K \exp (ξ_{k}^{\land}) {\vec{y}}_{j}

$s\vec{z}_{k,j}=\mathbf{K}\exp(\xi^\wedge_k)\vec{y}_j$
式中

s

$s$ 为像素点的距离。式中

{\vec{z}}_{k, j}

$\vec{z}_{k,j}$ 和

{\vec{y}}_{j}

$\vec{y}_j$ 皆为齐次坐标.
不妨假设噪声项

{\vec{w}}_{k}

$\vec{w}_k$ 和

{\vec{v}}_{k, j}

$\vec{v}_{k, j}$ 满足零均值高斯分布：

{\vec{w}}_{k} \sim N (0, R_{k}), {\vec{v}}_{k, j} \sim N (0, Q_{k, j})

$\vec{w}_k \sim N(0, \mathbf{R}_k), ~~\vec{v}_{k, j} \sim N(0, \mathbf{Q}_{k,j})$
先把所有的待估计变量放在一个 状态变量中：

\vec{x} = {\vec{x}}_{1}, \dots, {\vec{x}}_{N}, {\vec{y}}_{1}, \dots, {\vec{y}}_{M}

$\vec{x} = {\vec{x}_1, \cdots, \vec{x}_N, \vec{y}_1, \cdots, \vec{y}_M}$

对机器人状态的估计，转变为求已知输入数据 $\vec{u}$ 和观测数据 $\vec{z}$ 和条件下，计算状态 $\vec{x}$ 的条件概率： $P(\vec{x}|\vec{z}, \vec{u})$ .

当没有运动测量的传感器，即不存在 $\vec{u}$ 时，上述问题转变为Structure from Motion(SfM)问题。此时，利用贝叶斯法则有：

P (\vec{x} | \vec{z}) = \frac{P (\vec{z} | \vec{x}) P (\vec{x})}{P (\vec{z})} \propto P (\vec{z} | \vec{x}) P (\vec{x})

$P(\vec{x}|\vec{z}) = \frac{P(\vec{z}|\vec{x})P(\vec{x})}{P(\vec{z})}\propto P(\vec{z}|\vec{x})P(\vec{x})$
上式中，

P (\vec{z} | \vec{x})

$P(\vec{z}|\vec{x})$ 为似然,

P (\vec{x})

$P(\vec{x})$ 为先验。此时可用 MAP(Maximize a Posterior, MAP)求解：

\vec{x} *_{M A P} = \arg max P (\vec{x} | \vec{z}) = \arg max P (\vec{z} | \vec{x}) P (\vec{x})

$\vec{x}*_{MAP} = \arg\max~P(\vec{x}|\vec{z}) = \arg\max~ P(\vec{z}|\vec{x})P(\vec{x})$
进一步地，假设机器人无法获得自己的位置，即

\vec{x}

$\vec{x}$ 没有，则可求解

\vec{x}

$\vec{x}$ 的 最大似然估计(Maximize Likelihood Estimation, MLE):

\vec{x} *_{M L E} = \arg max P (\vec{z} | \vec{x})

$\vec{x}*_{MLE} = \arg\max~ P(\vec{z}|\vec{x})$
上式可理解为”在什么样的状态下，最可能产生现在观测到的数据”.

最小二乘问题：

对一次观测：

{\vec{z}}_{k, j} = h ({\vec{y}}_{j}, {\vec{x}}_{k}) + {\vec{v}}_{k, j}

$\vec{z}_{k,j} = h(\vec{y}_j, \vec{x}_k) + \vec{v}_{k, j}$
并且有假设

{\vec{v}}_{k, j} \sim N (0, Q_{k, j})

$\vec{v}_{k, j} \sim N(0, \mathbf{Q}_{k,j})$ ,条件概率为：

P ({\vec{z}}_{k, j} | {\vec{x}}_{k}, {\vec{y}}_{j}) = N (h ({\vec{y}}_{j}, {\vec{x}}_{k}), Q_{k, j}))

$P(\vec{z}_{k,j} |\vec{x}_k, \vec{y}_j ) = N( h(\vec{y}_j, \vec{x}_k), \mathbf{Q}_{k,j}))$
为求解该问题，回忆一个任意的高维高斯分布

\vec{x} \sim N (\vec{u}, Σ)

$\vec{x} \sim N(\vec{u}, \Sigma)$ ,其概率密度函数为：

P (\vec{x}) = \frac{1}{\sqrt{(2 π)^{N} det (Σ)}} \exp (- \frac{1}{2} (\vec{x} - {\vec{u}}^{⊤} Σ^{- 1} (\vec{x} - \vec{u})))

$P(\vec{x}) = \frac{1}{\sqrt{(2\pi)^N \det(\Sigma)} } \exp\left(-\frac{1}{2} (\vec{x}-\vec{u}^\top \Sigma^{-1}(\vec{x}-\vec{u})) \right)$
两边取负对数后变为：

- \ln (P (\vec{x})) = \frac{1}{2} \ln ((2 π)^{N} det (Σ)) + \frac{1}{2} (\vec{x} - {\vec{u}}^{⊤} Σ^{- 1} (\vec{x} - \vec{u}))

$-\ln (P(\vec{x})) = \frac{1}{2}\ln \left( (2\pi)^N \det(\Sigma) \right) + \frac{1}{2}(\vec{x}-\vec{u}^\top\Sigma^{-1} (\vec{x}-\vec{u}))$
应用上述公式，第一项与

\vec{x}

$\vec{x}$ 无关可移除，代入slam观测模型结果为：

\vec{x} * = \arg min (({\vec{z}}_{k, j} - h ({\vec{x}}_{k}, {\vec{y}}_{i}))^{⊤} Q_{k, j}^{- 1} ({\vec{z}}_{k, j} - h ({\vec{x}}_{k}, {\vec{y}}_{i})))

$\vec{x}* = \arg\min \left( (\vec{z}_{k,j} - h(\vec{x}_k, \vec{y}_i) )^\top\mathbf{Q}^{-1}_{k,j} (\vec{z}_{k,j} - h(\vec{x}_k, \vec{y}_i) ) \right)$
定义数据与误差之间误差为：

{\begin{cases} {\vec{e}}_{v, k} = {\vec{x}}_{k} - f ({\vec{x}}_{k - 1}, {\vec{u}}_{k}) \\ {\vec{e}}_{y, j, k} = {\vec{z}}_{k, j} - h ({\vec{x}}_{k}, {\vec{y}}_{j}) \end{cases}

$\begin{cases} \vec{e}_{v,k} = \vec{x}_k - f(\vec{x}_{k-1}, \vec{u}_k)\\ \vec{e}_{y,j,k} = \vec{z}_{k,j} - h(\vec{x}_{k}, \vec{y}_j) \end{cases}$

则误差平方项之和为：

J (\vec{x}) = \sum_{k} {\vec{e}}_{v, k}^{⊤} R_{k}^{- 1} {\vec{e}}_{v, k} + \sum_{k} \sum_{j} e_{y, k, j}^{⊤} Q_{k, j}^{- 1} e_{y, k, j}

$J(\vec{x}) = \sum_{k}\vec{e}^\top_{v,k} \mathbf{R}^{-1}_k \vec{e}_{v,k} + \sum_{k}\sum_{j} e_{y,k,j}^\top\mathbf{Q}^{-1}_{k,j}e_{y,k,j}$
此式即为总体意义下的 最小二乘问题(Least Square Problem).