three_sensor_vertical.m

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

function [outx,outy,outz]=three_sensor_vertical(X,Y,Z,B,C)
[outx1,outy1,outz1,fangchax1,fangchay1,fangchaz1]=two_sensor_vertical(X,Y,Z,B,C);
[outx2,outy2,outz2,fangchax2,fangchay2,fangchaz2]=two_sensor_vertical(X,Y,Z,B,C);
[outx3,outy3,outz3,fangchax3,fangchay3,fangchaz3]=two_sensor_vertical(X,Y,Z,B,C);

outx=(1/(1/fangchax1^2+1/fangchax2^2+1/fangchax3^2))*(outx1/fangchax1^2+outx2/fangchax2^2+outx3/fangchax3^2);
outy=(1/(1/fangchay1^2+1/fangchay2^2+1/fangchay3^2))*(outy1/fangchay1^2+outy2/fangchay2^2+outy3/fangchay3^2);
outz=(1/(1/fangchaz1^2+1/fangchaz2^2+1/fangchaz3^2))*(outz1/fangchaz1^2+outz2/fangchaz2^2+outz3/fangchaz3^2);