TOA相关定位算法

【实例截图】

【核心代码】

clc;clear;close all

%移动台坐标
T  =[5 5];
%3个基站坐标
AP1=[4 5];
AP2=[5 3];
AP3=[8 5];
%将基站画在图上，并保持
plot(T(1),T(2),'*',AP1(1),AP1(2),'*',AP2(1),AP2(2),'*',AP3(1),AP3(2),'*')
hold on

%TOA定位示意图
%为了方便起见，电磁波速度设为1m/s,即距离差等于时间差
%三个圆交与一点，即可获得移动台坐标
figure(1)
centers = AP1;%圆心
radii =1;%半径为1，相当于TOA等于1
viscircles(centers,radii,'Color','k');%有了圆心和半径就可以画圆了，k代表黑色

centers = AP2;
radii =2;%半径为2，相当于TOA等于2
viscircles(centers,radii,'Color','g');

centers = AP3;
radii =3;%半径为3，相当于TOA等于3
viscircles(centers,radii,'Color','b');
axis equal;
grid on
title('TOA定位示意图（理想）')

%TDOA定位示意图
%为了方便起见，电磁波速度设为1m/s,即距离差等于时间差
%三条双曲线交与一点，即可获得移动台坐标
figure(2)
hold on
axis equal;
plot(T(1),T(2),'*',AP1(1),AP1(2),'*',AP2(1),AP2(2),'*',AP3(1),AP3(2),'*')%基站位置

%以AP1和AP3为双曲线焦点，移动台到达AP1和AP3的时间差为实轴画双曲线
%2a为实轴
%2c为焦距
a=1; 
b=sqrt(3); 
c=sqrt(a^2 b^2);
syms x y
A=ezplot((x-6)^2/a^2-(y-5)^2/b^2==1,[-10,10])%该双曲线的中心为(6,5),需要平移
set(A,'color','g','LineWidth',2)

%以AP3和AP2为双曲线焦点，移动台到达AP3和AP2的时间差为实轴画双曲线
%2a为实轴
%2c为焦距
a=1/2; 
c=sqrt(13)/2;
b=sqrt(c^2-a^2);
%该双曲线的中心为(6.5,4),需要平移，还需要逆时针旋转33.6901度(通过基站坐标反正切可以求出)
B=ezplot(((x-6.5)*cosd(33.6901) (y-4)*sind(33.6901))^2/a^2-((6.5-x)*sind(33.6901) (y-4)*cosd(33.6901))^2/b^2==1,[-10,10])
set(B,'color','b','LineWidth',2)

%以AP1和AP2为双曲线焦点，移动台到达AP1和AP2的时间差为实轴画双曲线
%2a为实轴
%2c为焦距
a=1/2; 
c=sqrt(5)/2;
b=sqrt(c^2-a^2);
%该双曲线的中心为(4.5,4),需要平移，还需要逆时针旋转180-63.4349度(通过基站坐标反正切可以求出)
C=ezplot(((x-4.5)*cosd(180-63.4349) (y-4)*sind(180-63.4349))^2/a^2-((4.5-x)*sind(180-63.4349) (y-4)*cosd(180-63.4349))^2/b^2==1,[-10,10])
set(C,'color','k','LineWidth',2)

grid on
title('TDOA定位示意图（理想）')