1. 项目概述：UWB与IMU紧耦合定位系统
这个MATLAB项目实现了一个基于扩展卡尔曼滤波(EKF)的三维空间定位系统，通过融合UWB(超宽带)和IMU(惯性测量单元)的传感器数据，实现对移动目标的高精度轨迹追踪。系统采用紧耦合(Tightly Coupled)架构，相比传统的松耦合方案具有更强的抗干扰能力和定位稳定性。
紧耦合定位的核心思想是直接处理原始传感器数据（如UWB的测距值），而不是先解算中间结果（如UWB单独定位的坐标）。这种方式能充分利用所有可用信息，即使在部分传感器信号丢失时（如UWB基站可见数量不足），仍能保持一定的定位能力。
系统主要技术特点：
- 使用9维状态向量描述目标运动状态（位置、速度、姿态）
- IMU数据更新频率100Hz，UWB数据更新频率10Hz
- 支持自定义锚点布局和传感器噪声参数
- 输出三维轨迹、各轴误差曲线和RMSE统计
2. 系统原理与算法设计
2.1 传感器特性与互补性分析
IMU(惯性测量单元)
- 优势：高频输出(100-1000Hz)，短期精度高，可测量加速度和角速度
- 劣势：误差随时间累积（漂移现象），长期定位不可靠
UWB(超宽带)
- 优势：测距精度高(10-30cm)，无累积误差，抗多径干扰能力强
- 劣势：更新频率低(10-100Hz)，需要基站覆盖，可能被遮挡
通过EKF融合这两种传感器，可以实现：
- 利用IMU的高频特性提供平滑的运动追踪
- 利用UWB的绝对位置参考校正IMU的累积误差
- 在UWB信号暂时丢失时，依靠IMU维持短时定位
2.2 扩展卡尔曼滤波框架
系统采用标准的EKF流程，分为预测和更新两个阶段：
-
预测阶段（IMU驱动）
- 输入：加速度计和陀螺仪数据
- 过程：通过运动学方程推算当前位置、速度和姿态
- 输出：状态预测和协方差预测
-
更新阶段（UWB驱动）
- 输入：到各UWB基站的距离测量值
- 过程：计算预测距离与实际测量的残差，修正状态估计
- 输出：优化后的状态和协方差
2.3 状态空间建模
系统使用9维状态向量：
code复制x = [px, py, pz, vx, vy, vz, roll, pitch, yaw]^T
其中：
- p表示位置(m)，v表示速度(m/s)
- roll/pitch/yaw为欧拉角(rad)
运动模型采用牛顿力学方程：
code复制p_k = p_{k-1} + v_{k-1}*dt + 0.5*a_{k-1}*dt^2
v_k = v_{k-1} + a_{k-1}*dt
其中加速度a通过IMU测量值转换得到：
code复制a_nav = R_b^n * a_body - g
R_b^n为从机体坐标系到导航坐标系的旋转矩阵，由当前姿态角计算得到。
3. 代码实现详解
3.1 环境配置与参数设置
代码开头设置了仿真基本参数：
matlab复制dt = 0.01; % 采样时间(s)
T = 30; % 总时长(s)
N = T/dt; % 总步数
% 传感器噪声参数
imu_acc_noise = 0.05; % 加速度计噪声(m/s^2)
imu_gyro_noise = 0.01; % 陀螺仪噪声(rad/s)
uwb_noise = 0.15; % UWB测距噪声(m)
UWB基站(锚点)坐标设置：
matlab复制anchors = [
0 0 0; % 锚点1
30 0 0; % 锚点2
30 20 0; % 锚点3
0 20 0; % 锚点4
15 10 10 % 锚点5
];
实际应用中，锚点布局对定位精度影响很大。建议采用非共面布置，避免所有锚点位于同一平面，以提供更好的几何精度因子(GDOP)。
3.2 轨迹生成与初始化
系统使用generate_traj_3D函数生成测试轨迹：
matlab复制function traj = generate_traj_3D(t)
% 生成包含螺旋上升和正弦波动的3D轨迹
omega = 0.5; % 角频率(rad/s)
traj.x = 15 + 5*sin(omega*t);
traj.y = 10 + 5*cos(omega*t);
traj.z = 0.5*t;
% 计算速度和加速度...
end
状态初始化时添加了人为的初始误差，模拟实际系统：
matlab复制x_est(:,1) = x_true(:,1) + [0.5;0.5;0.5;0.1;0.1;0.1;0.02;0.02;0.02];
3.3 核心滤波算法实现
预测步骤(ekf_predict函数)：
- 从状态向量提取当前姿态角
- 计算旋转矩阵R_b^n
- 将机体加速度转换到导航系
- 考虑重力影响，计算实际加速度
- 更新位置和速度预测
- 传播协方差矩阵
matlab复制function [x_pred, P_pred] = ekf_predict(x, P, acc, gyro, dt, Q)
% 提取姿态角
roll = x(7); pitch = x(8); yaw = x(9);
% 计算旋转矩阵
R = euler_to_dcm(roll, pitch, yaw);
% 加速度转换和重力补偿
acc_nav = R * acc - [0; 0; 9.81];
% 状态预测
x_pred = x;
x_pred(1:3) = x(1:3) + x(4:6)*dt + 0.5*acc_nav*dt^2;
x_pred(4:6) = x(4:6) + acc_nav*dt;
% 协方差预测
F = calc_F(x, acc, dt); % 计算状态转移矩阵
P_pred = F*P*F' + Q;
end
更新步骤(ekf_update函数)：
- 计算当前位置到各锚点的预测距离
- 构建观测矩阵H（距离对状态的偏导）
- 计算卡尔曼增益K
- 更新状态和协方差
matlab复制function [x_upd, P_upd] = ekf_update(x_pred, P_pred, z, anchors, R)
% 计算预测距离
h = calc_range_3D(x_pred(1:3), anchors);
% 计算观测矩阵H
H = zeros(size(anchors,1), 9);
for i = 1:size(anchors,1)
dx = x_pred(1) - anchors(i,1);
dy = x_pred(2) - anchors(i,2);
dz = x_pred(3) - anchors(i,3);
dist = norm(x_pred(1:3) - anchors(i,:));
H(i,1:3) = [dx, dy, dz]/dist;
end
% 卡尔曼增益
K = P_pred * H' / (H * P_pred * H' + R);
% 状态更新
x_upd = x_pred + K * (z - h);
% 协方差更新
P_upd = (eye(9) - K*H) * P_pred;
end
4. 多速率传感器融合策略
系统采用IMU 100Hz、UWB 10Hz的异步融合方案，在主循环中通过模运算实现：
matlab复制for k = 2:N
% IMU预测(每步都执行)
[x_pred, P_pred] = ekf_predict(x_est(:,k-1), P, acc_body(:,k), gyro(:,k), dt, Q);
% UWB更新(每10步执行一次)
if mod(k,10) == 0
[x_upd, P_upd] = ekf_update(x_pred, P_pred, z(:,k), anchors, R);
x_est(:,k) = x_upd;
P = P_upd;
else
x_est(:,k) = x_pred;
P = P_pred;
end
end
这种实现方式避免了复杂的同步处理，同时保证了计算效率。在实际系统中，通常会使用时间戳对齐等更精确的方法。
5. 性能评估与结果分析
5.1 轨迹可视化
代码提供了四种可视化结果：
- 3D轨迹对比图：显示真实轨迹、估计轨迹和锚点位置
- 分轴位移曲线：X/Y/Z三个方向的位置随时间变化
- 误差曲线：各轴估计误差随时间变化
- 总误差统计：计算并显示RMSE(均方根误差)
典型结果如下图所示：
5.2 误差来源分析
主要误差来源包括：
- IMU误差：加速度计和陀螺仪的零偏、噪声
- UWB测距误差：多径效应、时钟同步误差
- 模型误差：运动模型简化、姿态表示奇异点
- 数值误差：离散化处理、矩阵运算精度
通过调整噪声参数Q和R，可以在响应速度和稳定性之间取得平衡：
- 增大Q表示更信任观测，响应快但噪声敏感
- 增大R表示更信任预测，平滑但响应滞后
6. 实际应用建议
- 初始对准：实际系统中需要良好的初始姿态估计，可通过静态初始化或磁力计辅助完成
- 传感器校准：IMU需要定期校准零偏和比例因子
- 异常处理：增加卡方检验等机制检测异常测量
- 非线性优化：对于更高精度需求，可考虑基于优化的方法(如因子图)
在UWB信号被部分遮挡时，紧耦合方案仍能工作，但定位精度会下降。建议在实际部署时保证至少4个非共面锚点同时可见。
7. 扩展与改进方向
- 增加传感器类型：融合视觉、里程计等多源数据
- 改进运动模型：考虑更复杂的动力学特性
- 自适应滤波：根据运动状态动态调整噪声参数
- 闭环检测：引入环境特征匹配消除累积误差
- 硬件加速：将算法移植到嵌入式平台实时运行
这个MATLAB实现提供了很好的算法验证平台，开发者可以基于此代码快速测试不同的传感器配置、运动轨迹和滤波参数，为实际系统开发奠定基础。