后方交会MATLAB程序

 clcclear;%清理命令窗口和工作区间
format long g;%规定输出数据为长整型
x0=0;y0=0;A=pi/2;%A=1.56363;
f=0.1014;%输入已知的内方位元素
%f=0.15277;
x=[-6.864-16.009-15.947-4.5748.3472.903]/1000;
%x=[25.8516.67616.82928.20541.16535.676]/1000;
y=[11.2587.638-3.0810.6111.57611.383]/1000;
%y=[14.16310.437-0.2883.5254.61814.384]/1000;
Xt=[85326.66485260.60085067.94485135.90585154.99885330.078];
Yt=[51255.51651091.31751093.64451298.15751529.32051430.771];
for n=1:6
X（n）=Xt（n）*cos（A）+Yt（n）*sin（A）;Y（n）=Xt（n）*sin（A）-Yt（n）*cos（A）;
end
Z=[72.05167.31665.42569.83374.33677.145];%输入地面点坐标值
xd=sqrt（（x（5）-x（2））^2+（y（5）-y（2））^2）;
Xd=sqrt（（X（5）-X（2））^2+（Y（5）-Y（2））^2）;
m=Xd/xd;%计算比例尺分母
Xs=（X（1）+X（2）+X（3）+X（4）+X（5）+X（6））/6;
Ys=（Y（1）+Y（2）+Y（3）+Y（4）+Y（5）+Y（6））/6;
Zs=m*f+（Z（1）+Z（2）+Z（3）+Z（4）+Z（5）+Z（6））/6;
phi=0;omi=0;kap=0;%计算外方位元素初始值
ci=0;
while 1
%进行迭代
ci=ci+1;
a1=cos（phi）*cos（kap）-sin（phi）*sin（omi）*sin（kap）; 
a2=-cos（phi）*sin（kap）-sin（phi）*sin（omi）*cos（kap）; a3=-sin（phi）*cos（omi）;
b1=cos（omi）*sin（kap）; b2=cos（omi）*cos（kap） ;b3=-sin（omi）;
c1=sin（phi）*cos（kap）+cos（phi）*sin（omi）*sin（kap） ;
c2=-sin（phi）*sin（kap）+cos（phi）*sin（omi）*cos（kap）; c3=cos（phi）*cos（omi）;
R=[a