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

  系列文章目录

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

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

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

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

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

文章目录

系列文章

文章目录

前言

一、列文伯格-马夸尔算法

1.LM算法

 2.LMF算法

二、狗腿算法

总结


前言

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

非线性最小二乘有多种解法,本篇博客介绍列文伯格-马夸尔算法及其变种(狗腿算法)求解最小二乘问题。


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

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

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

为了阐述方便,进行如下表示:

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

一、列文伯格-马夸尔算法

1.LM算法

前面提到GN(高斯-牛顿)算法当展开点距离目标点较远时,可能会出现迭代效果下降,甚至发散的情况,原因是J^{T}J不一定正定,而列文伯格和马夸尔两人对GN算法进行了改进,后来Fletcher又对其中的策略进行了改进,即实际中常用的LMF算法。

既然GN算法展开点距离目标点必须保持在一定范围内才有效,那么我们在进行该算法时,可以给迭代更新量\bigtriangleup x一个限制范围,以保证GN算法的收敛,那么原来的无约束最小二乘就变成了带约束的最小二乘:

\bigtriangleup x=\underset{\bigtriangleup x}{argmin}\left \| e(x^{(k)}) + J\cdot \bigtriangleup x \right \|^{2},\left \| \bigtriangleup x \right \| \leq d_{k}

对于这种问题,在高数中就学过,可以使用拉格朗日乘子\mu _{k}将上式转化为无约束的形式:

\bigtriangleup x=\underset{\bigtriangleup x}{argmin} (\left \| e(x^{(k)}) + J\cdot \bigtriangleup x \right \|^{2} + \mu_{k}\left \| \bigtriangleup x \right \|^{2})

到这里就和前面一样了,求最小值就对上式求导,使导数等于0,得到下面的线性方程:

(J^{T}J+\mu_{k}\cdot I)\bigtriangleup x=-J^{T}\cdot e

LM算法引入了\mu_{k}J^{T}J​进行了修正,可见LM算法的关键就是选取合适的\mu_{k}​使得系数矩阵保持正定,这样就可以保证每一步迭代目标函数都会下降。

LM算法是GN算法和梯度下降法的结合产物,当\mu_{k}​较大时,\mu_{k}\cdot I占主要地位,忽略J^{T}J​就变成了梯度下降法;当\mu_{k}​较小时,J^{T}J​占主要地位,忽略\mu_{k}\cdot I​就变成了GN算法。

 2.LMF算法

 既然上面提到了LM算法的关键就是选取合适的\mu_{k}​使得系数矩阵保持正定,以保证每一步迭代目标函数都会下降,那么我们如何确定\mu_{k}​的取值呢?

LMF算法给出了依据。

代价函数的变化量为:

\bigtriangleup \psi =\psi(x^{(k+1)})-\psi(x^{(k)})

 近似函数的变化量为:

\bigtriangleup \varphi =\varphi(\bigtriangleup x)-\varphi(0)=2\bigtriangleup x^{T}J^{T}e+\bigtriangleup x^{T}J^{T}J\bigtriangleup x

 然后定义一个评价量-线性度\gamma _{k}来衡量近似程度,\gamma _{k}​越接近1则证明原函数和近似后的函数变化量越接近,也就是说明展开点的线性度更好。

\gamma _{k}=\frac{\bigtriangleup \psi}{\bigtriangleup \varphi }

有了这个评价标准那么LMF算法就简单了,就是根据线性度\gamma _{k}​的值来确定\mu_{k}​的取值,当\gamma _{k}​>0.75时(接近1时)说明线性度较好,应用GN算法主导,即\mu_{k}​应该调小一点(一般减小10倍);当\gamma _{k}​<0.25时(接近0时)说明线性度较差,应用梯度下降法主导, 即\mu_{k}​应该调大一点(一般增大10倍);当\gamma _{k}​在0.25到0.75之间时,认为取值合适,不作调整;当\gamma _{k}​为负时,说明此时代价函数是上升的,应该拒绝本次迭代更新量,并将\mu_{k}调大10倍。

MATLAB实验:

主函数:

% 目标函数为 z=f(x,y)=(x^2+y^2)/2
clear all;
clc;
%构造函数
fun = inline('(x^4+2*y^2)/2','x','y');
dfunx = inline('2*x^3','x','y');
dfuny = inline('2*y','x','y'); 
ddfunx = inline('6*x^2','x','y');
ddfuny = inline('2','x','y'); 

x0 = 2;y0 = 2;                                  %初值

[x,y,n,point] = LMF(fun,dfunx,dfuny,ddfunx,ddfuny,x0,y0);    %LMF算法

figure
x = -2:0.1:4;
y = x;
[x,y] = meshgrid(x,y);
z = (x.^2+y.^2)/2;
surf(x,y,z)    %绘制三维表面图形
% hold on
% plot3(point(:,1),point(:,2),point(:,3),'linewidth',1,'color','black')
hold on
scatter3(point(:,1),point(:,2),point(:,3),'r','*');
n

