02｜两状态卡尔曼滤波：估计角度与陀螺仪偏置

四状态融合适合多传感器系统，但初学姿态估计时，可以先从更轻量的两状态模型入手。本文以 filter.c 和 filter.h 为例，只估计角度与陀螺仪偏置，并用互补滤波作为对照。

输入和输出是什么

滤波器需要两个输入：由加速度计计算的倾角，以及陀螺仪对应轴角速度。输出是融合后的角度估计。

加速度计倾角 ─┐
              ├→ 两状态卡尔曼 → 融合角度
陀螺仪角速度 ─┘              → 陀螺仪偏置估计

代码不负责从原始寄存器计算倾角，也不负责单位换算。调用者必须保证角度和角速度的单位与公式一致。

状态只有角度 angle 和陀螺仪偏置 bias。预测时先从角速度中减去偏置，再积分：

angle += (gyro - bias) * dt;

加速度计倾角到来后，计算残差：

error = accel_angle - angle;
angle += K0 * error;
bias  += K1 * error;

K0 决定本次测量对角度的修正，K1 把长期残差的一部分解释为陀螺仪偏置。

代码维护角度方差、角度与偏置的协方差，以及偏置方差。预测阶段加入 Q_angle、Q_gyro，更新阶段加入测量噪声 R_angle。

这些参数表达的是噪声假设，不是越小越好。R_angle 过小会让滤波器过度跟随加速度计，车辆加速时容易把线加速度误认为重力方向变化。

Q_gyro 过小则会让偏置估计变化太慢。应使用静止数据、已知角度变化和残差统计共同调节。

源码把 dt 固定为 5 ms。只要真实任务周期不是稳定的 5 ms，角度积分和协方差传播都会偏离设计。

更可靠的接口应把 dt 作为参数传入，并限制异常值：

float Kalman_Update(FilterState *state,
                    float accel_angle,
                    float gyro_rate,
                    float dt);

这里是建议接口，不是原包函数。dt 应来自单调时钟，不能用可能回拨的日历时间。

示例为 X、Y 两轴分别复制函数，并使用静态或全局状态。它便于展示公式，却不能方便地创建多块传感器实例，也不利于统一复位和调参。

工程化应把角度、偏置、协方差和噪声参数封装进结构体。X、Y 轴使用同一个更新函数，仅状态实例不同。

这样还可以在传感器重启、姿态模式切换或时间戳异常时，明确调用复位函数，而不是依赖静态变量初值。

同一文件还提供：

angle = 0.02f * accel_angle
      + 0.98f * (angle + gyro * dt);

它让加速度计提供低频参考，让陀螺仪积分承担短时动态。计算量小、行为直观，适合先验证单位和方向。

系数同样依赖采样周期。把 0.98/0.02 原样搬到不同周期，不会保持相同截止特性；更好的做法是根据时间常数和实际 dt 计算权重。

滤波输出进入电机控制前，应先离线或架空验证。传感器异常值必须有范围检查和新鲜度判断，不能把陈旧角度持续送入闭环。