01｜四状态卡尔曼滤波：融合 IMU、编码器与视觉

单个传感器很难同时兼顾响应速度、长期稳定性和抗干扰能力。IMU 更新快但会漂移，编码器能提供直接位置参考，视觉观测更接近目标却通常延迟更大。卡尔曼滤波的任务，是让这些信息在同一个状态模型中各司其职。

为什么需要多传感器融合

三类观测各有缺陷：IMU 高频但有噪声和漂移，视觉测量较慢且有延迟，编码器精度有限但能提供直接角度参考。

卡尔曼滤波不是简单平均。它用系统模型预测状态，再根据每种测量的不确定性决定本次应修正多少。

设计稿选择四个状态：角度、角速度、角加速度和陀螺仪偏置。

x = [angle, angular_velocity, angular_acceleration, gyro_bias]ᵀ

角度描述云台当前方向；角速度和角加速度支撑短期运动预测；偏置用于估计陀螺仪在静止时仍存在的慢变化零偏。

状态维度越多并不天然更准。若角加速度模型与实际机构不符，增加状态反而会放大建模误差。

预测使用上一拍状态和时间间隔 dt 推算当前状态：

x⁻ = F·x + B·u
P⁻ = F·P·Fᵀ + Q

F 是状态转移矩阵，P 是估计协方差，Q 是过程噪声。预测时间越长或模型越不可信，P⁻ 应增长，表示当前估计的不确定性增加。

真实实现必须使用测量时间戳计算 dt。把视觉、IMU 和编码器强行按同一个固定周期处理，会掩盖异步到达和通信延迟。

新测量到达后，滤波器先计算创新，也就是“测量与预测相差多少”：

y = z - H·x⁻
S = H·P⁻·Hᵀ + R
K = P⁻·Hᵀ·S⁻¹
x = x⁻ + K·y
P = (I-K·H)·P⁻

H 说明某个传感器观测状态中的哪些分量，R 表示测量噪声。R 越小，滤波器越相信这次测量；P⁻ 越大，滤波器越愿意让测量纠正预测。

不同传感器应使用各自的更新函数。高频 IMU 到来时可以先预测，再用角速度或姿态观测更新；编码器和视觉到达时各自使用对应 H、R 更新。

IMU 到达      → predict(dt) → update_imu()
编码器到达    → predict(dt) → update_encoder()
视觉结果到达  → predict(dt) → update_vision()

同一时间段内不能重复预测。工程上应维护统一的最后预测时间，并清楚区分测量采样时刻与数据接收时刻。

视觉延迟若明显，直接用当前状态更新会把旧画面当成当前测量。当前示例只给出异步融合思路，没有实现历史状态回放或延迟补偿。

Q 不是传感器噪声，而是系统模型无法解释的运动变化；R 才是对应测量的不确定性。

调参应基于静态日志、动态残差和时间戳统计。只凭“输出抖不抖”调矩阵，可能得到看似平滑但严重滞后的估计。

协方差还应保持对称、半正定。代码实现中可定期检查对角线是否为负、是否出现 NaN，并在矩阵求逆前检查创新协方差是否接近奇异。

示例给出 KalmanFilter、4×4 协方差和预测/更新代码，但缺少完整头文件、矩阵库、初始化参数、构建配置和单元测试。

它应被视为设计参考。要迁移到 MSPM0G35XX，还需决定矩阵存储、浮点性能、异常测量剔除、时间戳来源和传感器坐标变换。

示例中出现的 100 Hz、STM32F4 和性能评价没有对应日志与测试脚本，不能作为当前小车的性能结论。