✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
本算法基于TDOA原理，通过获得标签与基站的到达时间差，求得其与两基站的距离差，通过设置多个基站得到双曲面方程组，运用SI算法快速求解方程组，解得标签位置。由于设备的时延误差等问题会造成定位不准，于是通过Taylor级数展开算法减小一次误差，再通过Kalman滤波算法获得更为准确的轨迹。
理论依据
SI算法的基本原理为由多个传感器测得电磁信号，然后对这些信号进行相关估计得出TDOA值，将这些TDOA值代入双曲面方程组，并在满足最小均方差准则下解方程组。该算法比普通解双曲面方程组算法快。
Taylor 级数展开算法是在得到待定位目标的初始估计位置之后提高定位精度的一种算法。该算法需进行多次递归运算，在每一次递归运算中求解到达时间差测量值误差的局部最小二乘解，并将其用于修正待定位目标的位置坐标，直到测量误差小于设定的门限。
Kalman滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。其能够对现场采集的数据进行实时的更新和处理，利用目标的动态信息,设法去掉噪声的影响，得到一个关于目标位置的好的估计。
⛄ 部分代码
function main
clc;
clear;
T=1;%扫描周期
N=120/T;%采样次数
CR=15;%基站位置
BS=zeros(2,7);
BS(:,1)=[0,0];
BS(:,2)=[0,3*CR];
BS(:,3)=[3*CR,3*CR];
BS(:,4)=[3*CR,0];
X=zeros(4,N);%目标真实位置、速度
X(:,1)=[30,2,40,2];%目标初始位置(30,40)速度(2,2)
Z=zeros(2,N);%基站对位置的观测
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 画图
figure
hold on;box on;
plot(BS(1,:),BS(2,:),'ko');%基站
plot(X(1,:),X(3,:),'-k');%真实轨迹
plot(Z(1,:),Z(2,:),'-mx');%观测轨迹
plot(TL(1,:),TL(2,:),'-b');%Taylor修正轨迹
plot(Xkf(1,:),Xkf(3,:),'-r');%Kalman滤波轨迹
legend('基站位置','真实轨迹','观测轨迹','Taylor修正轨迹','卡尔曼滤波后轨迹') ;
xlabel('横坐标 X/m'); ylabel('纵坐标 Y/m');
title('轨迹图');
figure
hold on;box on;
plot(Observation,'-ko','MarkerFace','m');
plot(TaylorFilter,'-kv','MarkerFace','b');
plot(KalmanFilter,'-ks','MarkerFace','r');
legend('滤波前误差','Taylor修正后误差','卡尔曼滤波后误差');
xlabel('观测时间/s'); ylabel('误差值');
title("均方误差对比");
⛄ 运行结果
⛄ 参考文献
[1] 黄鹤,张会生,许家栋,等.基于卡尔曼滤波和模糊数据融合的跟踪算法[J].火力与指挥控制, 2008, 33(1):94-96.DOI:10.3969/j.issn.1002-0640.2008.01.026.
[2] 周恭谦,杨露菁,刘忠.基于TDOA的自适应扩展卡尔曼滤波跟踪算法研究[J].舰船电子工程, 2018, 38(7):4.DOI:CNKI:SUN:JCGC.0.2018-07-008.
[3] 蔡博 高宏力 宋兴国 邹祖伟.基于改进增量卡尔曼滤波算法的UWB室内定位研究[J].机械设计与制造, 2020, 000(002):22-25.
[4] 李贵玉.基于UWB的室内服务机器人组合定位跟踪技术的研究[D].南昌航空大学[2023-06-08].
