上一篇博客中介绍的高斯牛顿算法可能会有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=7;b=7;c=7; %待求解的系数 x=(0:0.01:1)\'; w=rand(length(x),1)*2-1; %生成噪声 y=exp(a*x.^2+b*x+c)+w; %带噪声的模型 plot(x,y,\'.\') pre=rand(3,1); update=1; u=0.1; for i=1:100 if update==1 f = exp(pre(1)*x.^2+pre(2)*x+pre(3)); g = y-f; %计算误差 p1 = exp(pre(1)*x.^2+pre(2)*x+pre(3)).*x.^2; %对a求偏导 p2 = exp(pre(1)*x.^2+pre(2)*x+pre(3)).*x; %对b求偏导 p3 = exp(pre(1)*x.^2+pre(2)*x+pre(3)); %对c求偏导 J = [p1 p2 p3]; %计算雅克比矩阵 H=J\'*J; if i==1 e=dot(g,g); end end delta = inv(H+u*eye(length(H)))*J\'* g; pcur = pre+delta; %迭代 fcur = exp(pcur(1)*x.^2+pcur(2)*x+pcur(3)); ecur = dot(y-fcur,y-fcur); if ecur<e %比较两次差值，新模型好则使用 if norm(pre-pcur)<1e-10 break; end u=u/2; pre=pcur; e=ecur; update=1; else u=u*2; update=0; end end hold on; plot(x,exp(a*x.^2+b*x+c),\'r\'); plot(x,exp(pre(1)*x.^2+pre(2)*x+pre(3)),\'g\'); %比较一下 [a b c] pre\'
迭代结果，其中散点为带噪声数据，红线为原始模型，绿线为解算模型
参考：
《视觉slam十四讲》
http://www.docin.com/p-63281100.html
请发表评论