1. 项目概述：UWB与IMU紧耦合定位系统
在室内定位领域，单一传感器往往难以满足高精度需求。UWB（超宽带）技术虽然测距精度可达厘米级，但在非视距环境下性能下降；IMU（惯性测量单元）短期精度高但存在累积误差。这个MATLAB仿真项目展示了一种紧耦合的解决方案——通过扩展卡尔曼滤波（EKF）融合两种传感器的优势。
系统在二维平面下运行，核心流程是：
- IMU提供载体加速度和角速度的原始数据
- UWB基站通过TOA（到达时间）算法计算距离
- EKF实时融合两类数据，输出最优位置估计
关键创新点：不同于松耦合（独立解算后融合），紧耦合直接在原始观测层进行数据融合，能更充分利用传感器间的互补特性。实测显示，这种方案在5m×5m区域内可将定位误差控制在0.3m以内。
2. 系统建模与算法设计
2.1 状态空间建模
系统状态向量定义为：
code复制X = [x, y, vx, vy, θ]'
其中(x,y)为平面坐标，(vx,vy)为速度分量，θ为航向角。状态转移模型采用经典的惯性导航方程：
code复制x_k = x_{k-1} + vx_{k-1}*Δt + 0.5*a_x*Δt²
y_k = y_{k-1} + vy_{k-1}*Δt + 0.5*a_y*Δt²
vx_k = vx_{k-1} + a_x*Δt
vy_k = vy_{k-1} + a_y*Δt
θ_k = θ_{k-1} + ω*Δt
2.2 观测模型构建
UWB观测值为到各基站的距离：
code复制z_uwb = sqrt((x-x_i)² + (y-y_i)²) + v
其中(x_i,y_i)为基站坐标，v为观测噪声。
IMU直接提供加速度和角速度：
code复制z_imu = [a_x, a_y, ω]' + w
w为IMU测量噪声。
2.3 扩展卡尔曼滤波实现
EKF的核心步骤包括：
-
状态预测：
matlab复制
X_pred = F*X_est; P_pred = F*P_est*F' + Q; -
观测更新：
matlab复制K = P_pred*H'/(H*P_pred*H' + R); X_est = X_pred + K*(z - h(X_pred)); P_est = (eye(5) - K*H)*P_pred;
实操技巧：雅可比矩阵H需在线计算，在MATLAB中可用符号工具箱自动求导：
matlab复制syms x y vx vy theta h_uwb = sqrt((x-x_i)^2 + (y-y_i)^2); H_uwb = jacobian(h_uwb, [x,y,vx,vy,theta]);
3. MATLAB实现详解
3.1 主程序架构
完整代码包含以下模块：
matlab复制%% 初始化
% 设置UWB基站坐标、IMU参数、EKF初始状态
anchors = [0 0; 5 0; 0 5; 5 5]; % 4个基站
imu_noise = 0.01; % IMU噪声方差
dt = 0.1; % 采样周期
%% 轨迹生成
% 模拟载体运动（可自定义轨迹）
traj = generate_trajectory('circle');
%% 传感器数据仿真
[acc, gyro] = imu_sim(traj, imu_noise);
ranges = uwb_sim(traj.pos, anchors);
%% EKF主循环
for k = 1:length(traj.time)
% 预测步骤
[X_pred, P_pred] = predict(X_est, P_est, acc(k,:), gyro(k), dt);
% 更新步骤
[X_est, P_est] = update(X_pred, P_pred, ranges(k,:), anchors);
% 记录结果
results.pos(k,:) = X_est(1:2);
end
3.2 关键函数实现
predict.m 状态预测函数：
matlab复制function [X_pred, P_pred] = predict(X, P, acc, gyro, dt)
% 状态转移矩阵
F = [1 0 dt 0 0;
0 1 0 dt 0;
0 0 1 0 0;
0 0 0 1 0;
0 0 0 0 1];
% 控制输入（IMU加速度转换到全局坐标系）
theta = X(5);
B = [0.5*dt^2*cos(theta) 0;
0.5*dt^2*sin(theta) 0;
dt*cos(theta) 0;
dt*sin(theta) 0;
0 dt];
u = [acc(1); gyro]; % 输入向量
X_pred = F*X + B*u;
% 过程噪声协方差
Q = diag([0.1 0.1 0.5 0.5 0.01]);
P_pred = F*P*F' + Q;
end
update.m 观测更新函数：
matlab复制function [X_est, P_est] = update(X_pred, P_pred, ranges, anchors)
% 观测模型雅可比矩阵
H = zeros(length(ranges), 5);
h = zeros(length(ranges), 1);
for i = 1:length(ranges)
dx = X_pred(1) - anchors(i,1);
dy = X_pred(2) - anchors(i,2);
dist = sqrt(dx^2 + dy^2);
H(i,:) = [dx/dist, dy/dist, 0, 0, 0];
h(i) = dist;
end
% 观测噪声
R = 0.1*eye(length(ranges));
% 卡尔曼增益
K = P_pred*H'/(H*P_pred*H' + R);
% 状态更新
X_est = X_pred + K*(ranges' - h);
P_est = (eye(5) - K*H)*P_pred;
end
4. 仿真结果分析
4.1 轨迹对比
系统输出三种轨迹可视化：
- 红色：真实轨迹（模拟生成）
- 蓝色：仅IMU解算轨迹
- 绿色：紧耦合融合轨迹
可见IMU单独解算会快速发散，而紧耦合方案能稳定跟踪真实轨迹。
4.2 误差统计
位置误差通过RMSE（均方根误差）评估：
matlab复制err_imu = sqrt(mean(sum((imu_pos - true_pos).^2, 2)));
err_fusion = sqrt(mean(sum((fusion_pos - true_pos).^2, 2)));
典型输出结果：
code复制IMU单独定位RMSE: 1.872m
紧耦合定位RMSE: 0.287m
误差曲线显示紧耦合方案将误差控制在0.5m以内：
5. 工程实践建议
5.1 参数调优经验
-
过程噪声Q：应根据IMU实际性能调整。MEMS IMU建议值：
matlab复制Q = diag([0.1 0.1 0.5 0.5 0.01]); % 位置、速度、航向噪声 -
观测噪声R：与UWB环境相关。视距环境下：
matlab复制R = 0.1*eye(N_anchors); % 10cm精度 -
初始协方差P0：反映初始状态不确定性，建议：
matlab复制P0 = diag([1 1 0.5 0.5 0.1]);
5.2 常见问题排查
问题1：滤波器发散
- 检查IMU和UWB数据的时间对齐
- 验证雅可比矩阵计算是否正确
- 降低过程噪声Q的初始值
问题2：定位结果跳变
- 检查UWB非视距误差
- 增加R矩阵中的噪声参数
- 添加新息检测（Innovation Check）
问题3：计算速度慢
- 预计算雅可比矩阵的解析形式
- 改用平方根卡尔曼滤波实现
- 降低更新频率（10-20Hz通常足够）
6. 扩展应用方向
本框架可进一步扩展：
- 三维定位：增加z轴状态，使用6轴IMU
- 多基站优化：研究基站几何分布对精度的影响
- 硬件部署：替换仿真数据为实际UWB模块（如DW1000）和IMU（如MPU6050）
- 深度学习融合：用NN学习残差误差，提升非视距场景性能
实测中发现，在走廊等狭长环境中，将基站布置在路径两侧（而非均匀分布）可提升50%以上的定位精度。这是因为改善了GDOP（几何精度因子）。