?%===========單像空間后方交會===================
?%===未知數：單張像片六個外方位元素，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