上一篇博客中介绍的高斯牛顿算法可能会有J'*J为奇异矩阵的情况,这时高斯牛顿法稳定性较差,可能导致算法不收敛。比如当系数都为7或更大的时候,算法无法给出正确的结果。
Levenberg-Marquardt法一定程度上修正了这个问题。
计算迭代系数deltaX公式如下:
当lambda很小的时候,H占主要地位,公式变为高斯牛顿法,当lambda很大的时候,H可以忽略,公式变为最速下降法。该方法提供了更稳定的deltaX。
算法步骤如下:
1.给定初始系数,以及初始优化半径u。
2.计算使用当前系数的模型得到的结果与测量结果差值e。
3.使用迭代公式更新带解算系数。
4.计算更新后系数的模型得到的结果与测量结果差值ecur。
5.如果ecur>e,则u=2*u;否则u=u/2,并且更新模型系数x(k+1)=x(k)+deltaX。
6.判断算法是否收敛,不收敛返回2,否则结束。
代码如下:
clear all;
close all;
clc;
warning off all; a=;b=;c=; %待求解的系数 x=(:0.01:)';
w=rand(length(x),)*-; %生成噪声
y=exp(a*x.^+b*x+c)+w; %带噪声的模型
plot(x,y,'.') pre=rand(,);
update=;
u=0.1;
for i=:
if update==
f = exp(pre()*x.^+pre()*x+pre());
g = y-f; %计算误差 p1 = exp(pre()*x.^+pre()*x+pre()).*x.^; %对a求偏导
p2 = exp(pre()*x.^+pre()*x+pre()).*x; %对b求偏导
p3 = exp(pre()*x.^+pre()*x+pre()); %对c求偏导
J = [p1 p2 p3]; %计算雅克比矩阵
H=J'*J;
if i==
e=dot(g,g);
end
end delta = inv(H+u*eye(length(H)))*J'* g;
pcur = pre+delta; %迭代
fcur = exp(pcur()*x.^+pcur()*x+pcur());
ecur = dot(y-fcur,y-fcur); if ecur<e %比较两次差值,新模型好则使用
if norm(pre-pcur)<1e-10
break;
end
u=u/;
pre=pcur;
e=ecur;
update=;
else
u=u*;
update=;
end
end hold on;
plot(x,exp(a*x.^+b*x+c),'r');
plot(x,exp(pre()*x.^+pre()*x+pre()),'g'); %比较一下
[a b c]
pre'
迭代结果,其中散点为带噪声数据,红线为原始模型,绿线为解算模型
参考:
《视觉slam十四讲》
http://www.docin.com/p-63281100.html