摄影测量后方交会MATLAB

%aa = textread('物方控制点坐标.txt');

fid=fopen('CONTROL.txt');  %打开数据总文件
B=textscan(fid,'%f %f %f %f');%把每一列的数据读入到读入到单元数组B中
C=[B{2} B{3} B{4}];          %从单元数组B中提取每列数据赋值给矩阵C
n=max(size(C));         %N为矩阵的行总数也就是控制点位个数Xa

fid=fopen('IAMGE.txt');  %打开数据总文件
J=textscan(fid,'%f %f %f');%把每一列的数据读入到读入到单元数组B中
I=[J{2} J{3}];          %从单元数组B中提取每列数据赋值给矩阵C        %确定读入数据的坐标数目 

%Cp为平均值
Cpx1=sum(C);%控制点的X,Y,Z总和
Xap=Cpx1(1,1)/n;%控制点平均X average
Yap=Cpx1(1,2)/n;
Zap=Cpx1(1,3)/n;

%要求待定参数Xs=3373.40082 mm,Ys=-141.55657 mm,Zs=92.77483 mm,Phi,Omega,Kappa
%像方坐标，控制点，Xa,Ya,Za,delta

%设定外方位元素初始值 
x0 =   47.4857    ;
y0 =   12.0276    ;
f  =  4547.9352   ;
Xs=3370;
Ys=-140;
Zs=92;
Phi=0;
Omega=0;
Kappa=0;

%RPhi=[cos(Phi),0,-sin(Phi);0,1,0;sin(Phi),0,cos(Phi)];
%ROmega=[1,0,0;0,cos(Omega),-sin(Omega);0,sin(Omega),cos(Omega)];
%RKappa=[cos(Kappa),-sin(Kappa),0;sin(Kappa),cos(Kappa),0;0,0,1];

%R=RPhi*ROmega*RKappa;
%a1=R(1,1);
%a2=R(1,2);
%a3=R(1,3);

q=Phi;
w=Omega;
k=Kappa;
zy=0;

while 1

for j=n:-1:1
i=n-j+1;
a(1)=cos(q)*cos(k)-sin(q)*sin(w)*sin(k);
a(2)=-cos(q)*sin(k)-sin(q)*sin(w)*cos(k);
a(3)=-sin(q)*cos(w);
b(1)=cos(w)*sin(k);
b(2)=cos(w)*cos(k);
b(3)=-sin(w);
c(1)=sin(q)*cos(k)+cos(q)*sin(w)*sin(k);
c(2)=-sin(q)*sin(k)+cos(q)*sin(w)*cos(k);
c(3)=cos(q)*cos(w);
%以i开始迭代Control坐标
%开始迭代
Xbar(i)=a(1)*(C(i,1)-Xs)+b(1)*(C(i,2)-Ys)+c(1)*(C(i,3)-Zs);
Ybar(i)=a(2)*(C(i,1)-Xs)+b(2)*(C(i,2)-Ys)+c(2)*(C(i,3)-Zs);
Zbar(i)=a(3)*(C(i,1)-Xs)+b(3)*(C(i,2)-Ys)+c(3)*(C(i,3)-Zs);

H(i)=-Zbar(i);
%H赋予初值
%误差方程的系数aij改为xsij
xs(2*i-1,1)=-f/H(i);
xs(2*i-1,2)=0;
xs(2*i-1,3)=-(I(i,1)-x0)/H(i);
xs(2*i-1,4)=-(f+(I(i,1)-x0)*(I(i,1)-x0)/f);
xs(2*i-1,5)=-(I(i,1)-x0)*(I(i,2)-y0)/f;
xs(2*i-1,6)=I(i,2)-y0;

xs(2*i,1)=0;
xs(2*i,2)=-f/H(i);
xs(2*i,3)=-(I(i,2)-y0)/H(i);
xs(2*i,4)=-(I(i,1)-x0)*(I(i,2)-y0)/f;
xs(2*i,5)=-(f+(I(i,2)-y0)*(I(i,2)-y0)/f);
xs(2*i,6)=-(I(i,1)-x0);

%L=[x0-I(i,1),y0-I(i,2)];
L(2*i-1,1)=x0-I(i,1);
L(2*i,1)=y0-I(i,2);
%此时L为矩阵2n*1
end

%=[deltaXs;deltaYs;deltaZs;deltaq;deltaw;deltak];
N=xs'*xs;
%X=pinv(N)*xs'*L;
[Q,R]=qr(xs);
X=pinv(R)*Q'*L;
deltaXs=X(1,1);
deltaYs=X(2,1);
deltaZs=X(3,1);
deltaq=X(4,1);
deltaw=X(5,1);
deltak=X(6,1);

Xs=deltaXs+Xs;
Ys=deltaYs+Ys;
Zs=deltaZs+Zs;
q=q+deltaq;
w=w+deltaw;
k=k+deltak;
zy=zy+1;
PD(1)=deltaq/pi*60;
PD(2)=deltaw/pi*60;
PD(3)=deltak/pi*60;
if PD(1)<0.1&&PD(2)<0.1&&PD(3)<0.1
    break;
end

end

Sigmav=0;
 for zx=1:n:1
    v(2*zx-1,1)=xs(1,:)*X-L(2*zx-1,1);   
    %v矩阵为2n行一列
    Sigmav=Sigmav+v(2*zx-1,1);
 end

 m0=sqrt(Sigmav/(n-6));

fid111=fopen('jieguo.txt','wt');
%%%%需要改文件名称的地方
fprintf(fid111,' %f %f %f %f %f %f\n %f',Xs,Ys,Zs,q,w,k,m0);      %%%%ve：需要导出的数据名称