多像空间前方交会（共线条件方程法)

clc,clear;%清理命令窗口和工作区间
format long g;%规定输出数据为长整型
x=[17.383 18.675 19.424 13.981 11.178 11.199;-4.706 -3.428 -2.722 -8.173 -10.947 -10.864]/1000;
y=[8.396 4.516 -0.855 -3.870 -0.068 6.970;6.444 2.548 -2.826 -5.793 -1.953 5.081]/1000;
%代入左右像片的同名像点坐标
f=0.1014;%主距f
x0=-0.180/1000;y0=0;%内方位元素
Xs=[50766.27048297550;51406.58175310970];
Ys=[85092.60204012240;85098.60787072870];
Zs=[2764.51598579742;2763.02759736577];%代入左右两张像片外方位元素中的线元素值
phi=[0.00749335445825581;-0.01158295898298480];
omi=[-0.00699425949527119;0.01003605897533120];
kap=[-0.00213195906904753;0.00849024086417501];%代入左右两张像片外方位元素中的角元素值
B=[];L=[];
for i=1:2
a1(i)=cos(phi(i))*cos(kap(i))-sin(phi(i))*sin(omi(i))*sin(kap(i));
a2(i)=-cos(phi(i))*sin(kap(i))-sin(phi(i))*sin(omi(i))*cos(kap(i));
a3(i)=-sin(phi(i))*cos(omi(i));
b1(i)=cos(omi(i))*sin(kap(i));b2(i)=cos(omi(i))*cos(kap(i));
b3(i)=-sin(omi(i));
c1(i)=sin(phi(i))*cos(kap(i))+cos(phi(i))*sin(omi(i))*sin(kap(i));
c2(i)=-sin(phi(i))*sin(kap(i))+cos(phi(i))*sin(omi(i))*cos(kap(i));
c3(i)=cos(phi(i))*cos(omi(i));%计算旋转矩阵R1,R2中各系数
end
for j=1:6
for i=1:2
l1(i,j)=f*a1(i)+(x(i,j)-x0)*a3(i);
l2(i,j)=f*b1(i)+(x(i,j)-x0)*b3(i);
l3(i,j)=f*c1(i)+(x(i,j)-x0)*c3(i);
lx(i,j)=f*a1(i)*Xs(i)+f*b1(i)*Ys(i)+f*c1(i)*Zs(i)+(x(i,j)-x0)*a3(i)*Xs(i)+(x(i,j)-x0)*b3(i)*Ys(i)+(x(i,j)-x0)*c3(i)*Zs(i);