ccl_math_solve_lm

 [xf, S, msg] = ccl_math_solve_lm (varargin)

 Solve nonlinear least squared problem using Levenberg-Maquardt algoritm

 Input
   FUN                                   Objective fnction
   xc                                    Initial guesses of solution
   Options                                 'TolFun'   norm(FUN(x),1) stopping tolerance;
                                         'TolX'     norm(x-xold,1) stopping tolerance;
                                         'MaxIter'  Maximum number of iterations;
 Output:
   xf                                    Final solution approximation
   S                                     Sum of squares of residuals
   msg                                   Output message

0001 function [xf, S, msg] = ccl_math_solve_lm (varargin)
0002 % [xf, S, msg] = ccl_math_solve_lm (varargin)
0003 %
0004 % Solve nonlinear least squared problem using Levenberg-Maquardt algoritm
0005 %
0006 % Input
0007 %   FUN                                   Objective fnction
0008 %   xc                                    Initial guesses of solution
0009 %   Options                                 'TolFun'   norm(FUN(x),1) stopping tolerance;
0010 %                                         'TolX'     norm(x-xold,1) stopping tolerance;
0011 %                                         'MaxIter'  Maximum number of iterations;
0012 % Output:
0013 %   xf                                    Final solution approximation
0014 %   S                                     Sum of squares of residuals
0015 %   msg                                   Output message
0016 
0017 % 1. objective function
0018 FUN = varargin{1};
0019 if ~(isvarname(FUN) || isa(FUN,'function_handle'))
0020     error('FUN Must be a Function Handle or M-file Name.')
0021 end
0022 
0023 % 2. initial guess
0024 xc = varargin{2};
0025 
0026 % 3. search options
0027 options.MaxIter  = 100;       % maximum number of iterations allowed
0028 options.TolFun   = 1e-7;      % tolerace for final function value
0029 options.TolX     = 1e-4;      % tolerance on difference of x-solutions
0030 if nargin > 2
0031     if isfield(varargin{3}, 'MaxIter'), options.MaxIter = varargin{3}.MaxIter ; end
0032     if isfield(varargin{3}, 'TolFun'), options.TolFun = varargin{3}.TolFun ;    end
0033     if isfield(varargin{3}, 'TolX'), options.TolX = varargin{3}.TolX ;          end
0034 end
0035 x    = xc(:);
0036 dim_x   = length(x);
0037 epsx = options.TolX * ones(dim_x,1) ;
0038 epsf = options.TolFun(:);
0039 
0040 % get function evaluation and jacobian at x
0041 %
0042 r   = FUN(x) ;                  % E(x,b)
0043 J   = finjac(FUN, r, x, epsx);  % dE(x,b)/db
0044 S   = r'*r;                     % sum of squared error
0045 %  nfJ = 2;
0046 A   = J.'*J;                    % System matrix
0047 v   = J.'*r;
0048 D   = diag(diag(A));            % automatic scaling
0049 for i = 1 : dim_x
0050     if D(i,i)==0, D(i,i)=1 ; end
0051 end
0052 
0053 Rlo = 0.25;
0054 Rhi = 0.75;
0055 l = 1;  lc=.75 ;
0056 d = options.TolX;
0057 
0058 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0059 %   Main iteration
0060 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0061 iter = 0 ;
0062 while   iter < options.MaxIter && ...   % current iteration < maximum iteration
0063         any(abs(d) >= epsx) && ...      % d > minimum x tolerance
0064         any(abs(r) >= epsf)             % r > minimum f(x) toleration
0065     
0066     %   d  = (A+l*D)\v;            % negative solution increment d = (J'J+lambda*I)^(-1)*J'*(y-f(x,w))
0067     d   = pinv(A+l*D)*v ;
0068     xd  = x-d;                  % the next x
0069     
0070     rd  = FUN(xd);              % residual error at xd
0071     J   = finjac(FUN, r, x, epsx);  % jacobian at x
0072     %   nfJ = nfJ + 1;
0073     Sd  = rd.'*rd;              % squared error if xd is taken
0074     dS  = d.'*(2*v-A*d);
0075     R   = (S-Sd)/dS;            % reduction is squared error is xd is taken
0076     
0077     if R > Rhi                  % halve lambda if R too high
0078         l = l/2;
0079         if l < lc, l = 0; end
0080     elseif R < Rlo              % find new nu if R too low
0081         nu = (Sd-S)/(d.'*v) + 2;
0082         if nu < 2
0083             nu = 2;
0084         elseif nu > 10
0085             nu = 10;
0086         end
0087         if l==0
0088             lc = 1 / max(abs(diag(inv(A))));
0089             l  = lc;
0090             nu = nu/2;
0091         end
0092         l = nu*l;
0093     end
0094     iter = iter+1;
0095     % update only if f(x-d) is better
0096     if Sd < S
0097         S = Sd;
0098         x = xd;
0099         r = rd;
0100         % nfJ = nfJ+1;
0101         A = J'*J;
0102         v = J'*r;
0103     end
0104 end % end while
0105 
0106 xf  = x;  % final solution
0107 
0108 if iter == options.MaxIter, msg = 'Solver terminated because max iteration reached\n' ;
0109 elseif any(abs(d) < epsx),  msg = 'Solver terminated because |dW| < min(dW)\n' ;
0110 elseif any(abs(r) < epsf),  msg = 'Solver terminated because |F(dW)| < min(F(dW))\n' ;
0111 else                        msg = 'Problem solved\n' ;
0112 end
0113 end
0114 
0115 function J = finjac(FUN, y, x, epsx)
0116 % J = finjac(FUN, y, x, epsx)
0117 %
0118 % Numerical approximation to Jacobian
0119 %
0120 % Input:
0121 %   FUN                                     Function
0122 %   y                                       Current y = FUN(x)
0123 %   x                                       Current x
0124 %   epsx                                    Increment of x
0125 %
0126 % Output                                    J = d[FUN] / d[x]
0127 
0128 dim_x = length(x);
0129 J  = zeros(length(y),dim_x);
0130 for k = 1 : dim_x
0131     dx      = .25*epsx(k);
0132     xd      = x;
0133     xd(k)   = xd(k)+dx;
0134     yd      = feval(FUN,xd);
0135     J(:,k)  = ((yd-y)/dx);
0136 end
0137 end