matlab实现高斯牛顿法、Levenberg–Marquardt方法

时间:2021-10-16 15:50:31

高斯牛顿法:

function [ x_ans ] = GaussNewton( xi, yi, ri)
% input : x = the x vector of 3 points
% y = the y vector of 3 points
% r = the radius vector of 3 circles
% output : x_ans = the best answer
% set up r equations
r1 = @(x, y) sqrt((x-xi(1))^2+(y-yi(1))^2) - ri(1);
r2 = @(x, y) sqrt((x-xi(2))^2+(y-yi(2))^2) - ri(2);
r3 = @(x, y) sqrt((x-xi(3))^2+(y-yi(3))^2) - ri(3);
% set up Dr matrix
Dr = @(x) [(x(1) - xi(1))/(sqrt((x(1) - xi(1))^2+(x(2)-yi(1))^2)), (x(2) - yi(1))/(sqrt((x(1) - xi(1))^2+(x(2)-yi(1))^2));
(x(1) - xi(2))/(sqrt((x(1) - xi(2))^2+(x(2)-yi(2))^2)), (x(2) - yi(2))/(sqrt((x(1) - xi(2))^2+(x(2)-yi(2))^2));
(x(1) - xi(3))/(sqrt((x(1) - xi(3))^2+(x(2)-yi(3))^2)), (x(2) - yi(3))/(sqrt((x(1) - xi(3))^2+(x(2)-yi(3))^2))];
% set up r matrix
r = @(x) [r1(x(1), x(2)); r2(x(1), x(2)); r3(x(1), x(2))];
x0 = [0, 0]; % initial guess
while 1
A = Dr(x0);
v0 = (A' * A) \ (- A' * r(x0));
x1 = x0 + v0';
if norm(x1-x0)<1e-6 % break squest
break;
end
x0 = x1;
end
x_ans = x1;
end

Levenberg–Marquardt方法:

function [ x_ans ] = LeveMarq( ti, yi, x_guess, lmd)
% input : t = the x vector of 5 points
% y = the y vector of 5 points
% x_guess = the guess vector of x_ans
% output : x_ans = the best answer
% set up r matrix
r = @(x) [x(1) * exp(-x(2)*(ti(1) - x(3))^2) - yi(1);
x(1) * exp(-x(2)*(ti(2) - x(3))^2) - yi(2);
x(1) * exp(-x(2)*(ti(3) - x(3))^2) - yi(3);
x(1) * exp(-x(2)*(ti(4) - x(3))^2) - yi(4);
x(1) * exp(-x(2)*(ti(5) - x(3))^2) - yi(5)];
% set up Dr matrix
Dr = @(x) [exp(-x(2)*(ti(1)-x(3))^2), -x(1)*(ti(1)-x(3))^2*exp(-x(2)*(ti(1)-x(3))^2), 2*x(1)*x(2)*(ti(1)-x(3))*exp(-x(2)*(ti(1)-x(3))^2);
exp(-x(2)*(ti(2)-x(3))^2), -x(1)*(ti(2)-x(3))^2*exp(-x(2)*(ti(2)-x(3))^2), 2*x(1)*x(2)*(ti(2)-x(3))*exp(-x(2)*(ti(2)-x(3))^2);
exp(-x(2)*(ti(3)-x(3))^2), -x(1)*(ti(3)-x(3))^2*exp(-x(2)*(ti(3)-x(3))^2), 2*x(1)*x(2)*(ti(3)-x(3))*exp(-x(2)*(ti(3)-x(3))^2);
exp(-x(2)*(ti(4)-x(3))^2), -x(1)*(ti(4)-x(3))^2*exp(-x(2)*(ti(4)-x(3))^2), 2*x(1)*x(2)*(ti(4)-x(3))*exp(-x(2)*(ti(4)-x(3))^2);
exp(-x(2)*(ti(5)-x(3))^2), -x(1)*(ti(5)-x(3))^2*exp(-x(2)*(ti(5)-x(3))^2), 2*x(1)*x(2)*(ti(5)-x(3))*exp(-x(2)*(ti(5)-x(3))^2)]; x0 = x_guess; % initial guess
while 1
A = Dr(x0);
M_A = A' * A + lmd .* diag(diag(A' * A));
M_b = - A' * r(x0);
v0 = M_A \ M_b;
x1 = x0 + v0';
if norm(x1-x0)<1e-6 % break squest
break;
end
x0 = x1;
end
x_ans = x1;
end