function Par = TR(XY,ParIni,DeltaIni) %-------------------------------------------------------------------------- % % Circle fit by Trust Region % in the full (a,b,R) parameter space % % Input: XY(n,2) is the array of coordinates of n points x(i)=XY(i,1), y(i)=XY(i,2) % ParIni = [a b R] is the initial guess % DeltaIni is the initial size of the trust region % (this is optional; if it is missing, TR sets it to 1) % % Output: Par = [a b R] is the fitting circle: % center (a,b) and radius R % Iter is the number of (main) iterations % %-------------------------------------------------------------------------- if (nargin < 3), DeltaIni = 1; end; % if Delta(initial) is not supplied, set it to one Delta = DeltaIni; % starting with the given initial guess epsilon=0.000001; % tolerance (small threshold) IterMAX = 50; % maximal number of (main) iterations; usually 5-10 suffice c1=0.1; c2=0.25; c3=0.75; % technical constants for updating Delta nPar = 3; % number of parameters in the model Par = ParIni; % starting with the given initial guess [J,g,F] = CurrentIteration(Par,XY); % compute objective function and its derivatives g0 = [g; zeros(nPar,1)]; % "extended" version of the g-vector for iter=1:IterMAX % main loop, each run is one (main) iteration while (1) % secondary loop - adjusting Lambda (no limit on cycles) lambda = 0.0001; % initial value for Lambda h = [J; sqrt(lambda)*eye(nPar)]\g0; % try the Gauss-Newton step if (norm(h) > (1+c1)*Delta) % check if the Gauss-Newton step is outside trust region lambda_min = 0; lambda_max = 2; while (2) % estimate lambda_max (upper bound for Lambda) lambda_max = lambda_max*lambda_max; h = [J; sqrt(lambda_max)*eye(nPar)]\g0; if (norm(h) < Delta), break, end end % while (2) now lambda_max can be used lambda = 0.01; while (3) % solving the subproblem (find Lambda given Delta) [Q R] = qr([J; sqrt(lambda)*eye(nPar)],0); Rinv = inv(R); h = Rinv*(Q'*g0); % potential step phi = norm(h) - Delta; % need phi=0, ideally if (abs(phi) c3 && ParTemp(3) > 0 && Delta < 2*norm(h)) Delta = 2*Delta; end; % (big improvement, enlarge the trust region) if (ratio > 0. && ParTemp(3) > 0) break; end; % (the step is acceptable, move on) end % while (1), the end of the secondary loop if (progress < epsilon) break; end % stopping rule Par = ParTemp; J = JTemp; g = gTemp; F = FTemp; g0 = [g; zeros(nPar,1)]; % (update the iteration) end % the end of the main loop (over iterations) end % TR %================ function CurrentIteration ================ function [J,g,F] = CurrentIteration(Par,XY) % computes F and its derivatives at the current point Par Dx = XY(:,1) - Par(1); Dy = XY(:,2) - Par(2); D = sqrt(Dx.*Dx + Dy.*Dy); J = [-Dx./D, -Dy./D, -ones(size(XY,1),1)]; g = D - Par(3); F = norm(g)^2; end % CurrentIteration