【TDOA与IMU融合定位，MATLAB程序】UWB+惯导的融合，使用EKF、紧耦合、三维定位滤波。附下载链接

三维空间下的 TDOA 与 IMU 紧耦合定位仿真系统，用于验证多传感器融合定位算法的性能。系统状态包括三维位置、三维速度以及航向角，通过 IMU 数据进行高频状态传播，并利用 TDOA 测量进行低频位置修正，实现稳定、连续的三维定位估计。
代码包运行成功，可联系获取一对一讲解、定制

程序介绍

本程序，用于验证多传感器融合定位算法的性能。系统状态包括三维位置、三维速度以及航向角，通过 IMU 数据进行高频状态传播，并利用 TDOA 测量进行低频位置修正，从而实现稳定、连续的三维定位估计。

程序给出了三种定位方式的对比分析：纯 IMU 惯导积分、纯 TDOA 最小二乘解算以及 TDOA+IMU 紧耦合 EKF 融合。纯 IMU 方法存在明显累积误差，尤其在 Z 轴方向漂移较为严重；纯 TDOA 方法不存在时间累积误差，但存在测量噪声引起的抖动；紧耦合 EKF 融合方法能够有效抑制 IMU 漂移，同时平滑 TDOA 测量噪声，在三维方向上均表现出更优的稳定性与精度。

解算完成后，输出三维轨迹对比图、分轴误差曲线、三维位置误差曲线以及误差统计表格（RMSE、均值、标准差、最大误差等），可用于全面评估不同算法在三维空间，尤其是 Z 轴方向上的定位性能差异。

该程序结构清晰，便于扩展至高维状态建模、偏置估计或鲁棒滤波研究，适合作为三维 UWB/IMU 融合定位算法的验证平台。

运行结果

轨迹对比：

定位误差对比：

三轴位置误差对比：

命令行输出：

MATLAB源代码

部分代码如下：

% TDOA和IMU数据进紧耦合的三维平面下的定位。N个锚点，滤波使用EKF
% 状态: [x y z vx vy vz psi]
% 作者：matlabfilter，可联系获取一对一讲解与代码定制
% 2026-02-24/Ver1

clear; close all; clc;
rng(0);

%% 参数
anchor_positions = [
    0  0  20;
    12 0  10;
    10 10 10;
    0  12 0;
    6  6  0];
num_anchors = size(anchor_positions,1);
dt = 0.01;
total_time = 200;
time_steps = total_time/dt;
t = 0:dt:total_time-dt;

imu_acc_noise  = 0.0001;
imu_gyro_noise = 0.00005;
uwb_range_noise = 0.001;

%% 构建三维运动轨迹
R = 4;
omega = 0.02;
center = [6 6];
true_traj.x = center(1) + R*cos(omega*t);
true_traj.y = center(2) + R*sin(omega*t);
true_traj.z = 0.1*t + 2*sin(0.1*t);
true_traj.vx = -R*omega*sin(omega*t);
true_traj.vy =  R*omega*cos(omega*t);
true_traj.vz = 0.1 + 2*0.1*cos(0.1*t);
true_traj.ax = -R*omega^2*cos(omega*t);
true_traj.ay = -R*omega^2*sin(omega*t);
true_traj.az = -2*(0.1)^2*sin(0.1*t);
true_traj.psi = omega*t + pi/2;
true_traj.omega = omega*ones(size(t));

完整代码下载链接：https://download.youkuaiyun.com/download/callmeup/92680048

或：
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者