本文共 2106 字，大约阅读时间需要 7 分钟。
重磅干货，第一时间送达
「本文对SLAM的状态估计问题中常见的最小二乘问题解法作了整理，作为入门学习笔记。状态估计尤其是无约束非线性最小二乘问题是SLAM前端和后端中经常遇到的问题」
一、状态估计中的最小二乘问题来源
根据经典的SLAM模型：
运动方程： 
观测方程： 
其中x为相机位姿，u为运动输入，w为噪声； 
为在位姿 
处对路标 
进行观测后，对应到图像上的像素位置，v为噪声。
我们通常假设噪声w,v服从高斯分布： 
, 
，即噪声分布的均值为0，协方差矩阵为R，Q。
在SLAM的一般情况下，我们可以获得相机观测数据和运动输入数据，然后根据数据对相机(也即机器人本体)进行位姿估计，将以上过程表示为数学形式即：
可见，求最大后验概率问题经过贝叶斯法则，变成了求最大化似然概率和先验概率的乘积：
进一步当我们连先验概率都不知道时，问题就变成了求解最大似然估计：
这就得到了状态估计问题的最终数学形式，下面考虑求解此最大似然估计。
根据观测模型： ，因为噪声服从高斯分布，所以：
对上式的概率密度函数取负对数：
考虑到对数函数的单调性，求原函数的最大值转化为求负对数最小值，并且注意到上式右边第一项与x无关，因此对状态的最大似然估计转换为了最小化右侧的二次型项。即：
其实该式就是最小化误差项的二次型，现在给出误差的定义：
当考虑实际中批量处理的数据时，由于各个时刻的输入和观测相互独立，所以：
再次利用负对数将乘积变为求和，求对批量数据的最小化估计误差即是求批量数据的最大似然估计，结合上式和 
式，可以得到：
这样我们就由SLAM的状态估计，得到了最小二乘问题。通过上面简短的介绍，应该对什么事SLAM的 状态估计，以及如何将状态估计转换为最小二乘问题有了了解。
二、非线性最小二乘问题及四种常用解法
来看一个这样的最小二乘问题：
如果 
不是简单的线性函数，那么此最小二乘问题就变为了非线性最小二乘问题，对于不方便求解的非线性最小二乘问题我们一般采用迭代的方式进行计算，思路是：从一个初始值出发，不断优化当前的优化变量，使得目标函数下降。显然初始值给的越好，优化效果会越好。步骤为：
给定某初始值
对于第k次迭代，寻找一个增量
，使得 
达到最小值若
足够小，则停止迭代否则，另
,返回第2步。
根据上面的求解思路，求解导函数为零的问题变成了一个不断寻找下降增量 的过程。
下面就来看看寻找增量 的四种方法！
1、最速下降法
在 处将目标函数进行泰勒展开：
上式中的 
是F函数的一阶导数，也叫雅克比矩阵； 
是F的二阶导数，也叫海塞矩阵。
我们选择保留一阶导数，忽略二阶导数时，增量可取为步长为 
的反向的梯度，这样就可以保证函数下降：
这种求解增量 的方法就称为最速下降法。
2、牛顿法
如果我们保留此式 的二阶导数，就可以得到增量方程：
对右侧式子求关于 的导数，并令它为0，可以得到：
这样也求得了增量 ，这种方法就叫做牛顿法。
3、高斯牛顿法
这里我们直接对 进行泰勒展开(注意不是目标函数F，而是f，这也是和牛顿法的区别)：
根据前面的思路，我们想寻找增量 ，使得目标函数最小，为了求增量需要解如下的最小二乘问题：
对上式右侧求关于 
的导数，并且令其为零：
得到的上式就是关于增量 的线性方程组，又称高斯牛顿方程。此求解方法即高斯牛顿法。
4、LM法(列文伯格-马夸尔特法)
在高斯牛顿法中采用的近似泰勒展开，只能在展开点附近有较好的近似效果。于是我们考虑给 添加一个范围(信赖区域)，并认为在此区域内近似有效，超出区域则会出问题。
下面定义一个指标 
描述近似的好坏：
如果 接近与显然近似是好的，否则，近似是不佳的。在高斯牛顿法基础上加上信赖区域，求解的最小二乘问题就变成了：
其中 
是信赖区域半径；D是系数矩阵，若D为单位阵，则相当于把增量 约束在一个球内，马夸尔特提出取为非负数对角阵，实际中常用 
的对角元素平方根。
利用拉格朗日乘子法，把约束项放到目标函数中，构造拉格朗日函数：
对拉格朗日函数求关于增量 的导数，并令为零:
这样就得到了求解增量 的方程，此方法就称为LM法，即列文伯格-马夸尔特法。
5、四种方法的优劣对比
下面总结以上四种方法的优劣对比：
最速下降法：直观上将本算法十分简单，直接按照梯度的反方向下降即可；缺点是过于贪心，容易呈锯齿状下降，从而增加迭代次数。
牛顿法：相对而言也非常直观，同时由于引入了二阶导数，可以处理一阶导为0的情况；但缺点是二阶导数具有非常大的计算量。
高斯牛顿法：在牛顿法的基础上进行了一定程度的简化，使用
代替海塞矩阵，避免了二阶导数的计算；缺点在于 很容易病态，导致无法得到正确的结果。LM法：通过引入阻尼项使得
不那么容易病态，并且可以通过调整阻尼完成在梯度法和牛顿法之间切换；缺点不太清楚。
上述内容，如有侵犯版权，请联系作者，会自行删文。
