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

计算步骤如下：

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

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

代码如下：

clear all;
close all;
clc;

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

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);      %步骤1
for i=1:1000
    
    f = exp(pre(1)*x.^2+pre(2)*x+pre(3));
    g = y-f;                    %步骤2中的误差 
    
    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];             %步骤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.^2+b*x+c),r);
plot(x,exp(pre(1)*x.^2+pre(2)*x+pre(3)),g);

%比较一下
[a b c]
pre

clear all; close all; clc; a=1;b=2;c=1; %待求解的系数 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); %步骤1 for i=1:1000 f = exp(pre(1)*x.^2+pre(2)*x+pre(3)); g = y-f; %步骤2中的误差 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]; %步骤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.^2+b*x+c),r); plot(x,exp(pre(1)*x.^2+pre(2)*x+pre(3)),g); %比较一下 [a b c] pre

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

计算步骤如下： 图片来自《视觉slam十四讲》6.2.2节。 下面使用书中的练习y=exp(a*x^2+b*x+c)+w这个模型验证一下，其中w为噪声，a、b、c为待解算系数。 代码如下： clear all; close all; clc; a=1;b=2;c=1; %待求解的系数 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); %步骤1 for i=1:1000 f = exp(pre(1)*x.^2+pre(2)*x+pre(3)); g = y-f; %步骤2中的误差 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]; %步骤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.^2+b*x+c),r); plot(x,exp(pre(1)*x.^2+pre(2)*x+pre(3)),g); %比较一下 [a b c] pre 迭代结果，其中散点为带噪声数据，红线为原始模型，绿线为解算模型