基于几何最短距离的椭圆拟合

up_x0 = [2;3;6;3;0];
 %up_x0 = [3;3;6;3;0];
max_N = 200; 
iter = 0;
lamda = 0.001;
mu = 10;
innov_x1 = zeros（51）;
while iter < max_N
%%%% solve the Jacobian     
       Jaco=[];
       FX=[];

   
    %%%if a     if up_x0（3）            up_x0（5）=up_x0（5）-sign（up_x0（5））*pi/2;
            tmp =up_x0（3） ;
            up_x0（3） = up_x0（4）;
            up_x0（4） = tmp;
     end
      xc = up_x0（1）;yc=up_x0（2）;
      a = up_x0（3）;b= up_x0（4）;
      alpha = up_x0（5）;
     COS = cos（alpha）;
     SIN = sin（alpha）;
     R = [COSSIN;-SINCOS];
    for k = 1:length（X）
        %%% cord transform [xx;yy] = R*（X（k:）-Xc）
        %%%R = [cos（up_x0（5））sin（up_x0（5））;-sin（up_x0（5））cos（up_x0（5））];
        
        xi = COS*（X（k1）-xc）+SIN*（X（k2）-yc）;
        yi = -SIN*（X（k1）-xc）+COS*（X（k2）-yc）;
    
        xk1 =a*b*xi/sqrt（（b*xi）^2+（a*yi）^2）;
        yk1 =a*b*yi/sqrt（（b*xi）^2+（a*yi）^2）;
        if abs（xi） < a 
            xk2 =xi;
            yk2 = sign（yi）*sqrt（a^2-xi^2）*b/a;
        else
            xk2=sign（xi）*a;
            yk2=0;
        end
        xk0 = 0.5*（xk1+xk2）;
        yk0 = 0.5*（yk1+yk2）;
        xy_up=[xk0;yk0];
        for kk = 1:5
            Q=[xk0*b^2 yk0*a^2; （a^2 - b^2）*yk0+yi*b^2（a^2 - b^2）*xk0-xi*a^2 ];%%% Q=[b^2*xk0 a^2*yk0;