系列文章目录
·【算法系列】卡尔曼滤波算法
·【算法系列】非线性最小二乘求解-直接求解法
文章目录
系列文章
文章目录
前言
算法推导
总结
前言
SLAM问题常规的解决思路有两种，一种是基于滤波器的状态估计，围绕着卡尔曼滤波展开；另一种则是基于图论（因子图）的状态估计，将SLAM问题建模为最小二乘问题，而且一般是非线性最小二乘估计去求解。
非线性最小二乘有多种解法，本篇博客介绍直接求解的方法。
算法推导
非线性最小二乘的一般形式如下：
其中是非线性函数，
表示协方差矩阵
首先对f(x)进行线性化近似，这样就能把非线性最小二乘转化成线性最小二乘，然后直接求解线性方程就可以得到解。
一般线性化的方法都是使用泰勒展开进行线性化，对精度要求不高时尝尝采用一阶泰勒展开，在x0处进行泰勒展开：
将展开后的式子带入到目标函数，并将协方差矩阵开方后放到平方运算中得到下式：
将前面可以求出的常数项整体用表示，后面待求项
的系数用
表示整理一下得到：
在理想情况下，可以取到最小值0，就相当于其中的每一项
取到最小值0，其实就是解下面这个线性方程组：
在平常求解线性方程组时直接两边左乘A的逆就求出解，但A一般不可逆，这时往往求该方程组的数值解，一般使用的方法时Cholesky分解和QR分解。
Cholesky分解方法：
Cholesky分解是在原来的式子两边左乘A的转置，然后利用Cholesky分解将分解成n×n的上三角矩阵R的乘积
，再令新的变量
，就得到了新的线性方程组
因为R是上三角矩阵，其转置后为下三角矩阵，第一行只有一个变量，行数越往下变量越多，于是从上到下很容易依次解出y，然后再解线性方程组，而这个方程组最后一行只有一个变量，自下而上也可以很容易的解出x，这样原方程组就可以求解出来了。
QR分解：
QR分解比Cholesky分解更加精准和稳定，利用QR分解将A分解为Q矩阵和R矩阵的乘积，其中Q是正交矩阵，R是上三角矩阵，然后在方程组两边同时左乘Q的逆对方程进行化简，如下：
R是上三角矩阵，很容易就能求出x。
总结
在实际的SLAM问题中，非线性最小二乘很难找到合适的线性化方法，仅仅使用一阶泰勒展开进行近似有时会产生较大的误差；而且代价函数的误差往往也不能最小化到0值，所以直接求解的方法在现实中的应用存在诸多问题。
所以下面的博客将介绍优化的方法求解非线性最小二乘问题，这种方法在实践中是比较常用的。
