代价敏感 代码_NLS 问题之鲁棒代价函数(robust cost function)处理外点

学习SLAM其实就是学状态估计，而在状态估计领域非线性最小二乘问题是非常关键重要的问题，在SLAM中的后端优化中其实就是求解一个非线性最小二乘问题(NLS)，NLS问题有很多种求解方法，经典和常见的有高斯牛顿(Gauss-Newton)，LM算法，DogLeg算法等。另外对于NLS问题中需要最小化的代价函数，一般情况下采用的是误差平方和，而这种代价函数对于外点(outlier)尤其的敏感，由于误差平方和代价函数会进一步放大外点带来的误差，因此对于一个好的数据集来说，加入几个偏差特别严重的外点，会造成估计精确度的严重下降。为了解决这个问题，提出了使用鲁棒代价函数来替代误差平方和函数，存在很多中不同的鲁棒代价函数，如Cauchy, Geman-McClure等，在本篇文章中，将通过编程实现高斯牛顿解法的NLS问题，并且比较使用误差平方和代价函数和Cauchy代价函数对于存在外点系统的估计效果。

---

对于误差平方和代价函数，其数学形式为：

其中

为非线性函数，

为测量协方差矩阵的逆，即信息矩阵，一般来说，对不同时刻的测量是保持不变的。信息矩阵反应了一次测量的置信度如何，信息矩阵越小，对应的协方差矩阵越大，置信度越小，说明这次测量的误差比较大，那么估计系统对这次测量越不看重，其对估计结果的影响就比较小。对于(1)式的代价函数来说，它把每次测量的信息矩阵都看作相同的，也就是说，系统将等同的看待所有的测量，那么，对于一些外点来说，它跟内点的置信度一样高，在加之，平方和后会放大外点误差，因此其对系统估计精确度的影响很高。

现在将误差平方和代价函数替换为鲁棒代价函数，其数学表达形式为：

其中