单向空间后方交会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);while X(4,1)=6.0/.0|X(5,1)=6.0/.0|X(6,1)=6.0/.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); for i=1:m %求像点坐标常数项% r=sqrt(IP(i,1)-x0)2+(IP(i,2)-y0)2); Dx=(IP(i,1)-x0)*(k1*(r2)+k2*(r4)+p1*(r2+2*(IP(i,1)-x0)2)+2*p2*(IP(i,1)-x0)*(IP(i,2)-y0); Dy=(IP(i,2)-y0)*(k1*(r2)+k2*(r4)+p2*(r2+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(%.6fn,A);%fprintf(n);%fprintf(%.6fn%.6fn%.6fn%.6fn%.6f