f=@(x)((x1-x(1)).^2+(y1-x(2)).^2-x(3).^2);
DF = @(x) [2.*((sqrt((x1-x(1)).^2 + (y1-x(2)).^2) - x(3)).*(x(1)-x1))./(sqrt((x1-x(1)).^2 + (y1-x(2)).^2));
2.*((sqrt((x1-x(1)).^2 + (y1-x(2)).^2) - x(3)).*(x(2)-y1))./(sqrt((x1-x(1)).^2 + (y1-x(2)).^2));
2.*(x(3)-sqrt((x1-x(1)).^2 + (y1-x(2)).^2))];
x0=[0,0,1]
mu0=1;
beta0 = 0.3;
beta1 = 0.9;
maxit = 100;
tol = 1*10^-5;
n=length(x0);
k=0;
mu=mu0;
x=x0;
s=-(DF(x0)*DF(x0)' + mu^2*eye(n))\(DF(x0)*f(x0)');
while norm(s)>tol && k<maxit
[s,mu] = korrektur(f,DF,x,mu,beta0,beta1);
x=x+s;
k=k+1;
end
function[s,mu] = korrektur(f,DF,x,mu,beta0,beta1)
n=length(x);
s=-(DF(x)*DF(x)'+mu^2*eye(n))\(DF(x)*f(x)');
eps_mu=(f(x)*f(x)'-f(x+s)*f(x+s)')\(f(x)*f(x)'-(f(x)'+DF(x)'*s)*(f(x)'+DF(x)'*s)');
if eps_mu <= beta0
[s,mu]=korrektur(f,DF,x,mu,beta0,beta1);
elseif eps_mu >= beta1
mu=mu/2;
end
end
0 Comments
Sign in to comment.