hj104215.m

摄影测量学中单像空间后方交会程序

k=0;
f=0.15;
H=30000*0.15;
Px= [-0.091007;-0.093708;0.094194;0.09239];
Py=[-0.077759;0.066986;-0.091534;0.07773];
X0=[103000.00;140000.00;4800.00;0;0;0];
ZBX=[100021.00;100009.00;105972.00;105865.00;];
ZBY=[137408.00;142047.00;136984.00;142336.00;];
ZBZ=[16.00;34.00;42.00;55.00;];
Xs=[0 0 0 1 1 1];
fd=fopen('E:/wxy.txt','a');
%***************************************************************循环结束判断条件
while abs(Xs(4))>(1/60/180*pi)|abs(Xs(5))>(1/60/180*pi)|Xs(6)>(1/60/180*pi) 
%*****************************************************************计算旋转矩阵
a1=cos(X0(4))*cos(X0(6))-sin(X0(4))*sin(X0(5))*sin(X0(6));
a2=-cos(X0(4))*sin(X0(6))-sin(X0(4))*sin(X0(5))*cos(X0(6));
a3=-sin(X0(4))*cos(X0(5));
b1=cos(X0(5))*sin(X0(6));
b2=cos(X0(5))*cos(X0(6));
b3=-sin(X0(5));
c1=sin(X0(4))*cos(X0(6))+cos(X0(4))*sin(X0(5))*sin(X0(6));
c2=-sin(X0(4))*sin(X0(6))+cos(X0(4))*sin(X0(5))*cos(X0(6));
c3=cos(X0(4))*cos(X0(5));
%*************************************************计算矩阵A和像点坐标的近似值
for n=1:4
    x=Px(n);
    y=Py(n);
    Z=a3*(ZBX(n)-X0(1))+b3*(ZBY(n)-X0(2))+c3*(ZBZ(n)-X0(3));
    A(2*n-1,1)=(a1*f+a3*x)/Z;
    A(2*n-1,2)=(b1*f+b3*x)/Z;
    A(2*n-1,3)=(c1*f+c3*x)/Z;
    A(2*n-1,4)=y*sin(X0(5))-(x*(x*cos(X0(6))-y*sin(X0(6)))/f+f*cos(X0(6)))*cos(X0(5));
    A(2*n-1,5)=-f*sin(X0(6))-x*(x*sin(X0(6))+y*cos(X0(6)))/f;
    A(2*n-1,6)=y;
    A(2*n,1)=(a2*f+a3*y)/Z;
    A(2*n,2)=(b2*f+b3*y)/Z;
    A(2*n,3)=(c2*f+c3*y)/Z;
    A(2*n,4)=-x*sin(X0(5))-(y*(x*cos(X0(6))-y*sin(X0(6)))/f-f*sin(X0(6)))*cos(X0(5));
    A(2*n,5)=-f*cos(X0(6))-y*(x*sin(X0(6))+y*cos(X0(6)))/f;
    A(2*n,6)=-x;
    pl(2*n-1)=-f*(a1*(ZBX(n)-X0(1))+b1*(ZBY(n)-X0(2))+c1*(ZBZ(n)-X0(3)))/Z;
    pl(2*n)=-f*(a2*(ZBX(n)-X0(1))+b2*(ZBY(n)-X0(2))+c2*(ZBZ(n)-X0(3)))/Z;
    pL(2*n-1)=Px(n);
    pL(2*n)=Py(n);
end
L=pL'-pl';
Xs=inv(A'*A)*(A'*L); %******************************************计算外方位元素
X0=X0+Xs;
k=k+1;
fprintf(fd,'迭代中间结果\r\n')
fprintf(fd,'Xs(1)=%d\r\nXs(2)=%d\r\nXs(3)=%d\r\nXs(4)=%d\r\nXs(5)=%d\r\nXs(6)=%d\r\n',...
    Xs(1),Xs(2),Xs(3),Xs(4),Xs(5),Xs(6));
end
format long
X0(4)=rad2dms(X0(4))/100;
X0(5)=rad2dms(X0(5))/100;
X0(6)=rad2dms(X0(6))/100;
fprintf(fd,'迭代次数为：%d\r\n',k);
fprintf(fd,'结果为:\r\nX0(1)=%d(米)\r\nX0(2)=%d(米)\r\nX0(3)=%d(米)\r\nX0(4)=%d(dms)\r\nX0(5)=%d(dms)\r\nX0(6)=%d(dms)\r\n',...
    X0(1),X0(2),X0(3),X0(4),X0(5),X0(6));
dos('start E:/wxy.txt' );