代码收藏家技术教程 2025-03-12

卡尔曼滤波算法从理论到实践：在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采样周期
    }
}