matlab练习程序（Levenberg-Marquardt法最优化）

上一篇博客中介绍的高斯牛顿算法可能会有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