单片空间后方交会matlab程序
f=0.15;fai=0;omi=0;ka=0;
Xd=[5083.205,5780.02,5210.879,5909.264];
Yd=[5852.099,5906.365,4258.446,4314.283];
Zd=[527.925,571.549,461.81,455.484];
%以上为输入gcp1到gcp4的地面坐标
xx=[-0.07393,-0.005252,-0.079122,-0.009887];
yx=[0.078705,0.078184,-0.078879,-0.080089];
%右片像点坐标
Xs=(5083.205+5780.02+5210.879+5909.264)/4;
Ys=(5852.099+5906.365+4258.446+4314.283)/4;
Sx=sqrt((xx(2)-xx(1))^2+(yx(2)-yx(1))^2);
Sd=sqrt((Xd(2)-Xd(1))^2+(Yd(2)-Yd(1))^2);
m=Sd/Sx
Zs=m*f+1/4*(Zd(1)+Zd(2)+Zd(3)+Zd(4));
X=[1;1;1;1;1;1];
while 1
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);
R=[a1,b1,c1;a2,b2,c2;a3,b3,c3]
for n=1:1:4
s1=[Xd(n)-Xs,Yd(n)-Ys,Zd(n)-Zs]';
X_(n)=[a1,b1,c1]*s1;
Y_(n)=[a2,b2,c2]*s1;
Z_(n)=[a3,b3,c3]*s1;
x(n)=-f*X_(n)/Z_(n);
y(n)=-f*Y_(n)/Z_(n);
end
%以上循环用于求解像空间坐标x(1),y(1)到x(4),y(4)
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)*(a2*f+a3*y(p)); a22(p)=1/Z_(p)*(b2*f+b3*y(p)); a23(p)=1/Z_(p)*(c2*f+c3*y(p));
a14(p)=y(p)*sin(omi)-(x(p)/f*(x(p)*cos(ka)-y(p)*sin(ka))+f*cos(ka))*cos(omi);
a15(p)=-f*sin(ka)-x(p)/f*(x(p)*sin(ka)+y(p)*cos(ka)); %计算A矩阵
a16(p)=y(p);
a24(p)=-x(p)*sin(omi)-(y(p)/f*(x(p)*cos(ka)-y(p)*sin(ka))-f*sin(ka))*cos(omi);
a25(p)=-f*cos(ka)-y(p)/f*(x(p)*sin(ka)+y(p)*cos(ka));
a26(p)=-x(p) ;
%右片
end
A=[a11(1),a12(1),a13(1),a14(1),a15(1),a16(1);a21(1),a22(1),a23(1),a24(1),a25(1),a26(1);a11(2),a12(2),a13(2),a14(2),a15(2),a16(2);a21(2),a22(2),a23(2),a24(2),a25(2),a26(2);a11(3),a12(3),a13(3),a14(3),a15(3),a16(3);a21(3),a22(3),a23(3),a24(3),a25(3),a26(3);a11(4),a12(4),a13(4),a14(4),a15(4),a16(4);a21(4),a22(4),a23(4),a24(4),a25(4),a26(4)]
lx=xx-x;
ly=yx-y;
%右片
L=[lx(1),ly(1),lx(2),ly(2),lx(3),ly(3),lx(4),ly(4)]'
X=inv(A'*A)*A'*L
dx=X(1,1);dy=X(2,1);dz=X(3,1);df=X(4,1);do=X(5,1);dk=X(6,1);
Xs=Xs+dx;
Ys=dy+Ys ;
Zs=dz+Zs;
fai=fai+df;
omi=omi+do;
ka=ka+dk;
if all(abs(X)<0.0003)
break;
end
end
V=A*X-L
Xs=Xs+dx
Ys=dy+Ys
Zs=dz+Zs
fai=fai+df
omi=omi+do
ka=ka+dk
VV=0;
for n=1:8
VV=VV+V(n,1)*V(n,1);
end
Qii=inv(A'*A)
m0=sqrt(VV/2)
mi=m0*sqrt(Qii)