SLAM系统保证精度的主要手段是优化，本文介绍了优化方程的构建过程和几种常见的非线性优化方法。

1. 状态估计问题

1.1 最大似然概念

极大似然估计，通俗理解来说，就是利用已知的样本结果信息，反推最具有可能（最大概率）导致这些样本结果出现的模型参数值。

对于函数$p(x|\theta)$输入有两个：$x$表示具体观测数据，$\theta$表示模型的参数。如果$\theta$是确定的，$x$是变量，这函数叫做概率函数(probability function)，他表述了在一个模型下，样本点$x$出现的概率是多少。

反之，如果$x$是确定的，$\theta$是变量，这个函数叫做似然函数(likelihood function)，它描述了在不同的模型下，样本点$x$出现的概率是多少。

举例说明：假如有一个罐子，里面有黑白两种颜色的球，数目多少不知，两种颜色的比例也不知。现在我们可以每次任意从已经摇匀的罐中拿一个球出来，记录球的颜色，然后把拿出来的球再放回罐中。假如在前面的一百次重复记录中，有七十次是白球，请问罐中白球所占的比例最有可能是多少？

要求这个式子的最大值，对他求导即可，得到的结果是：p=0.7时，结果最大，符合我们的日常逻辑认知。

1.2 SLAM中的最大后验与最大似然

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

其中$\mathit{x}{k}$为相机位姿，$u_k$为传感器采集到的数据；$z{k,j}$为图像的像素位置，$y_j$为观测到的路标。

在运动方程和观测方程中，假设假设噪声都满足于均值为0的高斯分布：

因此这个问题变成了：我们希望通过带噪声的数据 z 和 u来推断位姿 x 和地图 y(以及他们的概率分布)，在已知传感器采集数据u和图像数据z的情况下，x的条件概率为：

如果没有传感器采集数据，只有一张张图片时，就变成了：

其中$P(x|z)$为后验概率，$P(z|x)$为似然，$P(x)$为先验概率。直接求后验概率比较困难，我们可以先求出一个最有可能的模型（$P(z|x)$最大），与先验概率相结合，就可以求出在这个模型下，以后x的可能结果。即：最大后验概率=最大似然×先验概率。

当不知道机器人在哪里时，我们缺乏先验概率，因此需要直接求解最大似然估计(MLE)：

其中$argmax F(x)$返回的是函数F取得最大时的$x$

1.3 最小二乘法的引出

多变量的高维高斯分布$x\sim N(u,\Sigma)$它的概率密度函数展开形式为：

取负对数可知：

对于观测模型${z}{k,j}=f(\mathit{y}{j},\mathit{x}{k})+\mathit{v}{k,j} $来说，由于我们设置了噪声项$vk\sim N(0,Q{k,j})$，因此观测到的数据的条件概率是：

按照高维高斯模型，第一项和x无关，因此我们可以忽略。最后问题转化为求第二项取最小时，x的取值。

我们定义数据与估计值之间的误差：

并求该误差的平方和：

这就得到了一个总体意义下的最小二乘问题（Least Square Problem）。我们明白它的最优解等价于状态的最大似然估计。

2.非线性最小二乘

对于一个简单的最小二乘问题${min}\frac{1}{2}\left | f(x) \right |^{2}_{2}$，要求取他的最小值，只需要求取导数即可，通过比较这些极小值，能得到全局最优解，这样一般适用于线性最小二乘。对于非线性最小二乘，最好采用迭代的方式求取，但只能得到局部最优解(极小值)。其步骤如下:

  1. 给定某个初值
  2. 对于第$k$次迭代，寻找增量$\Delta x_k$，使得$||f(x_k+\Delta x_k)||^2$达到最小。
  3. 若$\Delta xk$足够小，则停止；否则令$x{k+1}=x_k+\Delta x_k$返回第2步

所以现在的问题时，如何选取$\Delta x_k$

2.1 一阶和二阶梯度法

求解增量最直观的方式是将目标函数在 x 附近进行泰勒展开，得到：

这里的$J$是$||f(x)||^2$关于x的一阶导数（雅克比矩阵），H则是二阶导数（黑塞矩阵），其中黑塞矩阵是对称矩阵。

其中$J(x)$是一阶梯度，$\frac{1}{2}\Delta x^TH$是二阶梯度，我们可以选择保留泰勒展开的一阶或二阶项，对应的求解方法则为一阶梯度或二阶梯度法。

