单片空间后方交会Matlab程序

1.说明

本文是结合实例的单片空间后方交会的学习笔记。本文侧重流程的理解与公式的实现，所以有大量注释。至于空间后方交会的原理及其他问题可查看相关书籍《摄影测量学》。
参考书籍：《摄影测量学》（第二版）武汉大学出版社，张剑清，潘励，王树根。

2.代码

代码如下（示例）：

%单像空间后方交会 
%线元素10^-3,角元素10^-6 

%已知四对点的影像坐标和地面坐标如下。内方位元素fk=153.24mm,x0=y0=0。比例尺系数m=50000。
%             影像坐标                                              地面坐标
%         x(mm)            y(mm)                X(m)                Y(m)           Z(m)
%   1    -86.15           -68.99               36589.41         25273.32      2195.17 
%   2    -53.40            82.21               37631.08         31324.51      728.69 
%   3    -14.78           -76.63               39100.97         24934.98      2386.50
%   4     10.46            64.43               40426.54         30319.81      757.31
%试计算近似垂直摄影情况下空间后方交会的解。

disp('后方交会求解')%屏幕输出 

%****************************************************************
%第一步。获取已知数据。x0,y0,f,H,X,Y,Z
%第二步。量测控制点的像点坐标并进行必要的影像坐标系统误差改正，得到像点坐标x,y。
m=50000;%比例尺系数 
fk=153.24/1000;%主距修正单位 
H=m*fk;%航高 
[x,y,X,Y,Z]=textread('坐标.txt','%f %f %f %f %f');%读取文件

x=x/1000;y=y/1000;%修正单位 
limit1=0.001;limit2=0.000001;%改正值精度 
R=zeros(3,3);%旋转矩阵 
dx=ones(6,1);%改正值 
round=0;%迭代次数 
c=[x(1);y(1);x(2);y(2);x(3);y(3);x(4);y(4)];%方便L计算 
PI=3.1415926;

%****************************************************************
%第三步。确定未知数的初始值。Xs,Ys,Zs,q,w,k
Xs=sum(X)/4;Ys=sum(Y)/4;Zs=fk*m+sum(Z)/4;%待定参数初始值
q=0;w=0;k=0;%外方位角元素 
%****************************************************************

%循环条件：线元素改正值<=limit1且角元素改正值<=limit2 
while(abs(dx(1,1))>limit1||abs(dx(2,1))>limit1||abs(dx(3,1))>limit1||abs(dx(4,1))>limit2||abs(dx(5,1))>limit2||abs(dx(6,1))>limit2) 
 
