单向空间后方交会matlab编程.docx

%读取包含点号的像点坐标、控制点坐标分别到矩阵IP、CP% IP=load(,f:1)； CP=load(,f:,)； %删除像点坐标、控制点坐标矩阵中的点号列％ IP(:,1 冃]; cp(：,i)=n； %像片大小％ H 二4008; W=5344; IP1=IP(:J卜 W/2; ￥ IP2=H/2.|P(:,2); IP=[IP1JP2]; %焦距％ fx=； %内方位元素％ x0=; yo=； %畸变参数％ kl=; k2=; pl=； p2=; %外方位元素初值％ Xs=3000; Ys=-100; Zs=100; Phi=0; Omega=0; Kappa=0; I m=size(IFl); AIP=zeros(m/2); L=zeros(2*m,l)； A=zeros(2*m,6); X=ones(6zl); while X(4,l)=| |X(5fl)=l ^(64)= %旋转矩阵的系数％ al=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); bl=cos(Omega)*sin(Kappa); b2=cos(Omega)*cos(Kappa); b3=?sin(Omega); cl=sin(Phi)*cos(Kappa)+cos(Phi)*sin(Omega)*sin(Kappa); c2=-sin(Phi)*sin(Kappa)+cos(Phi)*sin(0mega)*cos(Kappa); c3=cos(Phi)*cos(Omega); for i=l:m %求像点坐标常数项％ r=sqrt((IP(izl)-xO)A2+(IP(i/2)-yO)A2); Dx=(IP(i/l)-xO)*(kl*(rA2)+k2*(rA4))+pl*(rA2+2*((IP(i/l)-xO)A2))+2*p2*(IP(i/l)-xO)*(IP(i/2)-yO); Dy=(IP(i/2)-yO)*(kl*(rA2)+k2*(rA4))+p2*(rA2+2*((IP(i/2)-yO)A2))+2*pl*(IP(i/l)-xO)*(IP(i/2)-yO); X-=al*(CP(i/l)-Xs)+bl*(CP(i/2)-Ys)+cl*(CP(i/3)-Zs); Y-=a2*(CP(i/l)-Xs)+b2*(CP(i/2)-Ys)+c2*(CP(i/3)-Zs); Z-=a3*(CP(i/l)-Xs)+b3*(CP(i/2)-Ys)+c3*(CP(i/3)-Zs); %求像点坐标的近似值％ AIP(izl)=-fx*X JZ_+Dx+xO; AIP(i,2)=-fy*YVZ_+Dy+yO; %求误差方程式系数％ all=(al*fx+a3#IP(i/l))/Z_; al2=(bl*fx+b3*IP(i/l))/Z」 ( al3=(cl*fx+c3*IP(i/l))/Z_; al4=sin(Omega)*IP(i/2)-(IP(i/l)*(IP(i/l)*cos(Kappa)-IP(i/2)*sin(Kappa))/fx+fx*cos(Kappa))*cos(Om ega)； al5=-fx*sin(Kappa)-IP(i/l)*(IP(i/l)*sin(Kappa)+IP(i/2)*cos(Kappa))/fx; al6=IP(i，2); a21=(a2*fy+a3*IP(i/2))/ZJ a22=(b2*fy+b3*IP(iz2))/Zj a23=(c2*fy+c3*IP(iz2))/Z a24=-sin(Omega)*IP(i/l)-(IP(i/2)*(IP(i/l)*cos(Kappa)-IP(i/2)*sin(Kappa))/fy-fy*sin(Kappa))*cos{Om ega); a25=-fy*cos(Kappa)-IP(i/2)*(IP(i/l)*sin(Kappa)+IP(i/2)*cos(Kappa))/fy; a26=-IP(U); %求常数项％ L(2*i-l/l)=IP(i,l)-AIP(i/l)； L(2*i,l)=IP(i/2)-AIP(i,2); %求误差方程式系数阵％ A(2*i-l,l)=all; A(2*i-l,2)=al2; A(2*i-13)=al3; A(2*i-l,4)=al4; A(2*i-l,5)=al5; A(2*i-l,6)=al6; A A(2*i4)=a21; A(2*iz2)=a22; A(2*b3)=a23; A(2*i,4)=a24; A(2*b5)=a