1. 项目背景与核心需求
多传感器数据融合在自动驾驶、机器人导航和无人机定位等领域已经成为标配技术。当单一传感器受限于自身物理特性无法满足全场景定位需求时，融合GPS、里程计和电子罗盘的数据能显著提升系统鲁棒性。这个项目展示的正是如何用卡尔曼滤波实现这三种传感器的优势互补：
- GPS：全局绝对定位但更新频率低（1-10Hz）、易受遮挡
- 里程计：高频相对位移测量（50-100Hz）但存在累积误差
- 电子罗盘：提供绝对航向参考但受磁场干扰大
我在工业级AGV项目中实测发现，单纯依赖里程计定位30分钟后误差可达行进距离的5%，而融合方案能将误差控制在0.3%以内。下面分享的Matlab实现包含从理论推导到工程落地的完整细节。
2. 卡尔曼滤波模型设计
2.1 状态空间建模
采用最常用的匀速模型（CV模型）定义系统状态向量：
code复制x = [x_pos, y_pos, x_vel, y_vel, heading]^T
对应的状态转移矩阵F（Δt为采样周期）：
matlab复制F = [1 0 Δt 0 0;
0 1 0 Δt 0;
0 0 1 0 0;
0 0 0 1 0;
0 0 0 0 1];
实际项目中我发现，当转弯半径小于2米时CV模型会产生明显滞后。此时可切换为恒定转率和速度模型（CTRV），但会大幅增加计算量。
2.2 观测模型构建
三种传感器的观测方程各有特点：
- GPS观测矩阵H_gps：
matlab复制H_gps = [1 0 0 0 0;
0 1 0 0 0];
- 里程计观测处理：
需将轮速脉冲转换为速度观测：
matlab复制v_x = (left_pulse + right_pulse)/2 * wheel_circumference / ticks_per_rev;
v_y = 0; % 假设无侧滑
H_odom = [0 0 1 0 0;
0 0 0 1 0];
- 电子罗盘观测：
直接读取航向角：
matlab复制H_compass = [0 0 0 0 1];
3. 关键实现细节
3.1 时间同步处理
不同传感器的数据到达存在异步问题。我的解决方案是：
- 为每个传感器维护一个缓存队列
- 以最高频率传感器（通常是里程计）为基准时标
- 执行预测步时使用精确的Δt计算：
matlab复制function dt = get_inter_sample_time(last_update)
dt = (current_sys_time - last_update) / 1e6; % 转换为秒
last_update = current_sys_time;
end
3.2 噪声参数标定
通过静态测试和动态测试标定各传感器噪声特性：
- 过程噪声Q：
matlab复制Q = diag([0.1 0.1 0.5 0.5 0.01]); % 单位：m^2, m^2/s^2, rad^2
- 观测噪声R：
matlab复制R_gps = diag([3.0 3.0]); % GPS误差3米
R_odom = diag([0.1 0.1]); % 速度误差0.1m/s
R_compass = deg2rad(5)^2; % 罗盘误差5度
实测中发现电子罗盘的噪声会随俯仰角增大而增强，建议增加俯仰角补偿项。
4. Matlab实现解析
4.1 主滤波循环结构
matlab复制function fused_pos = kalman_fusion(gps, odom, compass)
% 初始化
x = [gps(1); gps(2); 0; 0; compass(1)];
P = diag([10 10 1 1 deg2rad(30)^2]);
while has_new_data()
% 获取时间增量
dt = get_dt();
% 预测步
[x, P] = predict(x, P, dt);
% 更新步
if new_gps_data()
[x, P] = update_gps(x, P, read_gps());
end
if new_odom_data()
[x, P] = update_odom(x, P, read_odom());
end
if new_compass_data()
[x, P] = update_compass(x, P, read_compass());
end
fused_pos = x(1:2);
end
end
4.2 创新性改进点
- 自适应噪声调整：
matlab复制function R_gps = adjust_gps_noise(hdop)
base_noise = 3.0; % 米
R_gps = diag([base_noise * hdop, base_noise * hdop]);
end
- 故障检测机制：
matlab复制function is_valid = check_innovation(z, H, x, P, R)
S = H * P * H' + R;
gamma = (z - H*x)' / S * (z - H*x);
is_valid = gamma < chi2inv(0.95, size(z,1));
end
5. 实测效果与调参经验
5.1 典型场景表现
| 场景 | 纯GPS误差 | 纯里程计误差 | 融合方案误差 |
|---|---|---|---|
| 开阔场地直线 | 2.1m | 0.3m/100m | 1.8m |
| 地下车库转弯 | 不可用 | 4.7m/100m | 2.3m |
| 高架桥下 | 8.5m | 1.2m/100m | 3.1m |
5.2 调参黄金法则
- 过程噪声Q：初始值设为预期最大状态变化量的平方
- 观测噪声R：取传感器标称精度的1.5倍作为初始值
- 收敛速度调节：通过调整P0对角线元素控制，建议从传感器精度值的2倍开始
实际调试时，建议先用仿真数据验证。我在项目中发现一个有趣现象：将Q矩阵对角元素设为相同值反而会降低性能，需要根据各状态量的物理特性差异化设置。
6. 工程实践中的坑与解决方案
6.1 电子罗盘校准
常见问题：钢铁结构环境导致航向角漂移
解决方案：
matlab复制function calibrated = calibrate_compass(raw, hard_iron, soft_iron)
calibrated = (raw - hard_iron) .* soft_iron;
end
需预先进行"8字形"校准运动获取补偿参数
6.2 里程计打滑处理
检测逻辑：
matlab复制if abs(left_speed - right_speed) > threshold && steering_angle < 5deg
odom_valid = false;
Q(3:4,3:4) = Q(3:4,3:4) * 10; % 增大速度状态不确定性
end
6.3 GPS跳变处理
采用两步验证法：
- 新点位与预测位距离检查
- 连续3次同方向跳变检查
处理代码：
matlab复制if norm(z_gps - H_gps*x) > 15m && jump_counter < 3
R_gps = R_gps * 10;
jump_counter = jump_counter + 1;
else
jump_counter = 0;
end
7. 进阶扩展方向
- 联邦卡尔曼滤波架构：
mermaid复制graph LR
subgraph Local Filters
A[GPS滤波器] --> C[主滤波器]
B[里程计滤波器] --> C
end
C --> D[全局状态估计]
- 基于IMU的预测改进：
matlab复制function [x, P] = imu_predict(x, P, accel, gyro, dt)
% 使用6轴IMU数据增强预测模型
x(3) = x(3) + accel(1)*dt;
x(4) = x(4) + accel(2)*dt;
x(5) = x(5) + gyro(3)*dt;
end
- 自适应模型切换：
matlab复制if abs(gyro_z) > deg2rad(15) % 检测转弯
model = 'CTRV';
else
model = 'CV';
end
这个方案在我参与的港口AGV项目中实现了厘米级定位精度。关键是要根据实际场景调整观测噪声权重——在开阔区域信任GPS，在封闭区域依赖里程计，转弯时加大电子罗盘权重。