resecl.asv

这是用于遥感影像的后方交汇计算的程序

[FileName,PathName]=uigetfile('*ao.txt','打开文件');
if FileName==0
    return
end
fn=fopen([PathName,FileName]);
Num=str2num(fgetl(fn));
for i=1:Num
    Code(i,:)=str2num(fgetl(fn)); 
end
fclose(fn);
deltat=sqrt((Code(1,1)-Code(2,1))^2+(Code(1,2)-Code(2,2))^2);
deltad=sqrt((Code(1,3)-Code(2,3))^2+(Code(1,4)-Code(2,4))^2);
m=deltat/deltad;
sigmaz=0;
sigmax=0;
sigmay=0;
for i=1:Num
    sigmaz=Code(i,5)+sigmaz;
    sigmax=Code(i,3)+sigmax;
    sigmay=Code(i,4)+sigmay;
end
sigmaz=sigmaz/Num;
sigmax=sigmax/Num;
sigmay=sigmay/Num;
f=str2num(get(edit3,'String'));
H=(1/m)*f+sigmaz;
x0=str2num(get(edit1,'String'));
y0=str2num(get(edit2,'String'));
phy=0;
omega=0;
kapa=0;
Zs0=H;
Xs0=sigmax;
Ys0=sigmay;
for k=1:100
R(1,1)=cos(phy)*cos(kapa)-sin(phy)*sin(omega)*sin(kapa);
R(1,2)=-cos(phy)*sin(kapa)-sin(phy)*sin(omega)*cos(kapa);
R(1,3)=-sin(phy)*cos(omega);
R(2,1)=cos(omega)*sin(kapa);
R(2,2)=cos(omega)*cos(kapa);
R(2,3)=-sin(omega);
R(3,1)=sin(phy)*cos(kapa)+cos(phy)*sin(omega)*sin(kapa);
R(3,2)=-sin(phy)*sin(kapa)+cos(phy)*sin(omega)*cos(kapa);
R(3,3)=cos(phy)*cos(omega);
for i=1:Num
    x0(i,1)=-f*((R(1,1)*(Code(i,3)-Xs0))+(R(2,1)*(Code(i,4)-Ys0))+(R(3,1)*(Code(i,5)-Zs0)))/((R(1,3)*(Code(i,3)-Xs0))+(R(2,3)*(Code(i,4)-Ys0))+(R(3,3)*(Code(i,5)-Zs0)));
    y0(i,1)=-f*((R(1,2)*(Code(i,3)-Xs0))+(R(2,2)*(Code(i,4)-Ys0))+(R(3,2)*(Code(i,5)-Zs0)))/((R(1,3)*(Code(i,3)-Xs0))+(R(2,3)*(Code(i,4)-Ys0))+(R(3,3)*(Code(i,5)-Zs0)));
    zba=(R(1,3)*(Code(i,3)-Xs0)+R(2,3)*(Code(i,4)-Ys0)+R(3,3)*(Code(i,5)-Zs0));
    a11=(R(1,1)*f+R(1,3)*Code(i,1))/zba;
    a12=(R(2,1)*f+R(2,3)*Code(i,1))/zba;
    a13=(R(3,1)*f+R(3,3)*Code(i,1))/zba;
    a21=(R(1,2)*f+R(1,3)*Code(i,2))/zba;
    a22=(R(2,2)*f+R(2,3)*Code(i,2))/zba;
    a23=(R(3,2)*f+R(3,3)*Code(i,2))/zba;
    a14=Code(i,2)*sin(omega)-(Code(i,1)*(Code(i,1)*cos(kapa)-Code(i,2)*sin(kapa))/f+f*cos(kapa))*cos(omega);
    a15=-f*sin(kapa)-Code(i,1)*(Code(i,1)*sin(kapa)+Code(i,2)*cos(kapa))/f;
    a16=Code(i,2);
    a24=-Code(i,1)*sin(omega)-(Code(i,2)*(Code(i,1)*cos(kapa)-Code(i,2)*sin(kapa))/f-f*sin(kapa))*cos(omega);
    a25=-f*cos(kapa)-Code(i,2)*(Code(i,1)*sin(kapa)+Code(i,2)*cos(kapa))/f;
    a26=-Code(i,1);
    lx=Code(i,1)-x0(i,1);
    ly=Code(i,2)-y0(i,1);
    A(i).a=[a11 a12 a13 a14 a15 a16;a21 a22 a23 a24 a25 a26];
    L(i).l=[lx;ly];
end
B=A(1).a;
lb=L(1).l;
for i=2:Num
    B=[B;A(i).a];
    lb=[lb;L(i).l];
end
dX=inv(B'*B)*B'*lb;
Xs0=Xs0+dX(1,1);
Ys0=Ys0+dX(2,1);
Zs0=Zs0+dX(3,1);
phy=phy+dX(4,1);
omega=omega+dX(5,1);
kapa=kapa+dX(6,1);
if dX(1,1)<1&dX(2,1)<1&dX(3,1)<1&dX(4,1)<0.00001&dX(5,1)<0.00001&dX(6,1)<0.00001
    break;
end
end
Xs0.l1=Xs0;
Ys0.l1=Ys0;
Zs0.l1=Zs0;
phy.l1=phy;
omega.l1=omega;
kapa.l1=kapa;
R1=R;
set(text7,'String',{Xs0.l1;Ys0.l1;Zs0.l1;phy.l1;omega.l1;kapa.l1});
clear