卡尔曼滤波算法从理论到实践：在STM32中的嵌入式实现

摘要：卡尔曼滤波（Kalman Filter）是传感器数据融合领域的经典算法，在姿态解算、导航定位等嵌入式场景中广泛应用。本文将从公式推导、代码实现、参数调试三个维度深入解析卡尔曼滤波，并给出基于STM32硬件的完整工程案例。

一、卡尔曼滤波核心思想

1.1 什么是卡尔曼滤波？

卡尔曼滤波是一种最优递归估计算法 ，通过融合预测值 （系统模型）与观测值 （传感器数据），在噪声干扰环境下实现对系统状态的动态估计。其核心优势在于实时性 和自适应性。

1.2 适用场景

存在高斯白噪声的线性系统
需要多传感器数据融合的场景
实时性要求高的嵌入式系统（如无人机、平衡车）

二、卡尔曼滤波算法推导

2.1 五大核心公式

参数说明：

QQ：过程噪声协方差（系统不确定性）
RR：观测噪声协方差（传感器精度）
PP：估计误差协方差

三、STM32硬件实现方案

3.1 开发环境配置

MCU: STM32F407ZGT6
传感器: MPU6050（加速度计+陀螺仪）
开发工具: STM32CubeIDE + HAL库

3.2 算法移植关键点

矩阵运算库选择：使用ARM CMSIS-DSP库加速矩阵运算
浮点运算优化：启用FPU硬件加速
实时性保障：算法耗时需小于采样周期

四、一维卡尔曼滤波代码实现

复制代码

// 卡尔曼结构体定义
typedef struct {
    float q;    // 过程噪声方差
    float r;    // 测量噪声方差
    float x;    // 状态估计值
    float p;    // 估计误差协方差
    float k;    // 卡尔曼增益
} KalmanFilter;

// 初始化滤波器
void Kalman_Init(KalmanFilter *kf, float q, float r) {
    kf->q = q;
    kf->r = r;
    kf->p = 1.0f;
    kf->x = 0;
}

// 卡尔曼迭代
float Kalman_Update(KalmanFilter *kf, float measurement) {
    // 预测阶段
    kf->p += kf->q;

    // 更新阶段
    kf->k = kf->p / (kf->p + kf->r);
    kf->x += kf->k * (measurement - kf->x);
    kf->p *= (1 - kf->k);
    
    return kf->x;
}

五、三维姿态解算应用实例

5.1 系统框图

复制代码

MPU6050 → I2C → STM32 → 卡尔曼滤波 → 串口输出
           ↑           ↓
        HAL库      PID控制器

5.2 关键代码片段

复制代码

// 在main.c中实现
float Gyro[3], Accel[3];
KalmanFilter kf_x, kf_y, kf_z;

int main(void) {
    // 初始化
    MPU6050_Init();
    Kalman_Init(&kf_x, 0.001, 0.5);
    // 类似初始化kf_y, kf_z

    while(1) {
        // 读取原始数据
        MPU6050_ReadData(Gyro, Accel);
        
        // 执行滤波
        float roll = Kalman_Update(&kf_x, Accel[0]);
        // 同样处理pitch/yaw
        
        // 通过串口输出
        printf("Roll:%.2f\tPitch:%.2f\r\n", roll, pitch);
        
        HAL_Delay(10); // 10ms采样周期
    }
}

六、参数调试经验

Q值调整：增大Q会使滤波器更信任新测量值，响应更快但噪声增大
R值调整：增大R会使滤波器更信任预测值，曲线平滑但滞后明显
典型参数范围：
- 加速度计：Q=0.001, R=0.5
- 陀螺仪：Q=0.003, R=0.1
调试工具：使用串口波形工具（如VOFA+）实时观察数据曲线

七、性能优化技巧

定点数优化：将float改为q15格式提升计算速度
矩阵预计算：对固定参数矩阵提前计算
DMA传输：使用DMA加速传感器数据读取
算法简化：根据应用场景降维处理（如将三维转为三个一维）

八、常见问题解答

Q1：如何处理非线性系统？

A：改用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）

Q2：滤波器发散怎么办？

A：检查系统模型是否准确，适当增大Q值

Q3：如何验证滤波效果？

A：通过静态测试（方差分析）和动态测试（阶跃响应）结合验证

结语：卡尔曼滤波的实战应用需要理论推导与工程经验的结合。希望本文能为嵌入式开发者在传感器数据处理方面提供有价值的参考。欢迎在评论区留言交流实际应用中的问题！