matlab 空间后方交会,摄影测量 单像空间后方交会 matlab编程

x=[-86.15 -53.40 -14.78 10.46];

y=[-68.99 82.21 -76.63 64.43];

X=[36589.41 37631.08 39100.97 40426.54];

Y=[25273.32 31324.51 24934.98 30319.81];

Z=[2195.17 728.69 2386.50 757.31];

f=153.24;

s=0.0;

S=0.0;

for i=1:3;

j=i+1;

sij=sqrt((x(i)-x(j))^2+(y(i)-y(j))^2);

Sij=sqrt((X(i)-X(j))^2+(Y(i)-Y(j))^2);

s=s+sij;

S=S+Sij;

end

m=S*1000/s

Xs0=0.0;

Ys0=0.0;

for k=1:4;

Xs0=Xs0+X(k);

Ys0=Ys0+Y(k);

end

Xs0=Xs0/4;

Ys0=Ys0/4;

Zs0=m*f;

fai0=0;

omig0=0;

ka0=0;

for u=1:+inf;

a1=cos(fai0)*cos(ka0)-sin(fai0)*sin(omig0)*sin(ka0);

a2=-cos(fai0)*sin(ka0)-sin(fai0)*sin(omig0)*cos(ka0);

a3=-sin(fai0)*cos(omig0);

b1=cos(omig0)*sin(ka0);

b2=cos(omig0)*cos(ka0);

b3=-sin(omig0);

c1=sin(fai0)*cos(ka0)+cos(fai0)*sin(omig0)*sin(ka0);

c2=-sin(fai0)*sin(ka0)+cos(fai0)*sin(omig0)*cos(ka0);

c3=cos(fai0)*cos(omig0);

R=[a1,a2,a3,;b1,b2,b3;c1,c2,c3];

L=[];

A=[];

for h=1:4;

O=a1*(X(h)-Xs0)+b1*(Y(h)-Ys0)+c1*(Z(h)-Zs0);

P=a2*(X(h)-Xs0)+b2*(Y(h)-Ys0)+c2*(Z(h)-Zs0);

Q=a3*(X(h)-Xs0)+b3*(Y(h)-Ys0)+c3*(Z(h)-Zs0);

x1h=-f*O/Q;%初值

y1h=-f*P/Q;

a11h=(a1*f+a3*x(h))/Q;

a12h=(b1*f+b3*x(h))/Q;

a13h=(c1*f+c3*x(h))/Q;

a14h=y(h)*sin(omig0)-(x(h)/f*(x(h)*cos(ka0)-y(h)*sin(ka0))+f*cos(ka0))*cos(omig0); a15h=-f*sin(ka0)-x(h)/f*(x(h)*sin(ka0)+y(h)*cos(ka0));

a16h=y(h);

a21h=(a2*f+a3*y(h))/Q;

a22h=(b2*f+b3*y(h))/Q;

a23h=(c2*f+c3*y(h))/Q;