matlab练习程序（高斯牛顿法最优化）

计算步骤如下：

图片来自《视觉slam十四讲》6.2.2节。

下面使用书中的练习y=exp(a*x^2+b*x+c)+w这个模型验证一下，其中w为噪声，a、b、c为待解算系数。

代码如下：

clear all;
close all;
clc;

a=;b=;c=;              %待求解的系数

x=(:0.01:)';
w=rand(length(x),)*-;   %生成噪声
y=exp(a*x.^+b*x+c)+w;     %带噪声的模型
plot(x,y,'.')

pre=rand(,);      %步骤1
for i=:

    f = exp(pre()*x.^+pre()*x+pre());
    g = y-f;                    %步骤2中的误差 

    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];             %步骤2中的雅克比矩阵

    delta = inv(J'*J)*J'* g;    %步骤3，inv(J'*J)*J'为H的逆

    pcur = pre+delta;           %步骤4
    if norm(delta) <1e-16
        break;
    end
    pre = pcur;
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'

迭代结果，其中散点为带噪声数据，红线为原始模型，绿线为解算模型