单片空间后方交会matlab

 %===========单像空间后方交会===================
 %===未知数：单张像片六个外方位元素，Xs，Ys，Zs，fai，omi，ka
%===迭代方程，X=inv（A‘*A）*A‘*L，X=[dXs，dYs，dZs，dfai，domi，dka] Xk+1=Xk+X
 %===未知量初值：Xs，Ys，Zs=控制点坐标平均值，fai，omi，ka=0
 %===输出：六个外方位元素
                                                                                                                                    
%=============已知数据
%输入GCP1到GCP4的地面坐标即XYZ，四个地面控制点
Xd=[36589.4137631.0839100.9740426.54];
Yd=[25237.3231324.5124934.9830319.81];
Zd=[2195.17728.692386.50757.31];
 
%输入像点坐标，即x，y，四个像点
xx=[-0.08615-0.05340-0.014780.01046];
yx=[-0.068990.08221-0.076630.06443];
f=0.15324;
 
%=============未知数赋初值
%%%%%%%设置Xs，Ys，Zs的初值初值通常是通过平均计算得到
Xs=（36589.41+37631.08+39100.97+40426.54）/4;
Ys=（25237.32+31324.51+24934.98+30319.81）/4;
Sx=sqrt（（xx（3）-xx（1））^2+（yx（2）-yx（1））^2）;%%%%两像点之间距离
Sd=sqrt（（Xd（3）-Xd（1））^2+（Yd（2）-Yd（1））^2）;%%%%两地面控制点之间距离
m=Sd/Sx; %%%%图像比例系数
Zs=m*f+1/4*（Zd（1）+Zd（2）+Zd（3）+Zd（4））;
fai=0;omi=0;ka=0;
%=============初始化输出量
X=[1;1;1;1;1;1];
 
%=============迭代 ??
while 1
%%%%%%%计算方向余弦，求出R （公式3-9  p38页）
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）;
 
%%%%%%%%循环用于求解像空间坐标x（1）y（1）到x（4）y（4）根据像空系与地辅系之间的关系，求解x（n），y（n）******（p41页 公式3-15）
for n=1:1:4
        s1=[Xd（n）-XsYd（n）-YsZd（n）-Zs]‘;
X_（n）=[a1b1c1]*s1;
Y_（n）=[a2b2c2]*s1;
Z_（n）=[a3b3c3]*s1;
x（n）=-f*X_（n）/Z_（n）;
y（n）=-f*Y_（n）/Z_（n）;
end
 
%%%%%%%%对每个控制点计算误差方程式系数和aij和lx、ly
for p=1:1:4
 
     a11（p）=1/Z_（p）*（a1*f+a3*x（p））;
 a12（p）=1/Z_（p）*（b1*f+b3*x（p））;
a13（p）=1/Z_（p）*（c1*f+c3*x（p））;
     a21（p）=1/Z_（p）*（a