matlab中双站异面直线法定位目标

calc.m

%% 参数信息初始化
[x1,y1,z1]=deal();
[x2,y2,z2]=deal(,,);
m1=/;
n1=/;
p1=^(/)/;
m2=;
n2=-^(/)/;
p2=^(/)/;

%% 开始计算各参数
A1=det([n1 p1;n2 p2]);
B1=det([p1 m1;p2 m2]);
C1=det([m1 n1;m2 n2]);

A2=det([n2 B1;p2 C1]);
B2=det([p2 C1;m2 A1]);
C2=det([m2 A1;n2 B1]);

delta1=det([A1 B1 C1;A2 B2 C2;n1 -m1 ]);
D1=A2*(x2-x1)+B2*(y2-y1)+C2*(z2-z1);

A3=n1*det([m1 n1;m2 n2])+p1*det([m1 p1;m2 p2]);
B3=m1*det([n1 m1;n2 m2])+p1*det([n1 p1;n2 p2]);
C3=m1*det([p1 m1;p2 m2])+n1*det([p1 n1;p2 n2]);
delta2=n2*det([B1 C1;B3 C3])+m2*det([A1 C1;A3 C3]);
D2=A3*(x1-x2)+B3*(y1-y2)+C3*(z1-z2);
%% 两直线垂足G和H点坐标
Xg=x1-(D1*m1*C1)/delta1;
Yg=y1-(D1*n1*C1)/delta1;
Zg=z1+D1*(A1*m1+B1*n1)/delta1;

Xh=x2-(D2*m2*C1)/delta2;
Yh=y2-(D2*n2*C1)/delta2;
Zh=z2+D2*(A1*m2+B1*n2)/delta2;

Xtarget=(Xg+Xh)/;
Ytarget=(Yg+Yh)/;
Ztarget=(Zg+Zh)/;