four_sensor_vertical.m

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

function [outx,outy,outz]=four_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);
[outx4,outy4,outz4,fangchax4,fangchay4,fangchaz4]=two_sensor_vertical(X,Y,Z,B,C);
[outx5,outy5,outz5,fangchax5,fangchay5,fangchaz5]=two_sensor_vertical(X,Y,Z,B,C);
[outx6,outy6,outz6,fangchax6,fangchay6,fangchaz6]=two_sensor_vertical(X,Y,Z,B,C);

outx=(1/(1/fangchax1^2+1/fangchax2^2+1/fangchax3^2+1/fangchax4^2+1/fangchax5^2+1/fangchax6^2))*(outx1/fangchax1^2+outx2/fangchax2^2+outx3/fangchax3^2+outx4/fangchax4^2+outx5/fangchax5^2+outx6/fangchax6^2);
outy=(1/(1/fangchay1^2+1/fangchay2^2+1/fangchay3^2+1/fangchay4^2+1/fangchay5^2+1/fangchay6^2))*(outy1/fangchay1^2+outy2/fangchay2^2+outy3/fangchay3^2+outy4/fangchay4^2+outy5/fangchay5^2+outy6/fangchay6^2);
outz=(1/(1/fangchaz1^2+1/fangchaz2^2+1/fangchaz3^2+1/fangchaz4^2+1/fangchaz5^2+1/fangchaz6^2))*(outz1/fangchaz1^2+outz2/fangchaz2^2+outz3/fangchaz3^2+outz4/fangchaz4^2+outz5/fangchaz5^2+outz6/fangchaz6^2);