该楼层疑似违规已被系统折叠 隐藏此楼查看此楼
a=0;b=0;c=0;u=0;DX=0;DY=0;DZ=0;
w=1+1/4*(a^2+b^2+c^2);
a1=(1+1/4*(a^2+b^2+c^2))/w;a2=(-c-a*b/2)/w;a3=(-b+a*c/2)/w;
b1=(c-a*b/2)/w;b2=(1+1/4*(-a^2+b^2-c^2))/w;b3=(-a-b*c/2)/w;
c1=(b+a*c/2)/w;c2=(a-b*c/2)/w;c3=(1+1/4*(-a^2-b^2+c^2))/w;
R=[a1,a2,a3;b1,b2,b3;c1,c2,c3];
X1=2;Y1=0;Z1=0;X11=2^0.5;Y11=-2^0.5;Z11=0;X2=0;Y2=2;Z2=0;X22=2^0.5;Y22=2^0.5;Z22=0; ...
X3=0;Y3=0;Z3=2;X33=0;Y33=0;Z33=2;
L1=[X1;Y1;Z1]-[DX;DY;DZ]-(1+u)*R*[X11;Y11;Z11];
L2=[X2;Y2;Z2]-[DX;DY;DZ]-(1+u)*R*[X22;Y22;Z22];
L3=[X3;Y3;Z3]-[DX;DY;DZ]-(1+u)*R*[X33;Y33;Z33];
B1=[1 0 0 a1*X1+a2*Y1+a3*Z1 (a*X1-b*Y1+c*Z1)*(1+u)/(2*w) (-b*X1-a*Y1-2*Z1)*(1+u)/(2*w) (-c*X1-2*Y1+a*Z1)*(1+u)/(2*w); ...
0 1 0 b1*X1+b2*Y1+b3*Z1 (-b*X1-a*Y1-2*Z1)*(1+u)/(2*w) (-a*X1+b*Y1-c*Z1)*(1+u)/(2*w) (2*X1-c*Y1-b*Z1)*(1+u)/(2*w); ...
0 0 1 c1*X1+c2*Y1+c3*Z1 (c*X1+2*Y1-a*Z1)*(1+u)/(2*w) (2*X1-c*Y1-b*Z1)*(1+u)/(2*w) (a*X1-b*Y1+c*Z1)*(1+u)/(2*w)];
B2=[1 0 0 a1*X2+a2*Y2+a3*Z2 (a*X2-b*Y2+c*Z2)*(1+u)/(2*w) (-b*X2-a*Y2-2*Z2)*(1+u)/(2*w) (-c*X2-2*Y2+a*Z2)*(1+u)/(2*w); ...
0 1 0 b1*X2+b2*Y2+b3*Z2 (-b*X2-a*Y2-2*Z2)*(1+u)/(2*w) (-a*X2+b*Y2-c*Z2)*(1+u)/(2*w) (2*X2-c*Y2-b*Z2)*(1+u)/(2*w); ...
0 0 1 c1*X2+c2*Y2+c3*Z2 (c*X2+2*Y2-a*Z2)*(1+u)/(2*w) (2*X2-c*Y2-b*Z2)*(1+u)/(2*w) (a*X2-b*Y2+c*Z2)*(1+u)/(2*w)];
B3=[1 0 0 a1*X3+a2*Y3+a3*Z3 (a*X3-b*Y3+c*Z3)*(1+u)/(2*w) (-b*X3-a*Y3-2*Z3)*(1+u)/(2*w) (-c*X3-2*Y3+a*Z3)*(1+u)/(2*w); ...
0 1 0 b1*X3+b2*Y3+b3*Z3 (-b*X3-a*Y3-2*Z3)*(1+u)/(2*w) (-a*X3+b*Y3-c*Z3)*(1+u)/(2*w) (2*X3-c*Y3-b*Z3)*(1+u)/(2*w); ...
0 0 1 c1*X3+c2*Y3+c3*Z3 (c*X3+2*Y3-a*Z3)*(1+u)/(2*w) (2*X3-c*Y3-b*Z3)*(1+u)/(2*w) (a*X3-b*Y3+c*Z3)*(1+u)/(2*w)];
>> B=[B1;B2;B3];L=[L1;L2;L3];
>> Q=(B'*B);O=inv(Q);
>> T=O*B'*L
刚开始赋初值都是0，然后经过这些矩阵运算后，又重新算出了新的a;b;c;u;DX;DY;DZ的值
然后我想实现的是将这新算出来的值再回到第一步，代替那些初值0，再进行运算，反复迭代，直到满足最新算出来的和前一步算出来的绝对值之差小于0.001为止，结束循环。以上那些公式可以编成M文件，例如gongshi.m，那么我想迭代计算，自己编了一个程序但是没有迭代，不知道问题在哪，希望各位大神解惑解惑。
a=0;b=0;c=0;u=1;DX=0;DY=0;DZ=0;i=1;
>> while i>0.001
gongshi;
DX=T(1);DY=T(2);DZ=T(3);u=T(4);a=T(5);b=T(6);c=T(7);
i=abs(DX-T(1))>1*10^(-3);
end
不知道我编的这个迭代算法问题在哪？