LMF算法:

%% LMF算法
function [x,y,n,point] = LMF(fun,dfunx,dfuny,ddfunx,ddfuny,x,y)
%输入:fun:函数形式 dfunx(y):梯度(导数)ddfunx(y):海森矩阵(二阶导数) x(y):初值
%输出:x(y):计算出的自变量取值 n:迭代次数 point:迭代点集

%初始化
a = feval(fun,x,y);                                 %初值
n=1;                                                %迭代次数
point(n,:) = [x y a];   
mu=1;                                               %拉格朗日乘子

while (1) 
  
  a = feval(fun,x,y);                               %当前时刻值
  adfun=2*(-(feval(fun,x,y))^0.5/(feval(dfunx,x,y)+mu))*feval(dfunx,x,y)*(feval(fun,x,y))^0.5+((-(feval(fun,x,y))^0.5)/(feval(dfunx,x,y)+mu))^2*(feval(dfunx,x,y))^2;
  x = x - (feval(fun,x,y))^0.5/(feval(dfunx,x,y)+mu);   %下一时刻自变量
  y = y - (feval(fun,x,y))^0.5/(feval(dfunx,x,y)+mu);   %下一时刻自变量
  b = feval(fun,x,y);                               %下一时刻值
  afun=b-a;                                         %原函数增量
  yy=afun/adfun;                                    %线性度
  if(yy>0.75)
      mu=mu/10;
  end
  if((yy<0.25)&&(yy>=0))
      mu=mu*10;
  end
  if(yy<0)
      mu=mu*10;
      continue;
  end
  
  if(b>=a)
      break;
  end
  n = n+1;
  point(n,:) = [x y b]; 
end

实验结果:

【算法系列】非线性最小二乘-列文伯格马夸尔和狗腿算法_第1张图片

二、狗腿算法

前面的LMF算法已经比较完善,有较好的性能了,但其中仍然存在一点瑕疵,当\gamma _{k}​为负时,说明此时代价函数是上升的,应该拒绝本次迭代更新量,这就白白浪费了求解这一步的计算代价,Powell提出的狗腿算法就很好的修正了这个问题。

在狗腿算法中,先计算最速下降法的迭代更新量和高斯-牛顿的迭代更新量:

\bigtriangleup x_{SD}=-\alpha _{k}\cdot 2 \cdot J^{T}\cdot e(x^{(k)})

\bigtriangleup x_{GN}=-(J^{T}J)^{-1}\cdot J^{T}e

在狗腿算法中也是使用线性度来调整参数,但不是调整\mu_{k}而是调整d_{k}(置信域),d_{k}\mu_{k}是负相关的,比如线性度好时,说明近似程度高,\mu_{k}应该调小点使用高斯牛顿算法,相应的d_{k}可以调大一点,扩大置信域,具体调整策略如下:

  •  当\bigtriangleup x_{GN} \leq d_{k}时,选择高斯牛顿算法,使\bigtriangleup x_{DL} =\bigtriangleup x_{GN}
  • \bigtriangleup x_{GN}超出置信域时,高斯牛顿算法不一定能让代价函数下降,而最速下降法一定能使代价函数下降,\bigtriangleup x_{DL}=d_{k}\frac{\bigtriangleup x_{SD}}{\left \| \bigtriangleup x_{SD} \right \|},取最速下降法的方向,步长取d_{k},因为步长小于\bigtriangleup x_{SD}的长度,肯定能让函数下降。
  • \bigtriangleup x_{GN}超出置信域,而\bigtriangleup x_{SD}在置信域内时,就选取二者之间折中的迭代量,\bigtriangleup x_{DL}=\bigtriangleup x_{SD}+\beta (\bigtriangleup x_{GN}-\bigtriangleup x_{SD}),选取\beta使得\bigtriangleup x_{DL}=d_{k}

 后面就是根据线性度对d_{k}进行调整:

  • \gamma _{k}>0.75时,说明线性度好,可以将置信域扩大,使d_{k+1}=max\{ d_{k},3\cdot \left \| \bigtriangleup x_{DL} \right \| \}
  • 0.25 \leq \gamma _{k}\leq 0.75时,说明置信域设置合理,不作调整,d_{k+1}=d_{k}
  • \gamma _{k}<0.25时,说明线性度较差,需要将置信域缩小,d_{k+1}=d_{k}/2

 可以看到在狗腿算法中,每次执行迭代都会使得代价函数下降,但相应的计算代价也升高。


总结

这几种算法,每一种都是前一种的改进,下降速度和效果越来越好,但相应的计算代价也越来越高,在实际应用中应结合具体问题合理选择。

你可能感兴趣的:(算法系列,算法,python,slam)