%****************************************************************  
%第四步。计算旋转矩阵R
R(1,1)=cos(q)*cos(k)-sin(q)*sin(w)*sin(k); 
R(1,2)=-cos(q)*sin(k)-sin(q)*sin(w)*cos(k);
R(1,3)=-sin(q)*cos(w); 
R(2,1)=cos(w)*sin(k); 
R(2,2)=cos(w)*cos(k);
R(2,3)=-sin(w); 
R(3,1)=sin(q)*cos(k)+cos(q)*sin(w)*sin(k);
R(3,2)=-sin(q)*sin(k)+cos(q)*sin(w)*cos(k); 
R(3,3)=cos(q)*cos(w);  
%****************************************************************
%第五步。利用未知数的近似值按共线方程条件式计算控制点像点坐标的近似值x0，y0。 
xy_0=zeros(8,1);%共线方程。奇数为x-x0,偶数为y-y0 
xy_0(1,1)=-fk*(R(1,1)*(X(1)-Xs)+R(2,1)*(Y(1)-Ys)+R(3,1)*(Z(1)-Zs))/(R(1,3)*(X(1)-Xs)+R(2,3)*(Y(1)-Ys)+R(3,3)*(Z(1)-Zs)); 
xy_0(2,1)=-fk*(R(1,2)*(X(1)-Xs)+R(2,2)*(Y(1)-Ys)+R(3,2)*(Z(1)-Zs))/(R(1,3)*(X(1)-Xs)+R(2,3)*(Y(1)-Ys)+R(3,3)*(Z(1)-Zs)); 
xy_0(3,1)=-fk*(R(1,1)*(X(2)-Xs)+R(2,1)*(Y(2)-Ys)+R(3,1)*(Z(2)-Zs))/(R(1,3)*(X(2)-Xs)+R(2,3)*(Y(2)-Ys)+R(3,3)*(Z(2)-Zs)); 
xy_0(4,1)=-fk*(R(1,2)*(X(2)-Xs)+R(2,2)*(Y(2)-Ys)+R(3,2)*(Z(2)-Zs))/(R(1,3)*(X(2)-Xs)+R(2,3)*(Y(2)-Ys)+R(3,3)*(Z(2)-Zs)); 
xy_0(5,1)=-fk*(R(1,1)*(X(3)-Xs)+R(2,1)*(Y(3)-Ys)+R(3,1)*(Z(3)-Zs))/(R(1,3)*(X(3)-Xs)+R(2,3)*(Y(3)-Ys)+R(3,3)*(Z(3)-Zs)); 
xy_0(6,1)=-fk*(R(1,2)*(X(3)-Xs)+R(2,2)*(Y(3)-Ys)+R(3,2)*(Z(3)-Zs))/(R(1,3)*(X(3)-Xs)+R(2,3)*(Y(3)-Ys)+R(3,3)*(Z(3)-Zs)); 
xy_0(7,1)=-fk*(R(1,1)*(X(4)-Xs)+R(2,1)*(Y(4)-Ys)+R(3,1)*(Z(4)-Zs))/(R(1,3)*(X(4)-Xs)+R(2,3)*(Y(4)-Ys)+R(3,3)*(Z(4)-Zs)); 
xy_0(8,1)=-fk*(R(1,2)*(X(4)-Xs)+R(2,2)*(Y(4)-Ys)+R(3,2)*(Z(4)-Zs))/(R(1,3)*(X(4)-Xs)+R(2,3)*(Y(4)-Ys)+R(3,3)*(Z(4)-Zs));   
%****************************************************************
%第六步。逐点计算误差方程式的系数A和常数项L，组成误差方程式。
A=zeros(8,6);%系数阵 
for i=1:4 
    Xbar=R(1,1)*(X(i)-Xs)+R(2,1)*(Y(i)-Ys)+R(3,1)*(Z(i)-Zs); 
    Ybar=R(1,2)*(X(i)-Xs)+R(2,2)*(Y(i)-Ys)+R(3,2)*(Z(i)-Zs);
    Zbar=R(1,3)*(X(i)-Xs)+R(2,3)*(Y(i)-Ys)+R(3,3)*(Z(i)-Zs);   
    
    A(2*i-1,1)=1/Zbar*(R(1,1)*fk+R(1,3)*xy_0(2*i-1,1)); 
    A(2*i-1,2)=1/Zbar*(R(2,1)*fk+R(2,3)*xy_0(2*i-1,1)); 
    A(2*i-1,3)=1/Zbar*(R(3,1)*fk+R(3,3)*xy_0(2*i-1,1)); 
    A(2*i,1)=1/Zbar*(R(1,2)*fk+R(1,3)*xy_0(2*i,1)); 
    A(2*i,2)=1/Zbar*(R(2,2)*fk+R(2,3)*xy_0(2*i,1));
    A(2*i,3)=1/Zbar*(R(3,2)*fk+R(3,3)*xy_0(2*i,1)); 
    
    A(2*i-1,4)=xy_0(2*i,1).*sin(w)-(xy_0(2*i-1,1)./fk.*(xy_0(2*i-1,1).*cos(k)-xy_0(2*i,1).*sin(k))+fk*cos(k))*cos(w); 
    A(2*i-1,5)=-fk*sin(k)-xy_0(2*i-1,1)./fk.*(xy_0(2*i-1,1).*sin(k)+xy_0(2*i,1).*cos(k)); 
    A(2*i-1,6)=xy_0(2*i,1);  
    A(2*i,4)=-xy_0(2*i-1,1).*sin(w)-(xy_0(2*i,1)./fk.*(xy_0(2*i-1,1).*cos(k)-xy_0(2*i,1).*sin(k))-fk*sin(k))*cos(w);  
    A(2*i,5)=-fk*cos(k)-xy_0(2*i,1)./fk.*(xy_0(2*i-1,1).*sin(k)+xy_0(2*i,1).*cos(k)); 
    A(2*i,6)=-xy_0(i*2-1,1); 
end 
L=c-xy_0; %常数项
%****************************************************************
%第七步。计算法方程的系数矩阵与常数项。组成法方程。
dx=((A')*A)\(A')*L; 
%****************************************************************
%第八步。解求外方位元素改正数，并与相应的近似值求和，得到外方位元素新的近似值。 
Xs=Xs+dx(1,1);Ys=Ys+dx(2,1);Zs=Zs+dx(3,1); 
q=q+dx(4,1);w=w+dx(5,1);k=k+dx(6,1); %改正外方位元素
%****************************************************************
%第九步。规定限差限制迭代次数，检查计算是否收敛。
round=round+1; %计迭代次数
end
%****************************************************************

%写入文件
fid1=fopen('后方交会.txt','w');%输出外方位元素 
fprintf(fid1,'Xs = %f\r\n',Xs); 
fprintf(fid1,'Ys = %f\r\n',Ys); 
fprintf(fid1,'Zs = %f\r\n\r\n',Zs); 
fprintf(fid1,'q = %f\r\n',q);
fprintf(fid1,'w = %f\r\n',w); 
fprintf(fid1,'k = %f\r\n\r\n',k); 
fprintf(fid1,'round = %d\r\n',round); 
fclose(fid1);  

fid2=fopen('R阵.txt','w');%输出矩阵R 
fprintf(fid2,'\r\n'); 
for m=1:3  
    for n=1:3 
    fprintf(fid2,'%f  ',R(m,n));  
    end 
    fprintf(fid2,'\r\n');
end 
fclose(fid2);

fid3=fopen('精度评定.txt','w');%输出中误差 
fprintf(fid3,'dXs =±%f（mm）\r\n',1000*abs(dx(1,1))); 
fprintf(fid3,'dYs =±%f（mm）\r\n',1000*abs(dx(2,1))); 
fprintf(fid3,'dZs =±%f（mm）\r\n',1000*abs(dx(3,1))); 
fprintf(fid3,'\r\ndq =±%f（″）\r\n',abs(dx(4,1))*180*3600/PI); 
fprintf(fid3,'dw =±%f（″）\r\n',abs(dx(5,1))*180*3600/PI); 
fprintf(fid3,'dk =±%f（″）\r\n',abs(dx(6,1))*180*3600/PI); 
fclose(fid3); 
disp('完成')%屏幕输出 

3.程序文件，坐标文件及结果文件

https://download.csdn.net/download/weixin_44711096/12885747

---