【算法系列】非线性最小二乘求解-直接求解法

系列文章目录

·【算法系列】卡尔曼滤波算法

·【算法系列】非线性最小二乘求解-直接求解法

·【算法系列】非线性最小二乘求解-梯度下降法

·【算法系列】非线性最小二乘-高斯牛顿法 

·【算法系列】非线性最小二乘-列文伯格马夸尔和狗腿算法 

文章目录

系列文章

文章目录

        前言

        算法推导

        总结


前言

SLAM问题常规的解决思路有两种,一种是基于滤波器的状态估计,围绕着卡尔曼滤波展开;另一种则是基于图论(因子图)的状态估计,将SLAM问题建模为最小二乘问题,而且一般是非线性最小二乘估计去求解。

非线性最小二乘有多种解法,本篇博客介绍直接求解的方法。


算法推导

非线性最小二乘的一般形式如下:

\underset{x}{min}\sum \left \| y_{i}-f(x_{i}) \right \|^{2}_{\sum _{i}^{-1}}

其中f(x_{i})是非线性函数,\sum{_{i}^{-1}}表示协方差矩阵

首先对f(x)进行线性化近似,这样就能把非线性最小二乘转化成线性最小二乘,然后直接求解线性方程就可以得到解。

一般线性化的方法都是使用泰勒展开进行线性化,对精度要求不高时尝尝采用一阶泰勒展开,在x0处进行泰勒展开:

f(x_{i})\approx f(x_{0_{i}})+f`(x_{i})(x_{i}-x_{0_{i}})

将展开后的式子带入到目标函数,并将协方差矩阵开方后放到平方运算中得到下式:

\underset{x}{min}\sum \left \| y_{i}-f(x_{i}) \right \|^{2}_{\sum _{i}^{-1}}=\underset{x}{min}\sum\left \| \sum {_{i}^{-1/2}} (y_{i}-f(x_{0_{i}})F_{i}x_{0_{i}})-\sum {_{i}^{-1/2}}F_{i}x_{i} \right \|^{2}

将前面可以求出的常数项整体用b_{i}表示,后面待求项x_{i}的系数用A_{i}表示整理一下得到:

\underset{x}{min}\sum \left \| b_{i}-A_{i}x_{i} \right \|^{2}

在理想情况下,\sum \left \| b_{i}-A_{i}x_{i} \right \|^{2}可以取到最小值0,就相当于其中的每一项b_{i}-A_{i}x_{i}取到最小值0,其实就是解下面这个线性方程组:

Ax=b

在平常求解线性方程组时直接两边左乘A的逆就求出解x=A^{-1}b,但A一般不可逆,这时往往求该方程组的数值解,一般使用的方法时Cholesky分解和QR分解。

Cholesky分解方法:

Cholesky分解是在原来的式子两边左乘A的转置,然后利用Cholesky分解将A^{T}A分解成n×n的上三角矩阵R的乘积R^{T}R,再令新的变量y=Rx,就得到了新的线性方程组R^{T}y=A^{T}b

Ax=b\Leftrightarrow (A^{T}A)x=A^{T}b\Leftrightarrow R^{T}y=A^{T}b

因为R是上三角矩阵,其转置后为下三角矩阵,第一行只有一个变量,行数越往下变量越多,于是从上到下很容易依次解出y,然后再解线性方程组Rx=y,而这个方程组最后一行只有一个变量,自下而上也可以很容易的解出x,这样原方程组就可以求解出来了。

QR分解:

QR分解比Cholesky分解更加精准和稳定,利用QR分解将A分解为Q矩阵和R矩阵的乘积,其中Q是正交矩阵,R是上三角矩阵,然后在方程组两边同时左乘Q的逆对方程进行化简,如下:

(QR)x=b\Leftrightarrow Rx=Q^{-1}b

R是上三角矩阵,很容易就能求出x。


总结

在实际的SLAM问题中,非线性最小二乘很难找到合适的线性化方法,仅仅使用一阶泰勒展开进行近似有时会产生较大的误差;而且代价函数的误差往往也不能最小化到0值,所以直接求解的方法在现实中的应用存在诸多问题。

所以下面的博客将介绍优化的方法求解非线性最小二乘问题,这种方法在实践中是比较常用的。

你可能感兴趣的:(算法系列,算法,slam,线性代数,机器人)