【算法系列】非线性最小二乘

编程入门行业动态更新时间:2024-10-28 07:20:42

【<a href=https://www.elefans.com/category/jswz/34/1770096.html style= 算法系列】非线性最小二乘"/>

【算法系列】非线性最小二乘

系列文章目录

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

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

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

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

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

文章目录

系列文章

文章目录

前言

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

1.LM算法

2.LMF算法

二、狗腿算法

总结

前言

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

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

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

其中是非线性函数，表示协方差矩阵

为了阐述方便，进行如下表示：

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

1.LM算法

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

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

对于这种问题，在高数中就学过，可以使用拉格朗日乘子将上式转化为无约束的形式：

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

LM算法引入了对进行了修正，可见LM算法的关键就是选取合适的使得系数矩阵保持正定，这样就可以保证每一步迭代目标函数都会下降。

LM算法是GN算法和梯度下降法的结合产物，当较大时，占主要地位，忽略就变成了梯度下降法；当较小时，占主要地位，忽略就变成了GN算法。

2.LMF算法

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

LMF算法给出了依据。

代价函数的变化量为：

近似函数的变化量为：

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

有了这个评价标准那么LMF算法就简单了，就是根据线性度的值来确定的取值，当>0.75时（接近1时）说明线性度较好，应用GN算法主导，即应该调小一点（一般减小10倍）；当<0.25时（接近0时）说明线性度较差，应用梯度下降法主导，即应该调大一点（一般增大10倍）；当在0.25到0.75之间时，认为取值合适，不作调整；当为负时，说明此时代价函数是上升的，应该拒绝本次迭代更新量，并将调大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;endif((yy<0.25)&&(yy>=0))mu=mu*10;endif(yy<0)mu=mu*10;continue;endif(b>=a)break;endn = n+1;point(n,:) = [x y b]; 
end

实验结果：