pianjuli.m

五元十字阵边方向的方向角定位精度仿真计算

clear all
close all
%二:轨迹2误差计算公式
b=zeros(1,20);
for k=1:1:20;
    a1=-pi/3+pi*k/30;H=50;L=1000;c=1500;S1=10/1000000;Sc=1;SL=1;
R1=H/cos(a1);
R=sqrt(R1^2+L^2);
d=sqrt(R^2-H^2);
a2=acot(L/d);
a3=acot(H/R);
%a2=acos(L/d);
%a3=acos(H/R);
R2=sqrt(1000^2+50^2+(R1*sin(a1)-1000)^2);
R3=sqrt(2000^2+(R1*sin(a1))^2+50^2);
R4=sqrt(1000^2+50^2+(1000+R1*sin(a1))^2);
tao1=(R-R1)/c;
tao2=(R-R2)/c;
tao3=(R-R3)/c;
tao4=(R-R4)/c;
A1=(2*c*tao1*(tao1+tao2+tao3+tao4)-(c^2*(tao1^2+tao2^2+tao3^2+tao4^2)-4*L^2))/(2*c*(tao1+tao2+tao3+tao4)^2);    %shiyanpiandao
A2=(2*c*tao2*(tao1+tao2+tao3+tao4)-(c^2*(tao1^2+tao2^2+tao3^2+tao4^2)-4*L^2))/(2*c*(tao1+tao2+tao3+tao4)^2);  
A3=(2*c*tao3*(tao1+tao2+tao3+tao4)-(c^2*(tao1^2+tao2^2+tao3^2+tao4^2)-4*L^2))/(2*c*(tao1+tao2+tao3+tao4)^2);  
A4=(2*c*tao4*(tao1+tao2+tao3+tao4)-(c^2*(tao1^2+tao2^2+tao3^2+tao4^2)-4*L^2))/(2*c*(tao1+tao2+tao3+tao4)^2);  
B1=(tao1^2+tao2^2+tao3^2+tao4^2)/(2*(tao1+tao2+tao3+tao4))+2*L^2/(c^2*(tao1+tao2+tao3+tao4));            %shenshupiandao 
C1=(0-4*L)/(c*(tao1+tao2+tao3+tao4));   %zhenxingpiandao
shiyan=S1*sqrt(A1^2+A2^2+A3^2+A4^2);
shensu=Sc*B1;
chicun=SL*C1;
b(1,k)=chicun;
end
n=[1:20];
stem(n,b)