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

calc.m

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

%% 开始计算各参数
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 0]);
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)/2;
Ytarget=(Yg+Yh)/2;
Ztarget=(Zg+Zh)/2;