第1章 状态估计问题

  对于运动方程、 观测方程：

  位姿由变换矩阵描述， 然后使用李代数进行优化， 观测方程由相机成像模型给出，其中内参随机固定，而外参是相机的位姿。

  但是受到噪声的影响， 两个方程只能近似成立， 因此需要在有噪声的数据中进行准确的状态（位姿、地图）估计。

1.1 状态估计问题

  对于运动方程、观测方程，假设假设两个噪声均服从高斯分布：

$$
\begin{cases} \boldsymbol{x}_k=f(\boldsymbol{x}_{k-1},\boldsymbol{u}_k)+\boldsymbol{\omega}_k \
\boldsymbol{z}_{k,j}=h(\boldsymbol{y}_j,\boldsymbol{x}_k)+\boldsymbol{v}_{k,j}
\end{cases}，k=1,...N;j=1,...,M
$$

$$ \boldsymbol{\omega}_k～N(0,\boldsymbol{R}_k)，\boldsymbol{v}_k～N(0,\boldsymbol{Q}_{k,j}) $$

  利用带噪声的数据$ \boldsymbol{z} $和$ \boldsymbol{u} $推断位姿$ \boldsymbol{x} $和地图$ \boldsymbol{y} $（以及它们的概率分布）。利用概率表示为求条件概率分布（将待估计状态记作一个状态变量）：

$$
\begin{cases}
P(\boldsymbol{x}|\boldsymbol{z},\boldsymbol{u}) \
\boldsymbol{x}={\boldsymbol{x}_1,...,\boldsymbol{x}_N,\boldsymbol{y}_1,...,\boldsymbol{y}_M}
\end{cases}
$$

  如果没有测量运动的传感器，只有图像，则条件概率分布简化为$ P(\boldsymbol{x}|\boldsymbol{z}) $，根据贝叶斯法则，有：

$$
P(\boldsymbol{x}|\boldsymbol{z})=\frac{P(\boldsymbol{z}|\boldsymbol{x})P(\boldsymbol{x})}{p(\boldsymbol{z})} \propto P(\boldsymbol{z}|\boldsymbol{x})P(\boldsymbol{x})
$$

• 后验概率：$ P(\boldsymbol{x}|\boldsymbol{z}) $
• 似然：$ P(\boldsymbol{z}|\boldsymbol{x}) $
• 先验：$ P(\boldsymbol{x}) $

1.1.1 最大后验

  直接求后验概率分布较为困难。

  方法：求一个最优估计，使得在该状态下，后验概率最大化（Maximize a Posterior，MAP）：

$$ {\boldsymbol{x}^*}_{MAP}=argmax(P(\boldsymbol{x}|\boldsymbol{z}))=argmax(P(\boldsymbol{z}|\boldsymbol{x})P(\boldsymbol{x})) $$

1.1.2 最大似然

  如果不知道机器人大概在什么位置，即没有先验，则问题为求最大似然估计（Maximize Likelihood Estimation，MLE）：

$$ \boldsymbol{x}^*_{MLE}=argmax(P(\boldsymbol{z}|\boldsymbol{x})) $$

似然：在现在位姿下，可能产生怎样的观测数据。

最大似然：在什么样的状态下，最可能产生现在的观测数据（观测数据已知）。

1.2 状态估计的概率解释

  后端优化中，通常考虑一段时间内（或所有时间内）的状态估计问题，而且不仅使用过去的信息更新自己的状态，也会用未来的信息来更新自己

1.2.1 实际中的运动/观测方程

观测方程多于运动方程：

  实际中， 机器人在一个位置通常只能看到一小部分路标。而且，由于视觉或激光 SLAM特征点数量都多，所以实际当中观测方程数量会远远大于运动方程的数量。

如果没有运动方程：

  如果没有测量运动的装置， 就没有明确的运动方程。 此时可认为没有运动方程，或假设机器人不动，或机器人匀速运动。
  此时， 后端优化问题由多个观测方程组成。类似于SfM（Structure from Motion）问题，即通过一组图像来恢复运动和结构。与 SfM 中不同的是， SLAM 中的图像有时间上的先后顺序，而 SfM 中允许使用完全无关的图像。

1.2.2 概率解释

  令$ \boldsymbol{x}_k $代表$ k $时刻所有未知量，包含当前时刻的位姿、$ m $个路标点。且$ \boldsymbol{z}_k $表示$ k $时刻所有的观测。

$$ \boldsymbol{x}_k \triangleq {\boldsymbol{x}_k,\boldsymbol{y}_1,...,\boldsymbol{y}_m} $$

  两个方程改写为如下：

$$
\begin{cases}
\boldsymbol{x}_k=f(\boldsymbol{x}_{k-1},\boldsymbol{u}_k)+{\omega}_k \
\boldsymbol{z}_k=h(\boldsymbol{x}_k)+\boldsymbol{v}_k
\end{cases}，
k=1,...,N
$$

  利用过去$ 0-k $中的数据来估计当期时刻的状态分布：

$$ P(\boldsymbol{x}_k|\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k}) $$

  根据贝叶斯法则，有：

$$ P(\boldsymbol{x}_k|\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k}) \propto P(\boldsymbol{z}_k|\boldsymbol{x}_k) P(\boldsymbol{x}_k|\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k-1})$$

  右式第一项为似然，由观测方程给定。第二项为先验，当前状态由过去所有状态估计得到，可将先验依次向前一时刻展开。

1.2.3 两类方法

滤波方法（EKF 等）：

  马尔可夫性：$ k $时刻状态只与$ k-1 $时刻状态有关。将先验按照$ k-1 $时刻展开如下：

$$
P(\boldsymbol{x}_k|\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k-1})=\int P(\boldsymbol{x}_k|\boldsymbol{x}_{k-1},\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k-1}) P(\boldsymbol{x}_{k-1}|\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{1:k-1})d{\boldsymbol{x}_{k-1}}
$$

非线性优化方法：

  $ k $时刻状态与之前所有状态有关。可将先验继续往前展开。