Simple Matlab demo of LM algorithm

Special thanks to Pradit Mittrapiyanuruk.

His open article gives us a very good simple demo of Levenberg-Marquardt Algorithm.

The code is almost the same with code provided from http://www.cse.ucsd.edu/classes/fa04/cse252c/vrabaud1.pdf

The red statements are simple plotting graphic. The statements will show the result as iteration=1,2,3,5,10, and 100.

=================================================================

%definition, giving a function f and create jacobian function for function

%f

syms a b x y real;

f=( a * cos(b*x) + b * sin(a*x))

d=y-f;

Jsym=jacobian(d,[a b]);

%Generate the synthetic data from the curve function with some additional

%noise

a=100;

b=102;

x=[0:0.1:2*pi]';

y = a * cos(b*x) + b * sin(a*x);

% add random noise

y_input = y + 5*rand(length(x),1);

%start the LMA

% initial guess for the parameters

a0=100.5; b0=102.5;

y_init = a0 * cos(b0*x) + b0 * sin(a0*x);

Ndata=length(y_input);

Nparams=2; % a and b are the parameters to be estimated

n_iters=100; % set # of iterations for the LM

lamda=0.01; % set an initial value of the damping factor for the LM

updateJ=1;

a_est=a0;

b_est=b0;

for it=1:n_iters

if updateJ==1

% Evaluate the Jacobian matrix at the current parameters (a_est, b_est)

J=zeros(Ndata,Nparams);

for i=1:length(x)

J(i,:)=[-cos(b_est*x(i))-(b_est*cos(a_est*x(i))*x(i)) (a_est*sin(b_est*x(i))*x(i))-sin(a_est*x(i))];

end

% Evaluate the distance error at the current parameters

y_est = a_est * cos(b_est*x) + b_est * sin(a_est*x);

d=y_input-y_est;

% compute the approximated Hessian matrix, J’ is the transpose of J

H=J'*J;

if it==1 % the first iteration : compute the total error

e=dot(d,d);

end

end

% Apply the damping factor to the Hessian matrix

H_lm=H+(lamda*eye(Nparams,Nparams));

% Compute the updated parameters

dp=-inv(H_lm)*(J'*d(:));

a_lm=a_est+dp(1);

b_lm=b_est+dp(2);

% Evaluate the total distance error at the updated parameters

y_est_lm = a_lm * cos(b_lm*x) + b_lm * sin(a_lm*x);

% plot

if it == 1

h1=plot(y_input, 'b');

hold on

h1=plot(y_est_lm, 'r');

legend(h1,'iteration=1');

hold off

pause

end

if it == 2

h1=plot(y_input, 'b');

hold on

h1=plot(y_est_lm, 'r');

legend(h1,'iteration=2');

hold off

pause

end

if it == 3

h1=plot(y_input, 'b');

hold on

h1=plot(y_est_lm, 'r');

legend(h1,'iteration=3');

hold off

pause

end

if it == 5

h1=plot(y_input, 'b');

hold on

h1=plot(y_est_lm, 'r');

legend(h1,'iteration=5');

hold off

pause

end

if it == 10

h2=plot(y_input, 'b');

hold on

h2=plot(y_est_lm, 'r');

legend(h2,'iteration=10');

hold off

pause

end

if it == 100

h3=plot(y_input, 'b');

hold on

h3=plot(y_est_lm, 'r');

legend(h3,'iteration=100');

hold off

pause

end

d_lm=y_input-y_est_lm;

e_lm=dot(d_lm,d_lm);

% If the total distance error of the updated parameters is less than the previous one

% then makes the updated parameters to be the current parameters

% and decreases the value of the damping factor

if e_lm

lamda=lamda/10;

a_est=a_lm;

b_est=b_lm;

e=e_lm;

disp(e);

updateJ=1;

else % otherwise increases the value of the damping factor

updateJ=0;

lamda=lamda*10;

end

end

=================================================================

Code download, lmademo.m

You will the blue line is fitting close to red line after few iterations.