SLAM学习——非线性优化

1.状态估计问题

对于SLAM经典模型，我们知道是由一个运动方程和一个观测方程构成，如下方程：

$\{\begin{array}{c}{x}_k=f(x_{k-1},u_k)+w_k\\ z_{k,j}=f(y_j,x_k)+v_{k,j}\end{array}$

其中 $x_k$为相机的位姿，$u$ 为输入数据，即为采集到的数据。

假如我们在 $x_k$ 处观测到路标 $y_j$，对应到图像上的像素位置 $z_{k,j}$,那么我们的观测方程可以表示为：

$s{z}_{k,i}=K\ast exp({\epsilon }^{\wedge })\ast y_i$

其中$s$为像素距离，即摄像机坐标系上的一点到像素坐标系的距离，$K$为内参，$exp({\epsilon }^{\wedge })$ 为李代数$se(3)$。接着，在运动方程和观测方程中，我们假设噪声都满足于均值为0的高斯分布：

$w_k\sim N(0,R_k),v_k\sim N(0,Q_{k,j})$

在这些噪声的影响下，我们希望通过带噪声的数据 $z$ 和 $u$来推断位姿 $x$ 和地图 $y$ (以及他们的概率分布)，这构成了一个状态问题，其中有优化方法有：1.扩展卡尔曼滤波器（$EKF$）求解，关心的是当前时刻的状态估计，而对直接的状态没有多加考虑。2.非线性优化，使用所有时刻采集到的数据进行状态估计，被认为优于传统滤波器。

从概率学角度来看，把所有需要估计的变量都放到一个“状态变量中”，即 $\mathit{x}=\begin{Bmatrix} {x_{1},\cdots ,x_{N},y_{1},\cdots ,y_{M}}\end{Bmatrix} $,在已知输入数据$u$和观测数据$z$的条件下,求计算 $x$ 的条件概率分布：$P(x|z,u)$。

当然，如果在运动过程中没有采集数据，且只有一张张图片时，概率分布又可以转换为$P(x|z)$。**如果忽略图像在时间上的联系，看做一堆彼此没有关系的图片，该问题为SfM,即如何从许多图像中重建三维空间结构。**当我们把SLAM看做图像具有时间先后顺序，需要实时求解一个SfM问题。利用贝叶斯法则，有：

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

其中，贝叶斯法则左侧通常成为后验概率，右侧的$P(z|x)$成为似然，另一部分P(x)称为先验。直接求后验是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化，则是可行的。即最大后验概率为最大似然与先验的乘积。

如果当不知道机器人在哪里，那我们则没有了先验，可以求解 x 的最大似然估计($MLE$):

$x_{MLE}^{\ast }=\mathrm{a}\mathrm{r}\mathrm{g}maxP(z|x)$

上述公式可表达为：在什么样的状态下，最可能产生现在观测到的数据。

为转换为最小二乘问题，我们可以得到最大似然估计等价于最小化噪声项的平方（在范数意义下），我们定义数据与估计值之间的误差：

$\{\begin{array}{c}{e}_{v,k}=x_k-f(x_{k-1},u\end{array}$