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])

TOA与TDOA算法实现【MATLAB】相关推荐

UWB定位的3种算法：TWR、TOA和TDOA算法
UWB定位的3种算法:TWR.TOA和TDOA算法文章目录 UWB定位的3种算法:TWR.TOA和TDOA算法 UWB定位技术一.TER定位算法二. TOA定位算法三. TDOA定位算法 UW ...
TDOA算法的matlab仿真
这里,主要说明一下算法流程图: 这里,仿真结果如下图所示: 三种算法在不同的簇内节点数目下的估计误差: 三种算法在不同的网络大小下的估计误差: 这里,主要涉及到的参数有: 信号频率 fc ...
（转载）基于TDOA声源定位算法仿真–MATLAB仿真
(转载)基于TDOA声源定位算法仿真–MATLAB仿真转载自:https://blog.xxcxw.cn/archives/28 声源定位算法是利用麦克风阵列进行声音定位,属于宽带信号,传统的MUS ...
matlab 声源定位csdn_[转]基于TDOA声源定位算法仿真--MATLAB仿真
声源定位算法是利用麦克风阵列进行声音定位,属于宽带信号,传统的MUSIC和DOA算法并不适用该场景,本仿真主要用TDOA算法进行定位. 常用的阵列信号定位算法主要有三大类:基于高分辨率谱估计的定位技术 ...
基于TDOA声源定位算法仿真–MATLAB仿真
评论或私信 5元获取解压码声源定位算法是利用麦克风阵列进行声音定位,属于宽带信号,传统的MUSIC和DOA算法并不适用该场景,本仿真主要用TDOA算法进行定位. 常用的阵列信号定位算法主要有三大类: ...
【老生谈算法】matlab实现Chan算法及其验证源码——Chan算法
Chan算法及其验证 matlab程序 1.文档下载: 本算法已经整理成文档如下,有需要的朋友可以点击进行下载序号文档(点击下载) 本项目文档 [老生谈算法]chan算法定位-matlab.doc ...
【UWB 定位】测距TOA 和 TDOA详解
TOF定位方法基于TOF的定位方法与基于TOA的定位方法在本质上是相同的,而TOF测距不依赖基站与标签的时间同步,故没有时钟同步偏差带来的误差,但TOF测距方法的时间取决于时钟精度,时钟偏移会带来误 ...
TDOA算法综述--(2)--Chan‘s Method
目录 1. 概要 2. 变量代换 3. 方程求解 1. 概要本文介绍TDOA求解算法之Chan's Method的求解推导过程. 关于背景介绍参见TDOA算法综述(An overview of TD ...
2G到5G蜂窝网络的定位技术（E-CID、AoA、ToA、TDOA，5G PRS：DL-TDOA、UL-TDOA、DL-AoD、UL-AOA、E-CID）
从2G到4G,蜂窝网络的定位技术主要有:E-CID.AoA.ToA.TDOA等. E-CID 传统基站分为三个扇区,一个扇区对应一个小区,每扇区通常120度,每个小区都有不同的识别码(Cell ID) ...

TOA与TDOA算法实现【MATLAB】

TOA与TDOA算法原理

TOA与TDOA算法实现【MATLAB】相关推荐

最新文章

热门文章