小弟最近研究定位算法，在应用泰勒级数展开算法时编的程序总是达不到理想的结果，请从事本行业的各位高手指教。或给个源程序,不胜感谢！！！E-mail:shitou4684@163.com 小弟的程序如下：

%************************************************************
%Taylor算法
%************************************************************
for i=1:length(x0)
    xu=500+2.5*i;
    yu=500+2.5*i;
    m=500;
    e=1;
    j=0;

    while m>e&j<1000
            r1=sqrt((X1-xu)^2+(Y1-yu)^2);
            r2=sqrt((X2-xu)^2+(Y2-yu)^2);
            r3=sqrt((X3-xu)^2+(Y3-yu)^2);
            r4=sqrt((X4-xu)^2+(Y4-yu)^2);
            r5=sqrt((X5-xu)^2+(Y5-yu)^2);
            r6=sqrt((X6-xu)^2+(Y6-yu)^2);
            r7=sqrt((X7-xu)^2+(Y7-yu)^2);
         h=[c*T(1,i)-r2+r1
             c*T(2,i)-r3+r1
             c*T(3,i)-r4+r1
             c*T(4,i)-r5+r1
             c*T(5,i)-r6+r1
             c*T(6,i)-r7+r1];
         G=-[(X2-xu)/r2-(X1-xu)/r1 (Y2-yu)/r2-(Y1-yu)/r1
            (X3-xu)/r3-(X1-xu)/r1 (Y3-yu)/r3-(Y1-yu)/r1
            (X4-xu)/r4-(X1-xu)/r1 (Y4-yu)/r4-(Y1-yu)/r1
            (X5-xu)/r5-(X1-xu)/r1 (Y5-yu)/r5-(Y1-yu)/r1
            (X6-xu)/r6-(X1-xu)/r1 (Y6-yu)/r6-(Y1-yu)/r1
            (X7-xu)/r7-(X1-xu)/r1 (Y7-yu)/r7-(Y1-yu)/r1];
           Q=[sig^2 0 0 0 0 0
              0 sig^2 0 0 0 0
              0 0 sig^2 0 0 0
              0 0 0 sig^2 0 0
              0 0 0 0 sig^2 0
              0 0 0 0 0 sig^2];
          Z=inv(G'*inv(Q)*G)*G'*inv(Q)*h;
         xu=xu+Z(1);yu=yu+Z(2);
         m=abs(Z(1))+abs(Z(2));
         j=j+1;
    end
    x1(i)=xu;y1(i)=yu;
end