6.1 状态估计问题
根据前面几章的内容，我们可以根据典型的SLAM模型，得到下面的状态方程和运动方程：
这个式子中，第一个式子描述了运动，也就是当前位置可以通过上一个时刻的位置、传感器的示数以及噪声计算得到，第二个式子则描述了一个状态关系，表示在一个位置上观测一个路标点，在噪声的结果下产生了一个观测结果。对于第二个式子，在第五章中又引入了相机模型的内容，在相机模型中，我们可以具体化这个式子，首先位姿xk可以用变换矩阵或者变换向量来表示，zkj这时作为观测结果，就可以使用像素位置，那么观测方程就可以变成下面的样子：
加上噪声之后必然会产生变化，一般我们用wk和vkj来表示运动方程和观测方程中的噪声，而且一般假设这个噪声都满足均值为零的高斯分布（即标准正态分布），这样引入噪声之后，我们希望利用带有噪声的数据去估计位姿和地图，也就构成了一个状态估计问题。也就是说，如果没有噪声，那么一切都可以标准地计算出来，但是由于噪声的存在，噪声具有不确定性，从而得到的数据也是有不确定性的，进而利用不确定的数据去推测位姿和地图。
这主要是一个概率论的问题，一般都会先将所有待估计的变量存放在一个状态变量中：
也就是说我们要在输入数据u和观测数据z的条件下，去计算状态变量的概率，也就成了一个条件概率，其中u和z就是已知的条件：
这个式子中，最恶心的就是小写字母的含义，这里的小写字母表示的都是向量，这可以看做在产生了现在这个状态的条件下，一开始未知参数是某某状态的概率。
如果忽略时间上的联系，将其看做一堆彼此没有关系的图片，这种问题就转换为了SfM问题，即从许多图像中重建三维空间结构，这种情况下，SLAM可以看作是图像具有先后顺序的，需要实时求解的一个SfM问题，那么利用贝叶斯法则，条件概率可以转换为下面的式子：
左侧一般称为后验概率，右侧的P(Z|X)则称为似然，另一部分P(X)称为先验，直接求后验的分布是困难的，但是求一个状态最优估计，使得在这个状态下，后验概率最大化是可行的，也就是要最大化上面式子中右边的部分。本质上来说，原来是利用观测结果去推测参数，现在换成将参数作为条件去估计结果。
对于贝叶斯公式，由于分母部分与待估计的向量x无关，所以可以直接去掉，所以求解最大后验概率，相当于最大化似然和先验的乘积，进一步来看，这里面的先验P(X)本身表示的是机器人处在某个某个状态的概率，如果不知道这个概率，那么就不知道先验，于是问题就转换为了最大似然估计，即最大化似然：
我们可以这样去理解，既然已经得到了z这个观察真实存在的结果，那么不妨最大化产生这个结果的概率，既然要追求刺激就刺激到底。这就是最大似然估计的直观意义。
这里多插一嘴，上面的两个式子，对应的刚好是最大似然估计和最大后验估计，其实两种估计方法本质上就差一个先验，最大似然估计认为先验都是相等的，从而不去考虑，而最大后验则不这么认为，在一些情况下后验会更加准确一些，二者都是利用结果反推参数：
现在我们确定了，要想得到一个比较准确的预测系数，就需要最大化这个似然函数，根据概率论的知识，我们需要利用负对数去最大化概率密度，对于这个概率分布，本身是一个正态分布，其中均值为观测结果，方差为噪声，也就是下面的式子：
利用概率论的知识展开处理，并通过负对数处理，我们得到最后的优化对象：
对原式的最大化，相当于对负对数的最小化，相当于我们在求一个最小的二次型项，满足这个式子最小化的状态向量就是我们要求的参数：
而这个式子等价于最小化误差的平方，因此对于所有的运动和任意的观测，我们定义数据与估计值的误差：
由此可以得到误差的平方的和：
再从头梳理一遍，我们最根本的目的，是要求一个状态参数，直接求这个参数是困难的，我们转换为求一个最大化的似然函数值，最大化似然函数，在这里具体化为最大化高斯分布，为了方便计算我们转换为最小化负对数，而最小化负对数，等价于最小化误差的平方，也就是优化最后一个式子J(X)。这样就得到了一个总体意义下的最小二乘问题，它的最优解等价于状态的最大似然估计。
直观来讲，由于噪声的存在，当我们把估计的轨迹与地图代入 SLAM 的运动、观测方程中时，它们并不会完美的成立。这时候怎么办呢？我们把状态的估计值进行微调，使得整体的误差下降一些。当然这个下降也有限度，它一般会到达一个极小值。
6.2 非线性最小二乘
前面的一节我们将求参数转换为了求解一个最小二乘问题，这一节主要是看如何解决一个最小二乘问题，首先看一个简单的最小二乘问题：
这个最小二乘问题要求寻找一个向量x，让一个非线性函数的值最小。如果函数f的形式很简单，那么可以直接用解析形式来求，也就是求导然后让导数等于零，直接解出来向量x的最优值，这就和求解一个二元函数的极值是一样的。这样直接解出来的点，可能是极值或者鞍点的值，最后挨个比较就可以得到最值。但是，方程是否容易求解，取决于导函数的形式，在SLAM中，由于我们使用李代数，所以并不能得到一个很容易求解的导函数，也就是说上面这种直接让导函数等于零然后算出极值的情况在SLAM中是不成立的。
对于不方便直接求解的最小二乘问题，我们一般使用迭代的方法，也就是从一个初始值出发，不断更新，使目标函数下降：
所以找极值的过程，就转换为了寻找梯度并下降的过程，其实说白了就是梯度下降法。前面看了那么久的CS231N，里面梯度下降法和梯度下降法的各种优化这里终于派上用场。
在求解最小二乘问题的过程中，关键在于求解增量，求解增量，最直观的方法是用泰勒展开，也就是将目标函数在x附近进行泰勒展开，从而将非线性函数转换为好计算的线性函数：
对于上面的泰勒展开，由于在SLAM中待优化函数是矩阵形式的，所以一阶导数和二阶导数必须都是矩阵形式，所以这里式子中展开成了雅可比矩阵和海森矩阵。我们可以选择保留泰勒展开的一阶项或者二阶项，对应的求解方法则为一阶梯度法或者二阶梯度法。
保留一阶梯度的情况，增量的方向为：
在这个方向上取一个步长，就可以得到一个最快的下降方向。这其实就是梯度下降法，只不过换成了矩阵形式，一些写法发生了变化。这种方法称为最速下降法。
另一方面，保留二阶梯度，那么增量方程就变为：
求右侧关于△x的导数并令其等于零，就可以得到增量的解：
这其实就是牛顿法，利用二阶导去优化，找到一阶导等于零的点，从而找到最小值。
上面的两种方法，各有各的缺点，梯度下降法过于贪心，容易走出锯齿路线，从而增加了迭代次数，对于牛顿法，需要计算海森矩阵，运算过程很麻烦。一般来说使用更加实用的两种方法：高斯牛顿法和列文伯格——马夸尔特方法。
高斯牛顿法是优化算法里最简单的方法，它的思想是将待优化函数进行一阶泰勒展开，我们要寻找下降矢量，也就是解一个线性的最小二乘问题：
对△x求导并令其等于零，得到方程组：
这是一个线性方程组，只有一个变量△x，我们称这个线性方程组为增量方程，将左边的系数定义为H，右边定义为g，上面的式子变为：
也就是说，高斯牛顿法是使用JTJ作为海森矩阵的近似，从而忽略了计算H的过程。求解增量方程是整个优化问题的核心所在，如果可以解出这个方程，那么高斯牛顿法就可以转换为：
从步骤里也可以看出，增量方程的求解是最关键的一步。个人的理解就是找方向的过程中，用一阶导精度不够，用二阶导计算海森矩阵太麻烦，于是高斯牛顿法用一个量，近似海森矩阵，从而在展开到二阶导的情况下计算量又不那么大，从而一定程度上也保证了精度。
对于这个方法，原则上要求近似的矩阵H是可逆的，但实际中H一般是半正定的，也就是说增量会不稳定，导致算法不收敛。严重的情况甚至可能导致目标函数变得更大。
列文伯格——马夸尔特方法则使用了另一种思路。在高斯牛顿法中，我们使用近似二阶泰勒展开，只能在近似点附近才能有很好的效果，所以我们可以考虑加一个信赖区域，不让它太大而使得近似不准确。这种方法也被称为信赖区域法，区域内近似有效，出了这个区域，近似就无效了。
对于这个区域，我们可以使用下面的式子去衡量泰勒近似的效果：
ρ的分子是实际函数下降的值，分母是近似模型下降的值。如果ρ接近于1，则近似是好的。如果ρ太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果ρ比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。
于是框架可以优化为下面的流程：
这里近似范围扩大的倍数和阈值都是经验值，可以替换成别的数值。
标签：似然,函数,求解,非线性,近似,优化,二乘,式子 来源： https://blog.csdn.net/weixin_43849505/article/details/121730482
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。