% Levenberg-Marquardt method for non-linear least squares minimization % % Input: p0: initial parameter values % f: function to be optimized % has to return % - the error (r). % - the jacobian of the function (jac) % % thresh: allowed error threshold % maxit: the maximum number of iterations % % plotFunction: a function pointer to plot the result inside the % iteration % arguments: % - p the parameter vector to plot % - color and shape of the plot, e.g. 'rs' for a % red square. % % Output: p solution vector % r the squared norm of f(p) function [p, r] = levmar( p0, f, thresh, maxit, plotFunction ) %init: p = p0; i = 0; [r, jac] = feval(f, p); error_old = r'*r; LMinitialError = error_old errorDelta = 10000; lambda = 0.001; %loop: while (errorDelta > thresh & i < maxit) %solve (J^TJ + lambda*I)s=-J^Te (e = res-y): %s = ((jac'*jac) + lambda .* eye(size(jac,2))) \ - ( jac' * r ); s = -pinv( (jac'*jac) + lambda .* eye(size(jac,2)) ) * ( jac' * r ); %test for infinity: infty = 0; for c=1:size(s,1) if isinf(s(c,1)) == 1 infty = 1; end end if infty disp('stopping LM since parameter update gets too small.'); break; end %update parameter vector: p = p + s; %evaluate cost function: [r, jacNew] = feval(f, p); error = r'*r; if (error < error_old) %s is accepted disp('accepted parameter update.'); %for plotting: if (nargin > 4) feval(plotFunction, p, 'rd'); end lambda = lambda / 10; errorDelta = abs(error_old-error); error_old = error; jac = jacNew; else %s is not accepted disp('NOT accepted parameter update.'); lambda = lambda * 10; p = p - s; end i = i + 1; end numberiterations = i LMendError = error_old