利用Matlab实现的二维TDOA定位算法仿真程序.rar

clear all;close all; LS=[0,0;200,0;100,100*sqrt(3)]; sensor(1)=rand*200; sensor(2)=rand*200; X21 = LS(2,1) - LS(1,1); X31 = LS(3,1) - LS(1,1); Y21 = LS(2,2) - LS(1,2); Y31 = LS(3,2) - LS(1,2); A = inv([X21,Y21;X31,Y31]); R1 = sqrt((LS(1,1) - sensor(1))^2 + (LS(1,2) - sensor(2))^2); R2 = sqrt((LS(2,1) - sensor(1))^2 + (LS(2,2) - sensor(2))^2); R3 = sqrt((LS(3,1) - sensor(1))^2 + (LS(3,2) - sensor(2))^2); R21=R2-R1+normrnd(0,3); R31=R3-R1+normrnd(0,3); B=[R21;R31]; K1 = LS(1,1)^2 + LS(1,2)^2; K2 = LS(2,1)^2 + LS(2,2)^2; K3 = LS(3,1)^2 + LS(3,2)^2; C = 0.5*[R21^2 - K2 + K1; R31^2 - K3 + K1]; a = B'*A'*A*B - 1; b = B'*A'*A*C + C'*A'*A*B; c = C'*A'*A*C; root1 = (-b + sqrt(b^2 - 4*a*c))/(2*a); root2 = (-b - sqrt(b^2 - 4*a*c))/(2*a); EMS1= -A*(B*root1 + C); EMS2= -A*(B*root2 + C); R21ems1=sqrt((LS(2,1) - EMS1(1))^2 + (LS(2,2) - EMS1(2))^2)-sqrt((LS(1,1) - EMS1(1))^2 + (LS(1,2) - EMS1(2))^2); R31ems1=sqrt((LS(3,1) - EMS1(1))^2 + (LS(3,2) - EMS1(2))^2)-sqrt((LS(1,1) - EMS1(1))^2 + (LS(1,2) - EMS1(2))^2); if sign(R21)==sign(R21ems1)&&sign(R31)==sign(R31ems1), OUT=EMS1; else OUT=EMS2; end scatter(LS(:,1),LS(:,2),'rs','filled'); xlabel('X [m]'); ylabel('Y [m]'); box on; hold all; scatter(sensor(1),sensor(2),'bs','filled'); hold all; scatter(OUT(1),OUT(2),'ys','filled'); hold all;