zhongfuyangjiao.m

五元十字阵中方向的距离定位精度的仿真计算

clear all
close all
%二:轨迹1误差计算公式
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;
R=H/cos(a1);
R1=sqrt(R^2+L^2);
a2=(a1/abs(a1))*pi/2;     %a2是fai
a3=a1;                    %a3是seta
R2=sqrt(50^2+(R*sin(a1)-1000)^2);
R3=R1;
R4=sqrt(50^2+(1000+R1*sin(a1))^2);
tao1=(R-R1)/c;
tao2=(R-R2)/c;
tao3=(R-R3)/c;
tao4=(R-R4)/c;
A1=cos(a2)*(2*c^2*tao1-2*R*c-L^2)/(2*R*L*cos(a3));
A2=sin(a2)*(2*c^2*tao2-2*R*c-L^2)/(2*R*L*cos(a3));
A3=-cos(a2)*(2*c^2*tao3-2*R*c-L^2)/(2*R*L*cos(a3));
A4=-sin(a2)*(2*c^2*tao3-2*R*c-L^2)/(2*R*L*cos(a3));
E=[tao1,tao2,tao3,tao4];
D=zeros(1,4);
F=zeros(1,4);
G=zeros(1,4);
H=zeros(1,4);
J=zeros(1,4);
K=zeros(1,4);
M=zeros(1,4);
for i=1:1:4;
D(i)=((c*E(i))^2-2*R*c*E(i)-L^2)*cos(a2-(i-1)*pi/2);
F(i)=c*E(i)*cos(a2-(i-1)*pi/2);            %shiyan
G(i)=(2*c*(E(i))^2-2*R*E(i))*cos(a2-(i-1)*pi/2);%shengshu
H(i)=((c*E(i))^2-2*R*c*E(i)-L^2)*cos(a2-(i-1)*pi/2);%juli1
J(i)=(c*E(i))*cos(a2-(i-1)*pi/2);%juli2    
K(i)=((c*E(i))^2-2*R*c*E(i)-L^2)*cos(a2-(i-1)*pi/2);%zhenxing
M(i)=cos(a2-(i-1)*pi/2);%zhenxing    
end
shiyan=S1*sqrt(A1^2+A2^2+A3^2+A4^2);
shensu=Sc/(cos(a3)*(2*R*L))*sum(G);
zhenxing=-1/(2*R*L^2*cos(a3))*sum(K)-1/(R*cos(a3))*sum(M);
juli=-1/(2*L*R^2*cos(a3))*sum(H)-1/(R*L*cos(a3))*sum(J);
b(1,k)=zhenxing;
end
n=[1:20];
stem(n,b)