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

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

clear all;close all;

LS=[00;2000;100100*sqrt（3）];
sensor（1）=rand*200;
sensor（2）=rand*200;    
 
X21 = LS（21） - LS（11）;
X31 = LS（31） - LS（11）;
Y21 = LS（22） - LS（12）;
Y31 = LS（32） - LS（12）;
A = inv（[X21Y21;X31Y31]）;

R1 = sqrt（（LS（11） - sensor（1））^2 + （LS（12） - sensor（2））^2）;
R2 = sqrt（（LS（21） - sensor（1））^2 + （LS（22） - sensor（2））^2）;
R3 = sqrt（（LS（31） - sensor（1））^2 + （LS（32） - sensor（2））^2）;
R21=R2-R1+normrnd（03）;
R31=R3-R1+normrnd（03）;
B=[R21;R31];

K1 = LS（11）^2 + LS（12）^2;
K2 = LS（21）^2 + LS（22）^2;
K3 = LS（31）^2 + LS（32）^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（21） - EMS1（1））^2 + （LS（22） - EMS1（2））^2）-sqrt（（LS（11） - EMS1（1））^2 + （LS（12） - EMS1（2））^2）;
R31ems1=sqrt（（LS（31） - EMS1（1））^2 + （LS（32） - EMS1（2））^2）-sqrt（（LS（11） - EMS1（1））^2 + （LS（12） - 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;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2012-06-02 19:02  2维TDOA\
     文件        1380  2011-12-19 16:34  2维TDOA\tdoa.m
     文件        1169  2012-01-18 20:06  2维TDOA\tdoa2d3p.m
     文件         198  2012-01-18 19:57  2维TDOA\test.asv
     文件         227  2012-01-18 20:03  2维TDOA\test.m