《视觉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}$$
注意这里的 $\vec{x}$为相机的位姿，可以用 李群 $\mathbf{T}_k$或 李代数 $\exp(\xi^\wedge_k)$表示。假设在 $\vec{x}_k$对路标 $\vec{y}_j$进行一次观测，对应到图像的位置为 $\vec{z}_{k,j}$,则观测方程为：
$$s\vec{z}_{k,j}=\mathbf{K}\exp(\xi^\wedge_k)\vec{y}_j$$
式中 $s$为像素点的距离。式中$\vec{z}_{k,j}$和 $\vec{y}_j$皆为齐次坐标.
不妨假设噪声项 $\vec{w}_k$和 $\vec{v}_{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, \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{z}|\vec{x})$为 似然, $P(\vec{x})$为 先验。此时可用 MAP(Maximize a Posterior, MAP)求解：
$$\vec{x}*_{MAP} = \arg\max~P(\vec{x}|\vec{z}) = \arg\max~ P(\vec{z}|\vec{x})P(\vec{x})$$
进一步地，假设机器人无法获得自己的位置，即 $\vec{x}$没有，则可求解 $\vec{x}$的 最大似然估计(Maximize Likelihood Estimation, MLE):
$$\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{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), \mathbf{Q}_{k,j}))$$
为求解该问题，回忆一个任意的高维高斯分布 $\vec{x} \sim N(\vec{u}, \Sigma)$,其概率密度函数为：
$$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 \left( (2\pi)^N \det(\Sigma) \right) + \frac{1}{2}(\vec{x}-\vec{u}^\top\Sigma^{-1} (\vec{x}-\vec{u}))$$
应用上述公式，第一项与 $\vec{x}$无关可移除，代入slam观测模型结果为：
$$\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}$$

则误差平方项之和为：

$$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).