单片空间后方交会matlab程序

f=0.15;fai=0;omi=0;ka=0;
Xd=[5083.205,5780.02,5210.879,5909.264];
Yd=[5852.099,5906.365,4258.446,4314.283];
Zd=[527.925,571.549,461.81,455.484];
%以上为输入gcp1到gcp4的地面坐标
xx=[-0.07393,-0.005252,-0.079122,-0.009887];
yx=[0.078705,0.078184,-0.078879,-0.080089];
%右片像点坐标
Xs=(5083.205+5780.02+5210.879+5909.264)/4;
Ys=(5852.099+5906.365+4258.446+4314.283)/4;
Sx=sqrt((xx(2)-xx(1))^2+(yx(2)-yx(1))^2);
Sd=sqrt((Xd(2)-Xd(1))^2+(Yd(2)-Yd(1))^2);
m=Sd/Sx
Zs=m*f+1/4*(Zd(1)+Zd(2)+Zd(3)+Zd(4));
X=[1;1;1;1;1;1];
while 1
a1=cos(fai)*cos(ka)-sin(fai)*sin(omi)*sin(ka);a2=-cos(fai)*sin(ka)-sin(fai)*sin(omi)*cos(ka);a3=-sin(fai)*cos(omi);
b1=cos(omi)*sin(ka);b2=cos(omi)*cos(ka);b3=-sin(omi);
c1=sin(fai)*cos(ka)+cos(fai)*sin(omi)*sin(ka);c2=-sin(fai)*sin(ka)+cos(fai)*sin(omi)*cos(ka);c3=cos(fai)*cos(omi);
R=[a1,b1,c1;a2,b2,c2;a3,b3,c3]
for n=1:1:4
    s1=[Xd(n)-Xs,Yd(n)-Ys,Zd(n)-Zs]';
X_(n)=[a1,b1,c1]*s1;
Y_(n)=[a2,b2,c2]*s1;
Z_(n)=[a3,b3,c3]*s1;
x(n)=-f*X_(n)/Z_(n);
y(n)=-f*Y_(n)/Z_(n);
end
%以上循环用于求解像空间坐标x(1),y(1)到x(4),y(4)
for p=1:1:4