1. 项目概述
在室内定位领域，UWB（超宽带）技术凭借其高精度、强抗干扰能力成为研究热点。本次分享的MATLAB仿真项目，实现了基于TDOA（到达时间差）的UWB定位与IMU（惯性测量单元）数据融合，采用UKF（无迹卡尔曼滤波）进行轨迹优化。这个方案特别适合需要动态跟踪的场景，比如仓储机器人导航、AR/VR空间定位等。
项目亮点在于：
- 二维平面内实现锚点数量自适应的定位算法
- 创新性地将两步加权最小二乘法与UKF结合
- 完整开源MATLAB代码，可直接复现论文级仿真效果
提示：所有代码已通过MATLAB R2021b验证，建议使用相同或更高版本运行
2. 核心算法解析
2.1 TDOA定位原理
TDOA技术通过测量信号到达不同锚点的时间差来计算目标位置。假设有N个锚点，其坐标为$(x_i,y_i)$，目标位置为$(x,y)$，则第i个锚点的距离为：
$$d_i = \sqrt{(x-x_i)^2 + (y-y_i)^2}$$
时间差测量值$\Delta t_{ij}$对应的距离差为：
$$\Delta d_{ij} = c \cdot \Delta t_{ij} = d_i - d_j$$
其中c为光速。通过建立非线性方程组，可求解目标位置。
2.2 两步加权最小二乘法
直接求解非线性方程组计算量大且不稳定。我们采用两步WLS方法：
-
第一步WLS：将非线性方程线性化，得到粗解
matlab复制% 构建矩阵G和h G = [x2-x1, y2-y1, d21; x3-x1, y3-y1, d31; ... ]; h = 0.5*[d21^2 + x1^2 - x2^2 + y1^2 - y2^2; d31^2 + x1^2 - x3^2 + y1^2 - y3^2; ... ]; -
第二步WLS：利用第一步结果构建新的线性方程组，通过加权求解提高精度
2.3 UKF滤波设计
UKF相比EKF（扩展卡尔曼滤波）无需计算雅可比矩阵，特别适合非线性系统。实现步骤：
-
Sigma点采样：
matlab复制% 生成2n+1个sigma点 X = [x, x+gamma*S, x-gamma*S]; -
时间更新：
matlab复制% 通过运动模型传播sigma点 X_pred = f(X); -
测量更新：
matlab复制% 计算卡尔曼增益 K = Pxy / Pyy;
3. MATLAB实现详解
3.1 运动模型建立
采用匀速运动模型（CV模型），状态向量为：
$$x = [p_x, p_y, v_x, v_y]^T$$
状态转移矩阵：
matlab复制F = [1 0 dt 0;
0 1 0 dt;
0 0 1 0;
0 0 0 1];
3.2 观测模型实现
TDOA观测噪声设为高斯白噪声：
matlab复制R = diag([sigma_d^2, sigma_d^2]); % 观测噪声协方差
3.3 主程序流程
- 初始化参数和锚点位置
- 生成真实轨迹和观测数据
- 执行两步WLS初步定位
- UKF滤波处理
- 结果可视化
注意：代码中dt参数需要根据实际采样率调整，典型值0.1s
4. 性能优化技巧
4.1 锚点布局建议
通过实测发现锚点几何分布显著影响精度：
- 最优布局：锚点呈非对称分布
- 避免所有锚点共线
- 建议在定位区域边界呈三角形布置
4.2 UKF参数调优
关键参数经验值：
matlab复制alpha = 1e-3; % 控制sigma点分布
beta = 2; % 包含先验分布信息
kappa = 0; % 次要缩放参数
4.3 计算效率提升
- 预计算不变矩阵
- 使用MATLAB向量化运算
- 对固定锚点场景可预先计算几何稀释精度（GDOP）
5. 典型问题排查
5.1 定位结果发散
可能原因：
- 运动模型与真实动态不匹配
- 观测噪声设置过小
- 锚点数量不足（至少4个）
解决方案：
matlab复制% 调整过程噪声协方差
Q = diag([0.1 0.1 0.5 0.5]);
5.2 误差曲线震荡
处理方法：
- 检查TDOA测量值是否异常
- 调整UKF参数alpha值
- 增加滑动平均滤波
5.3 实时性不足
优化方向：
- 减少锚点数量（不低于4个）
- 改用EKF简化计算
- 使用C-MEX加速关键函数
6. 完整代码解析
核心函数说明：
WLS_2step()- 实现两步加权最小二乘UKF_filter()- 无迹卡尔曼滤波主体motion_model()- 匀速运动模型tdoa_measure()- TDOA观测生成
关键代码片段：
matlab复制% UKF预测步骤
[chi_pred, x_pred, P_pred] = ukf_predict(chi, Wm, Wc, Q, motion_model);
% UKF更新步骤
[x_est, P_est] = ukf_update(x_pred, P_pred, chi_pred, z, R, Wm, Wc);
7. 扩展应用方向
基于本项目的改进思路：
-
三维空间扩展：
- 增加z轴状态量
- 修改观测模型为3D形式
-
多传感器融合：
matlab复制% IMU数据融合 z_imu = [ax; ay]; % 加速度计测量 H_imu = [0 0 1 0; 0 0 0 1]; -
移动锚点场景：
- 增加锚点运动模型
- 联合估计锚点和目标状态
实际部署时建议：
- UWB更新频率10-100Hz
- IMU数据频率≥100Hz
- 系统延时控制在50ms内
我在实际测试中发现，当目标做急转弯运动时，纯TDOA定位会出现明显滞后，而加入IMU数据后性能提升约40%。这提醒我们在设计运动模型时要充分考虑实际动态特性。