单向空间后方交会matlab编程

%读取包含点号的像点坐标、控制点坐标分别到矩阵IP、CP%IP=load('f:imgpoint.txt');CP=load('f:ctlpoint.txt');%删除像点坐标、控制点坐标矩阵中的点号列%IP(:,1)=[];CP(:,1)=[];%像片大小%H=4008;W=5344;IP1=IP(:,1)-W/2;IP2=H/2-IP(:,2);IP=[IP1,IP2];%焦距%fx=4547.99665;fy=4547.87373;%内方位元素%x0=47.48571;y0=12.02756;%畸变参数%k1=-5.00793e-009;k2=1.83462e-016;p1=-2.24419e-008;p2=1.76820e-008;%外方位元素初值%Xs=3000;Ys=-100;Zs=100;Phi=0;Omega=0;Kappa=0;m=size(IP,1);AIP=zeros(m,2);L=zeros(2*m,1);A=zeros(2*m,6);X=ones(6,1);whileX(4,1)=6.0/206265.0||X(5,1)=6.0/206265.0||X(6,1)=6.0/206265.0%旋转矩阵的系数%a1=cos(Phi)*cos(Kappa)-sin(Phi)*sin(Omega)*sin(Kappa);a2=-cos(Phi)*sin(Kappa)-sin(Phi)*sin(Omega)*cos(Kappa);a3=-sin(Phi)*cos(Omega);b1=cos(Omega)*sin(Kappa);b2=cos(Omega)*cos(Kappa);b3=-sin(Omega);c1=sin(Phi)*cos(Kappa)+cos(Phi)*sin(Omega)*sin(Kappa);c2=-sin(Phi)*sin(Kappa)+cos(Phi)*sin(Omega)*cos(Kappa);c3=cos(Phi)*cos(Omega);fori=1:m%求像点坐标常数项%r=sqrt((IP(i,1)-x0)^2+(IP(i,2)-y0)^2);Dx=(IP(i,1)-x0)*(k1*(r^2)+k2*(r^4))+p1*(r^2+2*((IP(i,1)-x0)^2))+2*p2*(IP(i,1)-x0)*(IP(i,2)-y0);Dy=(IP(i,2)-y0)*(k1*(r^2)+k2*(r^4))+p2*(r^2+2*((IP(i,2)-y0)^2))+2*p1*(IP(i,1)-x0)*(IP(i,2)-y0);X_=a1*(CP(i,1)-Xs)+b1*(CP(i,2)-Ys)+c1*(CP(i,3)-Zs);Y_=a2*(CP(i,1)-Xs)+b2*(CP(i,2)-Ys)+c2*(CP(i,3)-Zs);Z_=a3*(CP(i,1)-Xs)+b3*(CP(i,2)-Ys)+c3*(CP(i,3)-Zs);%求像点坐标的近似值%AIP(i,1)=-fx*X_/Z_+Dx+x0;AIP(i,2)=-fy*Y_/Z_+Dy+y0;%求误差方程式系数%a11=(a1*fx+a3*IP(i,1))/Z_;a12=(b1*fx+b3*IP(i,1))/Z_;a13=(c1*fx+c3*IP(i,1))/Z_;a14=sin(Omega)*IP(i,2)-(IP(i,1)*(IP(i,1)*cos(Kappa)-IP(i,2)*sin(Kappa))/fx+fx*cos(Kappa))*cos(Omega);a15=-fx*sin(Kappa)-IP(i,1)*(IP(i,1)*sin(Kappa)+IP(i,2)*cos(Kappa))/fx;a16=IP(i,2);a21=(a2*fy+a3*IP(i,2))/Z_;a22=(b2*fy+b3*IP(i,2))/Z_;a23=(c2*fy+c3*IP(i,2))/Z_;a24=-sin(Omega)*IP(i,1)-(IP(i,2)*(IP(i,1)*cos(Kappa)-IP(i,2)*sin(Kappa))/fy-fy*sin(Kappa))*cos(Omega);a25=-fy*cos(Kappa)-IP(i,2)*(IP(i,1)*sin(Kappa)+IP(i,2)*cos(Kappa))/fy;a26=-IP(i,1);%求常数项%L(2*i-1,1)=IP(i,1)-AIP(i,1);L(2*i,1)=IP(i,2)-AIP(i,2);%求误差方程式系数阵%A(2*i-1,1)=a11;A(2*i-1,2)=a12;A(2*i-1,3)=a13;A(2*i-1,4)=a14;A(2*i-1,5)=a15;A(2*i-1,6)=a16;A(2*i,1)=a21;A(2*i,2)=a22;A(2*i,3)=a23;A(2*i,4)=a24;A(2*i,5)=a25;A(2*i,6)=a26;end%求改正数%X=pinv(A'*A)*A'*L;%求真值%Xs=Xs+X(1,1);Ys=Ys+X(2,1);Zs=Zs+X(3,1);Phi=Phi+X(4,1);Omega=Omega+X(5,1);Kappa=Kappa+X(6,1);end%求误差矩阵%V=A*X-L;%精度评定%m0=abs(sqrt(V'*V/(2*m-6)));Q=inv(A'*A);mi=m0*sqrt(Q);%{fprintf('%.6f\n',A);%%fprintf('\n');%fprintf('%.6f\n%.6f\n%.6f\n%.6f\n%.6f\n%.6f\n',Xs,Ys,Zs,Phi,Omega,Kappa);fprintf('\n');%fprintf('%.6f\n',L);%%fprintf('\n');%%fprintf('%.6f\n',V);%%fprintf('\n');%fprintf('%.6f\n',mi);