首先考虑只保留一阶梯度。梯度下降的时候我们需要确定下降$\Delta x$的方向，这就是所谓梯度下降法，下降的方向就是梯度的反方向$-J^T(x)$

在$y$是标量的情况下，$J^T$便是梯度，在$y$是高维的情况下，$J^T$每个列向量都是对应的$y_i$的梯度。

同时还需要添加一个步长$\lambda$，有了方向和步长，就可以的到$\Delta x$顺利求取局部最小值了。步长通过公式求解，越接近目标值，步长越小，前进越慢。这种方法被称为最速下降法。

另一方面，如果保留二阶梯度信息，那么增量方程为：

我们知道当$f’(x)=0$时函数有极小值，对右侧关于$\Delta x$求导就得到了增量解：

化简以后可得：

矩阵求导公式：

黑塞矩阵是一个对称矩阵$H^T=H$

这种方法又称为牛顿法。由于泰勒展开之后函数变成了多项式， 所以求解增量时只需解线性方程即可，避免了直接求导函数为零这样的非线性方程的困难。

牛顿法与最速下降法的区别在于：牛顿法是二阶收敛，最速下降法是一阶收敛。最速下降法每次只从你当前所处位置选一个坡度最大的方向走一步，在局部进行下降，然后步步逼近极值，往往是走之字型的。

牛顿法在二阶导数的作用下，从函数的凸性出发，直接搜索怎样到达极值点。在选择方向时，不仅会考虑坡度是否够大，还会考虑你走了一步之后，坡度是否会变得更大。所以，可以说牛顿法比最速下降法看得更远一点，能更快地走到最底部。如图，红色代表牛顿法，绿色代表最速下降法。

这两种方法的缺点在于：最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。牛顿法需要计算H矩阵，十分繁琐。牛顿法则需要计算目标函数的 H 矩阵，这在问题规模较大时非常困难。

2.2 高斯牛顿法 GN优化

高斯牛顿法，它的思想是将$f(x)$进行泰勒展开（目标函数不是$f(x)$的二范数，和2.1的内容前提相区别）：

根据前面的框架，需要求得下降矢量$\Delta x$使得$||f(x+\Delta x)||^{2}$最小，如果将$\Delta x$看做变量，就会得到如下的最小二乘问题：

目标函数展开后可得：

对目标函数的$\Delta x$求导，并令其等于0：

最后得到：

$||A||_2=\sqrt \lambda$其中$\lambda$是谱范数，即$A^TA$的最大特征值

这里认为$f(x)^TJ(x)\Delta x$是对称矩阵

这个方程称之为增量方程，也称之为高斯牛顿方程。将左边的系数设为$H$右边的系数设为$g$，则有：

注意这里并不是真正的黑塞矩阵，而是利用$J^TJ$代替了牛顿法中的黑塞矩阵，简化了计算。

求解增量方程是整个优化问题的核心所在。其步骤可以写为：

  1. 给定某个初值
  2. 对于第$k$次迭代，求出当前的雅克比矩阵$J(x_{k})$和误差$f(x_k)$
  3. 求解增量方程$H\Delta x=g$
  4. 若$\Delta xk$足够小，则停止；否则令$x{k+1}=x_k+\Delta x_k$返回第2步

缺点在于$H$是半正定矩阵，可能是奇异矩阵(不可逆)导致算法不收敛，且即使可逆，如果步长太长可能导致局部近似不准确，甚至不收敛。

2.3 列文伯格-马夸尔特法 LM优化

高斯牛顿法只能在展开点附近有比较好的效果，所以我们可以给$\Delta x$添加一个信赖区域(Trust Region)。

那么如何确定这个信赖区域的范围呢？一个比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定这个范围：如果差异小，我们就让范围尽可能大；如果差异大，我们就缩小这个近似范围。因此，考虑使用：

分子是实际函数下降的值，分母是近似模型下降的值。如果 ρ 接近于 1，则近似是好的。如果 ρ 太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 ρ 比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。

具体算法步骤如下：

这里近似范围扩大的倍数和阈值都是经验值，可以替换成其他数值。上述约束中相当于把增量限定在半径为u的球里面，认为在球内的才有效。带上D后成为椭圆。

在上述求解中，由于是有约束优化，可以利用拉格朗日乘子将其转化为一个无约束优化问题(其中λ为拉格朗日乘子)：

展开后可得：

当参数λ较小时，H占主导地位，说明二次近似模型在该范围内是比较好的，该方法接近于高斯牛顿法。当参数λ较大时，λ所在项接近于一阶梯度下降法。该方法可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题。