1. 项目概述：基于卡尔曼滤波的IMU姿态解算
在惯性导航和运动追踪领域，如何从IMU（惯性测量单元）的原始数据中准确估计载体姿态一直是个经典问题。这个项目实现了基于卡尔曼滤波的融合算法，通过整合加速度计、陀螺仪和磁力计数据，解算出载体的横滚（Roll）、俯仰（Pitch）、偏航（Yaw）三轴姿态角，同时还能估计陀螺仪的零偏（Bias），显著提升了姿态解算的精度和稳定性。
实际工程中，纯陀螺积分会因零偏导致角度漂移，而加速度计和磁力计虽然能提供绝对参考但易受瞬时干扰。卡尔曼滤波的优势在于能动态权衡不同传感器的特性，实现最优估计。
2. 核心原理与技术路线
2.1 传感器特性与误差分析
IMU通常包含三轴陀螺仪、三轴加速度计和三轴磁力计：
- 陀螺仪：测量角速度，积分得姿态角，但存在零偏（Bias）导致积分误差累积
- 加速度计：测量比力（重力+运动加速度），静止时可提供重力方向参考
- 磁力计：测量地磁场方向，提供绝对航向参考但易受铁磁物质干扰
典型误差来源：
matlab复制% 陀螺仪零偏模型示例 (单位: rad/s)
gyro_bias = [0.001; -0.0005; 0.002]; % X/Y/Z轴零偏
2.2 卡尔曼滤波框架设计
采用误差状态卡尔曼滤波（Error-State Kalman Filter）方案：
-
状态量选择：
- 姿态误差（3维）
- 陀螺仪零偏误差（3维）
-
观测模型：
- 加速度计观测：重力方向与当前姿态的偏差
- 磁力计观测：地磁方向与当前姿态的偏差
-
预测-更新流程：
mermaid复制graph TD
A[初始化] --> B[陀螺仪积分预测]
B --> C{有新观测数据?}
C -->|是| D[卡尔曼增益计算]
D --> E[状态更新]
E --> F[误差注入]
C -->|否| B
3. 关键实现步骤详解
3.1 传感器数据预处理
原始数据需经过以下处理：
-
单位统一：将各传感器输出统一到国际单位制
- 陀螺仪：rad/s
- 加速度计：m/s²
- 磁力计：μT
-
坐标系对齐：确保各传感器轴向一致
matlab复制% 坐标系对齐示例
accel_body = R_align * accel_raw; % R_align为安装矩阵
- 零偏去除：预先标定的静态零偏
matlab复制gyro_corrected = gyro_raw - gyro_bias_calib;
3.2 卡尔曼滤波实现
3.2.1 状态方程建立
采用四元数表示姿态，状态向量为：
code复制x = [δθ; δb] % 姿态误差(3x1) + 零偏误差(3x1)
状态转移矩阵：
matlab复制F = [ -skew(w_meas - b_hat) -eye(3);
zeros(3) zeros(3) ];
其中skew()为角速度的斜对称矩阵。
3.2.2 观测更新设计
加速度计观测模型：
matlab复制z_acc = g_normalized - R_est' * [0; 0; 1]; % g_normalized为归一化重力向量
H_acc = [ skew(R_est' * [0;0;1]) zeros(3) ];
磁力计观测模型（需考虑磁偏角）：
matlab复制mag_ref = [cos(dip_angle); 0; sin(dip_angle)]; % dip_angle为磁倾角
z_mag = mag_ref - R_est' * mag_normalized;
H_mag = [ skew(R_est' * mag_normalized) zeros(3) ];
3.3 姿态解算流程
完整处理流程代码框架：
matlab复制function [attitude, bias] = kalman_filter_imu(gyro, accel, mag)
% 初始化
persistent x P Q R quat
% 预测步骤
F = build_state_matrix(gyro, bias_hat);
x = F * x;
P = F * P * F' + Q;
% 更新步骤（加速度计）
if accel_update
[z_acc, H_acc] = accel_obs(quat, accel);
K = P * H_acc' / (H_acc * P * H_acc' + R_acc);
x = x + K * z_acc;
P = (eye(6) - K * H_acc) * P;
end
% 更新步骤（磁力计）
if mag_update
[z_mag, H_mag] = mag_obs(quat, mag);
K = P * H_mag' / (H_mag * P * H_mag' + R_mag);
x = x + K * z_mag;
P = (eye(6) - K * H_mag) * P;
end
% 误差注入
quat = quat_mult(quat, [1; 0.5*x(1:3)]);
bias_hat = bias_hat + x(4:6);
% 输出
attitude = quat2eul(quat);
bias = bias_hat;
end
4. 参数调优与性能提升
4.1 噪声协方差矩阵设置
噪声矩阵直接影响滤波效果：
- 过程噪声Q：反映模型不确定性
matlab复制Q = diag([0.01^2, 0.01^2, 0.01^2, 0.001^2, 0.001^2, 0.001^2]); - 观测噪声R：反映传感器精度
matlab复制R_acc = diag([0.1^2, 0.1^2, 0.1^2]); % 加速度计 R_mag = diag([0.05^2, 0.05^2, 0.05^2]); % 磁力计
4.2 自适应滤波策略
动态调整参数以应对不同运动状态：
- 运动检测：
matlab复制accel_norm = norm(accel); is_moving = abs(accel_norm - 9.8) > 0.5; % 阈值0.5m/s² - 动态调整R矩阵：
matlab复制if is_moving R_acc(3,3) = 1.0; % 增大Z轴噪声 end
5. 实际应用中的问题与对策
5.1 磁力计干扰处理
常见干扰场景及解决方案：
- 硬铁干扰：固定偏移，可通过校准消除
matlab复制mag_calib = A \ (mag_raw - b); % 椭圆拟合校准 - 软铁干扰：与磁场方向相关，需动态补偿
5.2 陀螺零偏时变问题
零偏会随时间缓慢变化，解决方案：
- 零偏动态估计：增大对应状态的过程噪声
matlab复制Q(4:6,4:6) = diag([1e-6, 1e-6, 1e-6]); - 零偏约束：设置合理的变化范围
5.3 奇异姿态处理
当俯仰角接近±90°时出现万向节锁：
- 四元数表示法：避免欧拉角奇异
- 观测有效性检测：
matlab复制if abs(pitch) > 85*pi/180 disable_mag_update = true; end
6. MATLAB实现要点
6.1 核心函数清单
- 主滤波函数：
imu_kf_filter.m - 四元数运算：
quat_mult.m,quat2eul.m - 观测模型：
accel_obs_model.m,mag_obs_model.m - 校准工具：
mag_calibration.m
6.2 实时实现技巧
- 定时中断处理：
matlab复制function timerCallback(~,~) [att, bias] = imu_kf_filter(gyro_read(), accel_read(), mag_read()); send_attitude(att); end - 计算优化：
- 预先计算常数矩阵
- 使用查找表替代三角函数
7. 验证与结果分析
7.1 静态测试结果
| 指标 | 陀螺积分 | 卡尔曼滤波 |
|---|---|---|
| 横滚角RMS(°) | 12.5 | 0.8 |
| 俯仰角RMS(°) | 15.2 | 0.7 |
| 偏航角RMS(°) | 25.7 | 1.2 |
7.2 动态测试场景
-
快速旋转测试：
- 陀螺积分：能跟踪快速变化但存在漂移
- 卡尔曼滤波：短期依赖陀螺，长期收敛到绝对参考
-
振动环境测试：
matlab复制% 振动检测逻辑 accel_var = var(accel_window); if accel_var > threshold increase_R_acc(); end
8. 扩展应用方向
- 多传感器融合：加入GPS或视觉里程计
matlab复制z_gps = [v_N; v_E]; % 东北速度 H_gps = [zeros(2,3), R_gps(1:2,:)]; - 机器学习辅助：用LSTM网络预测动态噪声参数
- 嵌入式移植：生成C代码部署到STM32等MCU
实际部署时发现，在MCU上实现完整的卡尔曼滤波需要约5KB RAM和20MHz主频，对于资源受限平台可考虑简化版（如减少状态量）或使用互补滤波。