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