slam中最小二乘问题描述

最新推荐文章于 2024-04-03 11:13:13 发布

wanna where

最新推荐文章于 2024-04-03 11:13:13 发布

阅读量393

点赞数

文章标签： slam

本文链接：https://blog.csdn.net/weixin_43911366/article/details/114932015

版权

1 问题描述

根据从零手写VIO第3讲的内容，把推导缕一下，根据以后学习的加深，再进行删改。

定义一个最小二乘问题，损失函数为F(x)。

殘差函数f(x)进行泰勒展开，表示如下：
在这里插入图片描述
那如何处理这个最小二乘问题呢，首先进行泰勒展开：

其中 J 和 H 分别为损失函数 F(x) 对变量 x 的一阶导和二阶导矩阵。

2 对上述最小二乘问题的求解

分为直接求解（线性最小二乘）和迭代下降法（线性和非线性最小二乘）。
由于slam问题中一般都是对于非线性问题的求解，因此一般采用迭代下降法。

最常用的迭代下降法有：最速下降法、牛顿法、Gauss-Newton法、LM法。
在slam中，一般采用Gauss-Newton法与LM法优化。
但是Gauss-Newton法有一个缺点，这里详细说明一下。Gauss-Newton法的损失函数表示可以写为：
在这里插入图片描述
其中，小 l(x)代表上述殘差函数的吧表达，J为殘差函数f(x)的雅克比矩阵。此式就是将损失函数转化为殘差函数表达。这样损失函数就近似成了一个二次函数，并且如果殘差函数的雅克比是满秩的，则 JTJ 正定，损失函数有最小值。
另外：
在这里插入图片描述
令上述公式的一阶导等于 0，得到：

因此高斯牛顿法无法保证JTJ 正定，即殘差函数f(x)的雅克比矩阵J若不满秩，则无法迭代。
因此Levenberg (1944) 和 Marquardt (1963) 先后对高斯牛顿法进行了改进，求解过程中引入了阻尼因子，就是LM算法：
在这里插入图片描述
只要保证阻尼因子大于0，就能保证正定，保证可以迭代下去。
另外，阻尼因子是在变化的，有一个初始值选择问题以及更新策略问题。这里不再详细赘述。