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