TOA与TDOA算法实现【MATLAB】

TOA与TDOA算法原理

csdn的公式编辑器用的不是很习惯,所以有些部分直接上传word截图。其中也有我的老师给的部分推导。

 

  • TOA算法实现


    首先就是根据基站位置绘圆,半径则为目标到基站的距离。也就是速度乘时间(c*t)。

然后联立等式相减,分离系数矩阵和常数矩阵,最后得到结果。

 

TOA与TDOA算法实现【MATLAB】_第1张图片

 

  

代码如下:

clear  
close all  
clc  
delta_t = 2; % 目标的本地时钟和基站系统的钟差  
  
c=340;    %信号的飞行速度,声音信号  
  
BS1 = [-7000, 2000]; % 基站1的位置  
BS2 = [6800, 5400];  % 基站2的位置  
BS3 = [3800, -5600]; % 基站3的位置  
BS4 = [-3200, -3900];% 基站4的位置  
  
targ_p =  [-2000, 1000];%目标的实际位置  
  
pack_s = 0;% 目标发出包裹时的目标本地时间  
  
t1 = sqrt((BS1-targ_p)*(BS1-targ_p)')/c; % 信号到达基站1的信号飞行时间  
t2 = sqrt((BS2-targ_p)*(BS2-targ_p)')/c; % 信号到达基站2的信号飞行时间  
t3 = sqrt((BS3-targ_p)*(BS3-targ_p)')/c; % 信号到达基站3的信号飞行时间  
t4 = sqrt((BS4-targ_p)*(BS4-targ_p)')/c; % 信号到达基站4的信号飞行时间  
  
t_flight = [t1 t2 t3 t4];  
  
pack_r_truth = pack_s + delta_t + t_flight; %包裹到达各个基站时的基站本地实际的时间  
  
pack_r_measure = pack_r_truth + randn(1,4)*0.1; %包裹到达各个基站时的基站本地记录的时间,含有偏差  
  
%% 设置算法矩阵参数 用的是老师方法2  
  
%{  
推导步骤如下  
(x-x1)^2+(y-y1)^2=(c*t1)^2   
(x-x2)^2+(y-y2)^2=(c*t2)^2   
(x-x3)^2+(y-y3)^2=(c*t3)^2   
(x-x4)^2+(y-y4)^2=(c*t4)^2   
  
展开为  
x^2+y^2-2x*x1-2y*y1+〖x1〗^2+〖y1〗^2=(c*t1)^2 ①  
x^2+y^2-2x*x2-2y*y2+〖x2〗^2+〖y2〗^2=(c*t2)^2 ②  
x^2+y^2-2x*x3-2y*y3+〖x3〗^2+〖y3〗^2=(c*t3)^2 ③  
  
②减①,③减①  
2(x1-x2)x+2(y1-y2)y=(c*t2)^2-(c*t1)^2-(〖x2〗^2-〖x1〗^2+〖y2〗^2-〖y1〗^2)   
2(x1-x3)x+2(y1-y3)y=(c*t3)^2-(c*t1)^2-(〖x3〗^2-〖x1〗^2+〖y3〗^2-〖y1〗^2)  
  
  
X=(A^T A)^(-1) A^T B  
  
%}  
  
A(1,:)=2*[BS1(1)-BS2(1) BS1(2)-BS2(2) pack_r_measure(2)-pack_r_measure(1)] ;    %方程相减之后,包裹位置系数A设置  
A(2,:)=2*[BS1(1)-BS3(1) BS1(2)-BS3(2) pack_r_measure(3)-pack_r_measure(1)] ;  
A(3,:)=2*[BS1(1)-BS4(1) BS1(2)-BS4(2) pack_r_measure(4)-pack_r_measure(1)] ;  
  
B(1)=(pack_r_measure(2)^2-pack_r_measure(1)^2)*340^2-BS2(1)^2-BS2(2)^2+BS1(1)^2+BS1(2)^2;   %方程相减之后,常数矩阵B  
B(2)=(pack_r_measure(3)^2-pack_r_measure(1)^2)*340^2-BS3(1)^2-BS3(2)^2+BS1(1)^2+BS1(2)^2;  
B(3)=(pack_r_measure(4)^2-pack_r_measure(1)^2)*340^2-BS4(1)^2-BS4(2)^2+BS1(1)^2+BS1(2)^2;  
  
Res=(A'*A)^(-1)*A'*B';      %利用矩阵乘法把系数矩阵转到方程右边求解包裹位置  
%也可以简便写为 Res=A^(-1)*B  
  
x=Res(1);  
y=Res(2);  
del=Res(3)/340/340;  
  
targ_p_estimated_x = x  
targ_p_estimated_y = y  
delta_t_estiamted = del  

 

  • TDOA算法实现

    tdoa写的代码最后出了点问题,但是我感觉思路是对的。因为有点问题所以就贴在这里供大家参考,以后有反馈我再来修改。
    clear     %清楚之前代码程序所产生的变量 workspace  
    close all %关掉之前代码程序的绘图   
    clc       %清除之前代码的输出的信息 command window  
    %% environment setting  
    syms x y z p real;  
    BS0 = [0;0;0];  
    BS1 = [20;20;0.1]*1e3;  
    BS2 = [-20;20;0.2]*1e3;  
    BS3 = [-20;-20;0.3]*1e3;  
      
    p = [x;y;z];  
      
    rho = [normest(BS1),normest(BS2),normest(BS3)];  
      
    td =1.0e-03 *[0.0210, 0.0738, 0.1085];      %到达时差  
    c = 3*10^8;  
      
    K1 = BS1(1)^2+BS1(2)^2+BS1(3)^2;  
    K2 = BS2(1)^2+BS2(2)^2+BS2(3)^2;  
    K3 = BS3(1)^2+BS3(2)^2+BS3(3)^2;  
      
    tc = td*c;                                  %距离差  
    r = tc+rho-sqrt(x^2+y^2+z^2);  
      
    r21 = r(2)-r(1); BS21 = BS2'-BS1';  
    r31 = r(3)-r(1); BS31 = BS3'-BS1';  
    r32 = r(3)-r(2); BS32 = BS3'-BS1';  
    r1 = r(1); r2= r(2);  
      
    td =1.0e-03 *[0.0210, 0.0738, 0.1085];  
      
    %% target state  
    target_position = [10;10;10]*1e3;  
    target_speed = [200;200;200];  
      
    %% Groud Truth of Signal measurement.  
      
    %%FDOA  
      
    %%TDOA  
    solve([BS21;BS31]*[x;y;z] == [0.5*(K2-K1-r21^2)-r21*r1; 0.5*(K3-K1-r31^2)-r31*r1])  

     

 

你可能感兴趣的:(定位,matlab,toa,tdoa,算法)