LBL-TDOA-time measuring error simulation.rar_Measuring_RQQ_TDOA

clc;clear all;close all; tic %%目标深度z已知，不同大小测时误差仿真 x0=0;y0=0;z0=50; %发起端坐标 x1=1000;y1=1000;z1=50; %第i个信标坐标，i=1,2,3,4 x2=1000;y2=-1000;z2=50; x3=-1000;y3=-1000;z3=50; x4=-1000;y4=1000;z4=50; z=0; %目标深度0 tn=0; %tn为目标接收发起端信号的时刻 c=1500; %c为声速 x=[0 500 1000 1000 1000 1500 1500 1500 1500]; y=[0 500 0 500 1000 0 500 1000 1500]; %观测点位置 q=numel(x); %x的维度 m=0:0.25e-3:5e-3; p=numel(m); r0=sqrt(x0^2+y0^2+z0^2); %r0为发起端到原点的距离 r1=sqrt(x1^2+y1^2+z1^2); %ri为第i个信标到原点的距离，i=1,2,3,4 r2=sqrt(x2^2+y2^2+z2^2); r3=sqrt(x3^2+y3^2+z3^2); r4=sqrt(x4^2+y4^2+z4^2); R1=sqrt((x1-x0)^2+(y1-y0)^2+(z1-z0)^2); %Ri为第i个信标到发起端的距离，i=1,2,3,4 R2=sqrt((x2-x0)^2+(y2-y0)^2+(z2-z0)^2); R3=sqrt((x3-x0)^2+(y3-y0)^2+(z3-z0)^2); R4=sqrt((x4-x0)^2+(y4-y0)^2+(z4-z0)^2); Rx=sqrt((x-x0).^2+(y-y0).^2+(z-z0)^2); %Rx为目标到发起端的距离 figure plot(x,y,'b*',x0,y0,'b^',x1,y1,'ko',x2,y2,'ko',x3,y3,'ko',x4,y4,'ko'); axis([-1500 2000 -1500 2000]);grid on; text(x0,y0+100,'Launch'); text(x1,y1+100,'Beacon1');text(x2,y2+100,'Beacon2'); text(x3,y3+100,'Beacon3');text(x4,y4+100,'Beacon4'); text(x(1)+10,y(1)-100,'pos1');text(x(2)-100,y(2)-100,'pos2');text(x(3)-100,y(3)-100,'pos3'); text(x(4)-100,y(4)-100,'pos4');text(x(5)-100,y(5)-100,'pos5');text(x(6),y(6)-100,'pos6'); text(x(7),y(7)-100,'pos7');text(x(8),y(8)-100,'pos8');text(x(9),y(9)-100,'pos9'); xlabel('East/m');ylabel('North/m'); %title('典型观测点选取图'); figure for v=1:p delta_t1=normrnd(0,m(v),1,1000); delta_t2=normrnd(0,m(v),1,1000); delta_t3=normrnd(0,m(v),1,1000); delta_t4=normrnd(0,m(v),1,1000); syms t1n t1n=vpasolve(R1+sqrt((x(1)-x1)^2+(y(1)-y1)^2+(z-z1)^2)-Rx(1)==c*(t1n-tn)); t1n=double(t1n); syms t2n t2n=vpasolve(R2+sqrt((x(1)-x2)^2+(y(1)-y2)^2+(z-z2)^2)-Rx(1)==c*(t2n-tn)); t2n=double(t2n); syms t3n t3n=vpasolve(R3+sqrt((x(1)-x3)^2+(y(1)-y3)^2+(z-z3)^2)-Rx(1)==c*(t3n-tn)); t3n=double(t3n); syms t4n t4n=vpasolve(R4+sqrt((x(1)-x4)^2+(y(1)-y4)^2+(z-z4)^2)-Rx(1)==c*(t4n-tn)); t4n=double(t4n); for s=1:1000 d10=c*(t1n-tn+delta_t1(s))-R1;d20=c*(t2n-tn+delta_t2(s))-R2; %di0为目标到第i个信标及发起端的距离差，i=1,2,3,4 d30=c*(t3n-tn+delta_t3(s))-R3;d40=c*(t4n-tn+delta_t4(s))-R4; A=[2*(x0-x1) 2*(y0-y1) -2*d10 2*(x0-x2) 2*(y0-y2) -2*d20 2*(x0-x3) 2*(y0-y3) -2*d30 2*(x0-x4) 2*(y0-y4) -2*d40]; B=[r0^2-r1^2+d10^2-2*(z0-z1)*z r0^2-r2^2+d20^2-2*(z0-z2)*z r0^2-r3^2+d30^2-2*(z0-z3)*z r0^2-r4^2+d40^2-2*(z0-z4)*z]; X=(A'*A)\(A'*B); xx(s)=X(1,1);yy(s)=X(2,1); end delta_x(v)=std(xx,1); delta_y(v)=std(yy,1); delta_r(v)=sqrt(delta_x(v)^2+delta_y(v)^2); end plot(m,delta_r,'b');hold on; for v=1:p delta_t1=normrnd(0,m(v),1,1000); delta_t2=normrnd(0,m(v),1,1000); delta_t3=normrnd(0,m(v),1,1000); delta_t4=normrnd(0,m(v),1,1000); syms t1n t1n=vpasolve(R1+sqrt((x(2)-x1)^2+(y(2)-y1)^2+(z-z1)^2)-Rx(2)==c*(t1n-tn)); t1n=double(t1n); syms t2n t2n=vpasolve(R2+sqrt((x(2)-x2)^2+(y(2)-y2)^2+(z-z2)^2)-Rx(2)==c*(t2n-tn)); t2n=double(t2n); syms t3n t3n=vpasolve(R3+sqrt((x(2)-x3)^2+(y(2)-y3)^2+(z-z3)^2)-Rx(2)==c*(t3n-tn)); t3n=double(t3n); syms t4n t4n=vpasolve(R4+sqrt((x(2)-x4)^2+(y(2)-y4)^2+(z-z4)^2)-Rx(2)==c*(t4n-tn)); t4n=double(t4n); for s=1:1000 d10=c*(t1n-tn+delta_t1(s))-R1;d20=c*(t2n-tn+delta_t2(s))-R2; %di0为目标到第i个信标及发起端的距离差，i=1,2,3,4 d30=c*(t3n-tn+delta_t3(s))-R3;d40=c*(t4n-tn+delta_t4(s))-R4; A=[2*(x0-x1) 2*(y0-y1) -2*d10 2*(x0-x2) 2*(y0-y2) -2*d20 2*(x0-x3) 2*(y0-y3) -2*d30 2*(x0-x4) 2*(y0-y4) -2*d40]; B=[r0^2-r1^2+d10^2-2*(z0-z1)*z r0^2-r2^2+d20^2-2*(z0-z2)*z r0^2-r3^2+d30^2-2*(z0-z3)*z r0^2-r4^2+d40^2-2*(z0-z4)*z]; X=(A'*A)\(A'*B); xx(s)=X(1,1);yy(s)=X(2,1); end delta_x(v)=std(xx,1); delta_y(v)=std(yy,1); delta_r(v)=sqrt(delta_x(v)^2+delta_y(v)^2); end plot(m,delta_r,'g');hold on; for v=1:p delta_t1=normrnd(0,m(v),1,1000); delta_t2=normrnd(0,m(v),1,1000); delta_t3=normrnd(0,m(v),1,1000); delta_t4=normrnd(0,m(v),1,1000); syms t1n t1n=vpasolve(R1+sqrt((x(3)-x1)^2+(y(3)-y1)^2+(z-z1)^2)-Rx(3)==c*(t1n-tn)); t1n=double(t1n); syms t2n t2n=vpasolve(R2+sqrt((x(3)-x2)^2+(y(3)-y2)^2+(z-z2)^2)-Rx(3)==c*(t2n-tn)); t2n=double(t2n); syms t3n t3n=vpasolve(R3+sqrt((x(3)-x3)^2+(y(3)-y3)^2+(z-z3)^2)-Rx(3)==c*(t3n-tn)); t3n=double(t3n); syms t4n t4n=vpasolve(R4+sqrt((x(3)-x4)^2+(y(3)-y4)^2+(z-z4)^2)-Rx(3)==c*(t4n-tn)); t4n=double(t4n); for s=1:1000 d10=c*(t1n-tn+delta_t1(s))-R1;d20=c*(t2n-tn+delta_t2(s))-R2; %di0为目标到第i个信标及发起端的距离差，i=1,2,3,4 d30=c*(t3n-tn+delta_t3(s))-R3;d40=c*(t4n-tn+delta_t4(s))-R4; A=[2*(x0-x1) 2*(y0-y1) -2*d10 2*(x0-x2) 2*(y0-y2) -2*d20 2*(x0-x3) 2*(y0-y3) -2*d30 2*(x0-x4) 2*(y0-y4) -2*d40]; B=[r0^2-r1^2+d10^2-2*(z0-z1)*z r0^2-r2^2+d20^2-2*(z0-z2)*z r0^2-r3^2+d30^2-2*(z0-z3)*z r0^2-r4^2+d40^2-2*(z0-z4)*z]; X=(A'*A)\(A'*B); xx(s)=X(1,1);yy(s)=X(2,1); end delta_x(v)=std(xx,1); delta_y(v)=std(yy,1); delta_r(v)=sqrt(delta_x(v)^2+delta_y(v)^2); end plot(m,delta_r,'k');hold on; for v=1:p delta_t1=normrnd(0,m(v),1,1000); delta_t2=normrnd(0,m(v),1,1000); delta_t3=normrnd(0,m(v),1,1000); delta_t4=normrnd(0,m(v),1,1000); syms t1n t1n=vpasolve(R1+sqrt((x(4)-x1)^2+(y(4)-y1)^2+(z-z1)^2)-Rx(4)==c*(t1n-tn)); t1n=double(t1n); syms t2n t2n=vpasolve(R2+sqrt((x(4)-x2)^2+(y(4)-y2)^2+(z-z2)^2)-Rx(4)==c*(t2n-tn)); t2n=double(t2n); syms t3n t3n=vpasolve(R3+sqrt((x(4)-x3)^2+(y(4)-y3)^2+(z-z3)^2)-Rx(4)==c*(t3n-tn)); t3n=double(t3n); syms t4n t4n=vpasolve(R4+sqrt((x(4)-x4)^2+(y(4)-y4)^2+(z-z4)^2)-Rx(4)==c*(t4n-tn)); t4n=double(t4n); for s=1:1000 d10=c*(t1n-tn+delta_t1(s))-R1;d20=c*(t2n-tn+delta_t2(s))-R2; %di0为目标到第i个信标及发起端的距离差，i=1,2,3,4 d30=c*(t3n-tn+delta_t3(s))-R3;d40=c*(t4n-tn+delta_t4(s))-R4; A=[2*(x0-x1) 2*(y0-y1) -2*d10 2*(x0-x2) 2*(y0-y2) -2*d20 2*(x0-x3) 2*(y0-y3) -2*d30 2*(x0-x4) 2*(y0-y4) -2*d40]; B=[r0^2-r1^2+d10^2-2*(z0-z1)*z r0^2-r2^2+d20^2-2*(z0-z2)*z r0^2-r3^2+d30^2-2*(z0-z3)*z r0^2-r4^2+d40^2-2*(z0-z4)*z]; X=(A'*A)\(A'*B); xx(s)=X(1,1);yy(s)=X(2,1); end delta_x(v)=std(xx,1); delta_y(v)=std(yy,1); delta_r(v)=sqrt(delta_x(v)^2+delta_y(v)^2); end plot(m,delta_r,'r');hold on; for v=1:p delta_t1=normrnd(0,m(v),1,1000); delta_t2=normrnd(0,m(v),1,1000); delta_t3=normrnd(0,m(v),1,1000); delta_t4=normrnd(0,m(v),1,1000); syms t1n t1n=vpasolve(R1+sqrt((x(5)-x1)^2+(y(5)-y1)^2+(z-z1)^2)-Rx(5)==c*(t1n-tn)); t1n=double(t1n); syms t2n t2n=vpasolve(R2+sqrt((x(5)-x2)^2+(y(5)-y2)^2+(z-z2)^2)-Rx(5)==c*(t2n-tn)); t2n=double(t2n); syms t3n t3n=vpasolve(R3+sqrt((x(5)-x3)^2+(y(5)-y3)^2+(z-z3)^2)-Rx(5)==c*(t3n-tn));