www.gusucode.com > robotmanip 工具箱 matlab源码程序 > robotmanip/+robotics/+manip/+internal/ErrorDampedLevenbergMarquardt.m

    classdef ErrorDampedLevenbergMarquardt < robotics.manip.internal.NLPSolverInterface
    %This class is for internal use only. It may be removed in the future.
    
    %ERRORDAMPEDLEVENBERGMARQUARDT Solver for unconstrained nonlinear
    %   lease squares problem (only with some very crude handling of 
    %   variable bounds)
    %
    %   min  0.5 * f(x)'* W * f(x)
    %    x
    %   s.t.    lb <= x <= ub
    %   where f(x) is an m-by-1 vector, x is an n-by-1 vector
    
    
    %   Copyright 2016 The MathWorks, Inc.
    %
    %   References:
    %
    %   [1] T. Sugihara, Solvability-unconcerned inverse kinematics by the
    %   Levenberg-Marquardt method
    %   IEEE Transactions on Robotics, Vol 27, No. 5, 2011
    
    properties (Access = protected)
        
        %GradientTolerance The solver terminates if the norm of the 
        %gradient falls below this value (a positive value)
        GradientTolerance
        
        %ErrorChangeTolerance The solver terminates if the change in all
        %elements of the error vector fall below this value (a positive
        %value)
        ErrorChangeTolerance
        
        % The LM damping (Wn) is comprised of two parts: Current
        % cost(error) and a damping bias. In some cases, the users might
        % want to disable the cost damping term, they can do so by setting
        % UseErrorDamping flag to false. 
        %DampingBias
        DampingBias
        
        %UseErrorDamping
        UseErrorDamping 
    end
    
    methods
        function obj = ErrorDampedLevenbergMarquardt()
            %ErrorDampedLevenbergMarquardt Constructor
            obj.MaxNumIteration = 1500;
            obj.MaxTime = 10;
            obj.SolutionTolerance = 1e-6;
            obj.ConstraintsOn = true;
            obj.RandomRestart = true;
            obj.StepTolerance = 1e-12;
            obj.GradientTolerance = 0.5e-8;
            obj.ErrorChangeTolerance = 1e-12;
            obj.DampingBias = 1e-2*(0.5^2);
            obj.UseErrorDamping = true;
            
            obj.Name = 'LevenbergMarquardt';
        end   
        
        function params = getSolverParams(obj)
            params.Name = obj.Name;
            params.MaxNumIteration = obj.MaxNumIteration;
            params.MaxTime = obj.MaxTime;
            params.GradientTolerance = obj.GradientTolerance;
            params.SolutionTolerance = obj.SolutionTolerance;
            params.ConstraintsOn = obj.ConstraintsOn;
            params.RandomRestart = obj.RandomRestart;
            params.StepTolerance = obj.StepTolerance;
            params.ErrorChangeTolerance = obj.ErrorChangeTolerance;
            params.DampingBias = obj.DampingBias;
            params.UseErrorDamping = obj.UseErrorDamping;
        end
        
        function setSolverParams(obj, params)
            obj.MaxNumIteration =       params.MaxNumIteration;
            obj.MaxTime =               params.MaxTime;
            obj.GradientTolerance =     params.GradientTolerance;
            obj.SolutionTolerance =     params.SolutionTolerance;
            obj.ConstraintsOn =         params.ConstraintsOn;
            obj.RandomRestart =         params.RandomRestart;
            obj.StepTolerance =         params.StepTolerance;
            obj.ErrorChangeTolerance=          params.ErrorChangeTolerance;
            obj.DampingBias =           params.DampingBias;
            obj.UseErrorDamping =       params.UseErrorDamping;
        end

        function newobj = copy(obj)
            %COPY
            newobj = robotics.manip.internal.ErrorDampedLevenbergMarquardt();
                        
            newobj.Name = obj.Name;
            newobj.ConstraintBound = obj.ConstraintBound;
            newobj.ConstraintMatrix = obj.ConstraintMatrix;
            newobj.ConstraintsOn = obj.ConstraintsOn;
            newobj.SolutionTolerance = obj.SolutionTolerance;
            newobj.RandomRestart = obj.RandomRestart;
            
            newobj.SeedInternal = obj.SeedInternal;
            newobj.MaxTimeInternal = obj.MaxTimeInternal;
            newobj.MaxNumIterationInternal = obj.MaxNumIterationInternal;
            
            newobj.CostFcn = obj.CostFcn;
            newobj.GradientFcn = obj.GradientFcn;
            newobj.SolutionEvaluationFcn = obj.SolutionEvaluationFcn;
            newobj.RandomSeedFcn = obj.RandomSeedFcn;
            newobj.BoundHandlingFcn = obj.BoundHandlingFcn;
            
            newobj.ExtraArgs = obj.ExtraArgs;
            
            newobj.setSolverParams(obj.getSolverParams);
        end
        
    end
    
    methods (Access = {?robotics.manip.internal.NLPSolverInterface, ...
                       ?matlab.unittest.TestCase})
        
        function [xSol, exitFlag, en, iter] = solveInternal(obj)
            t0 = tic;
            x = obj.SeedInternal;
            
            % Dimension
            n = size(x,1);
            
            for i = 1:obj.MaxNumIterationInternal
                [cost, W, J, obj.ExtraArgs] = obj.CostFcn(x, obj.ExtraArgs);
                grad = obj.GradientFcn(x, obj.ExtraArgs);
                grad = grad(:);
                [en, ev] = obj.SolutionEvaluationFcn(x, obj.ExtraArgs);
                
                xSol = x;
                iter = i;
                
                % Exit conditions
                if atLocalMinimum(obj,grad)
                    exitFlag = robotics.manip.internal.NLPSolverExitFlags.LocalMinimumFound;
                    return;
                end
                
                if i>1 && stepSizeBelowMinimum(obj, x, xprev)
                    exitFlag = robotics.manip.internal.NLPSolverExitFlags.StepSizeBelowMinimum;
                    return;
                end
                
                if i>1 && changeInErrorBelowMinimum(obj, ev, evprev)
                    exitFlag = robotics.manip.internal.NLPSolverExitFlags.ChangeInErrorBelowMinimum;
                    return;
                end
                
                if timeLimitExceeded(obj, toc(t0))
                    exitFlag = robotics.manip.internal.NLPSolverExitFlags.TimeLimitExceeded;
                    return;
                end
                
                evprev = ev;
                xprev = x;
                
                % Levenberg-Marquardt update
                
                cc = obj.UseErrorDamping * cost;
                Wn = (cc + obj.DampingBias)*eye(n); 
                H0 = J'*W*J;
                
                H = H0 + Wn;
                step = - H\grad; 
                newcost = obj.CostFcn(x+step, obj.ExtraArgs);
                lambda = 1;

                % Make sure it's a (sufficient) decent
                while newcost > cost
                    lambda = lambda*2.5;
                    Wn = (cc + lambda*obj.DampingBias)*eye(n); 
                    step = - (H0 + Wn)\grad; 
                    newcost = obj.CostFcn(x+step, obj.ExtraArgs);
                end
                
                x = x + step;
                
                % Naive handling of constraints
                if obj.ConstraintsOn
                    x = obj.BoundHandlingFcn(x, obj.ExtraArgs); 
                end
                
            end
            
            en= obj.SolutionEvaluationFcn(xSol, obj.ExtraArgs);
            xSol = x;
            iter = i;
            exitFlag = robotics.manip.internal.NLPSolverExitFlags.IterationLimitExceeded; % Maximum iteration has been reached
            
        end
        
        function flag = atLocalMinimum(obj, grad)
            flag = norm(grad) < obj.GradientTolerance;
        end
        
        function flag = changeInErrorBelowMinimum(obj, ev, evprev)
            flag = all(abs(ev - evprev) < obj.ErrorChangeTolerance);
        end
        
        function flag = stepSizeBelowMinimum(obj, x, xprev)
            flag = all(abs(x - xprev) < obj.StepTolerance);
        end
        
    end
    
end