TOA与TDOA算法原理
csdn的公式编辑器用的不是很习惯,所以有些部分直接上传word截图。其中也有我的老师给的部分推导。
- TOA算法实现
首先就是根据基站位置绘圆,半径则为目标到基站的距离。也就是速度乘时间(c*t)。
然后联立等式相减,分离系数矩阵和常数矩阵,最后得到结果。
代码如下:
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])