two_sensor_vertical.m

此算法为多传感器目标定位法

function [outx,outy,outz,fangchax,fangchay,fangchaz]=two_sensor_vertical(X,Y,Z,B,C)

%two_sensor_vertical采用两传感器垂线定位法
%（X1,Y1,Z1）（X2,Y2,Z2）为两个传感器的坐标
%(B1,C1) (B2,C2)分别为两个传感器对空中同一目标观测得到的方位角和俯仰角
%(out1x,out1y,out1z),(out2x,out2y,out2z)为公垂线与两视线交点的坐标
syms p1 p2 q1 q2 r1 r2 x1 y1 z1 b1 c1 x2 y2 z2 b2 c2 ;

p1=cos(c1)*cos(b1);
p2=cos(c2)*cos(b2);
q1=cos(c1)*sin(b1);
q2=cos(c2)*sin(b2);
r1=sin(c1);
r2=sin(c2);

D1=det([y2-y1,z2-z1;q2,r2]);
E1=det([z2-z1,x2-x1;r2,p2]);
F1=det([x2-x1,y2-y1;p2,q2]);

t_num_1=det([q1,r1;q2,r2]);
t_num_2=det([r1,p1;r2,p2]);
t_num_3=det([p1,q1;p2,q2]);
t_den_1=det([p1,q1;p2,q2]);
t_den_2=det([q1,r1;q2,r2]);
t_den_3=det([r1,p1;r2,p2]);

t1=(D1*t_num_1+E1*t_num_2+F1*t_num_3)/((t_den_1)^2+(t_den_1)^2+(t_den_1)^2);

D2=det([y2-y1,z2-z1;q1,r1]);
E2=det([z2-z1,x2-x1;r1,p1]);
F2=det([x2-x1,y2-y1;p1,q1]);

t2=(D2*t_num_1+E2*t_num_2+F2*t_num_3)/((t_den_1)^2+(t_den_1)^2+(t_den_1)^2);

out1x=x1+p1*t1;
out1y=y1+q1*t1;
out1z=z1+r1*t1;
out2x=x2+p2*t2;
out2y=y2+q2*t2;
out2z=z2+r2*t2;

%两视线交点位置采用加权最小二乘准则融合

ax1=[diff(out1x,'b1',1),diff(out1x,'c1',1),diff(out1x,'b2',1),diff(out1x,'c2',1)];
ay1=[diff(out1y,'b1',1),diff(out1y,'c1',1),diff(out1y,'b2',1),diff(out1y,'c2',1)];
az1=[diff(out1z,'b1',1),diff(out1z,'c1',1),diff(out1z,'b2',1),diff(out1z,'c2',1)];
ax2=[diff(out2x,'b1',1),diff(out2x,'c1',1),diff(out2x,'b2',1),diff(out2x,'c2',1)];
ay2=[diff(out2y,'b1',1),diff(out2y,'c1',1),diff(out2y,'b2',1),diff(out2y,'c2',1)];
az2=[diff(out2z,'b1',1),diff(out2z,'c1',1),diff(out2z,'b2',1),diff(out2z,'c2',1)];

yx=[out1x;out2x];
yy=[out1y;out2y];
yz=[out1z;out2z];

ax=[ax1;ax2];
ay=[ay1;ay2];
az=[az1;az2];

Yx=subs(yx,{x1,y1,z1,b1,c1,x2,y2,z2,b2,c2},{X(1),Y(1),Z(1),B(1),C(1),X(2),Y(2),Z(2),B(2),C(2)});
Yy=subs(yy,{x1,y1,z1,b1,c1,x2,y2,z2,b2,c2},{X(1),Y(1),Z(1),B(1),C(1),X(2),Y(2),Z(2),B(2),C(2)});
Yz=subs(yz,{x1,y1,z1,b1,c1,x2,y2,z2,b2,c2},{X(1),Y(1),Z(1),B(1),C(1),X(2),Y(2),Z(2),B(2),C(2)});

Ax=subs(ax,{x1,y1,z1,b1,c1,x2,y2,z2,b2,c2},{X(1),Y(1),Z(1),B(1),C(1),X(2),Y(2),Z(2),B(2),C(2)});
Ay=subs(ay,{x1,y1,z1,b1,c1,x2,y2,z2,b2,c2},{X(1),Y(1),Z(1),B(1),C(1),X(2),Y(2),Z(2),B(2),C(2)});
Az=subs(az,{x1,y1,z1,b1,c1,x2,y2,z2,b2,c2},{X(1),Y(1),Z(1),B(1),C(1),X(2),Y(2),Z(2),B(2),C(2)});
C=[1;1];
R=pi/180;

outx_1=inv(Ax*R*Ax');
outy_1=inv(Ay*R*Ay');
outz_1=inv(Az*R*Az');

%目标位置估计
outx=inv(C'*outx_1*C)*C'*outx_1*Yx;
outy=inv(C'*outy_1*C)*C'*outy_1*Yy;
outz=inv(C'*outz_1*C)*C'*outz_1*Yz;

%估计误差的方差
fangchax=inv(C'*outx_1*C);
fangchay=inv(C'*outy_1*C);
fangchaz=inv(C'*outz_1*C);