MATLAB【二维TDOA轨迹定位与EKF融合仿真系统】到达时间差+IMU，使用EKF滤波进行融合，与纯INS、纯TDOA的结果对比，适用于二维平面，TDOA锚点数量可自适应。附下载链接

matlabfilter

721人浏览 · 2026-02-24 12:36:33

matlabfilter · 2026-02-24 12:36:33 发布

在这里插入图片描述

TDOA（到达时间差）与IMU（惯性测量单元）数据融合滤波，使用EKF作为滤波，与纯INS、纯TDOA的结果对比，目标在二维平面下运动，运动模型为标准的CV模型。运动轨迹、TDOA的锚点数量和坐标、噪声大小等均能在代码中修改
原创代码，禁止翻卖，可联系我获取讲解与个性化定制

程序简介

提供完整的二维TDOA动态定位与滤波融合仿真框架，从测量建模到轨迹估计，再到误差分析，流程完整、结构清晰，适合用于教学演示、算法验证以及工程开发前期测试。

系统支持任意数量锚点布局，通过构造TDOA距离差观测，利用两步加权最小二乘方法进行高精度单历元定位。在此基础上，引入恒速运动模型，并通过扩展卡尔曼滤波对轨迹进行动态平滑处理，有效抑制观测噪声带来的抖动问题。

程序输出包括：

锚点与真实轨迹分布图
TDOA纯观测轨迹
EKF融合后轨迹
纯惯导模拟对比
分轴误差曲线
各方法最大误差与平均误差统计
精度提升百分比

通过对比可以直观展示：

纯观测定位的离散性
纯惯导误差累积特性
滤波融合后的平滑与精度提升效果

适合作为TDOA定位与滤波融合的基础模板，也可直接作为工程级算法验证原型。

运行结果

轨迹对比曲线：
请添加图片描述
误差对比曲线：

在这里插入图片描述

命令行窗口输出各方法估计的位置误差的统计特性：
在这里插入图片描述

MATLAB源代码

部分代码如下：

% TDOA对轨迹的定位，使用两步加权最小二乘方法，定位二维目标、N个锚点，EKF对轨迹进行滤波
% 作者：matlabfilter
% 2026-02-18/Ver1

%% 初始化
clc;clear;close all;
rng(0);

%% 定义参数和待测点位置
num_stations = 10;
std_var1_TDOA = 1e-9;
stations_position = 10*randn(num_stations, 2);  % 改为2列(x,y)
c = 3e8;
dt = 1;
%% 生成二维轨迹
point1 = -10*ones(1, 2);  % 设置起点
positions = point1 + ones(201,1)*point1 + [0:0.2:40; 0:0.2:40]';

%% TDOA定位程序
for i3 = 1:size(positions, 1)
    position = positions(i3, :);
    delta = ones(num_stations,1)*position - stations_position;
    r_ideal = (sum(delta.^2, 2)).^(1/2);
    delta_t = r_ideal/c + std_var1_TDOA*randn(size(r_ideal));
    r = delta_t*c;
    Ri = r(2:end, :);
    R1 = ones(num_stations-1, 1)*r(1, :);
    R = Ri - R1;

    %% 迭代初值
...


% 打印统计性能
fprintf('距离误差的统计特性:\n');
fprintf('  > 纯惯导(INS) 最大误差: %f m, \n\t\t\t\t平均误差: %f m\n', max(rmse_val_ins), mean(rmse_val_ins));
fprintf('  > 纯观测(TDOA)最大误差: %f m, \n\t\t\t\t平均误差: %f m\n', max(rmse_val_obs), mean(rmse_val_obs));
fprintf('  > EKF 滤波后  最大误差: %f m, \n\t\t\t\t平均误差: %f m\n', max(rmse_val_ekf), mean(rmse_val_ekf));

% 核心改进指标
improvement = (mean(rmse_val_obs) - mean(rmse_val_ekf)) / mean(rmse_val_obs) * 100;
fprintf('【结论】EKF 相比纯观测精度提升了: %.2f%%\n', improvement);

figure;
hold on;
plot(RMSE_ins, '-',  'DisplayName','纯惯导');
plot(RMSE_out, '.:', 'MarkerSize',10, 'DisplayName','纯观测');
plot(RMSE_EKF, '-',  'DisplayName','EKF滤波后','LineWidth',2);
xlabel('time'); ylabel('RMSE');
legend('Location','best');
title('各时刻距离误差对比');