STM32卡尔曼滤波算法详解与实战应用

一、引言

在嵌入式系统开发中，传感器数据的噪声处理一直是一个关键问题。无论是姿态测量、温度采集还是位置追踪，原始传感器数据往往包含各种噪声和干扰。卡尔曼滤波器（Kalman Filter）作为一种最优估计算法，能够有效地从带噪声的测量数据中提取真实信号，在STM32嵌入式系统中得到了广泛应用。

本文将深入讲解卡尔曼滤波的数学原理，并通过详细的代码实现和实际传感器应用案例，帮助你在STM32项目中快速部署这一强大的算法。

二、卡尔曼滤波基本原理

2.1 算法核心思想

卡尔曼滤波器是一种递归的状态估计算法，它通过以下两个步骤不断更新系统状态：

1. 预测步骤：基于系统模型预测当前状态
2. 更新步骤：利用新的测量值修正预测结果

卡尔曼滤波的核心优势在于：

• 能够融合多个信息源
• 考虑了过程噪声和测量噪声
• 提供最优估计（在线性高斯假设下）
• 计算量小，适合实时系统

2.2 数学模型

2.2.1 状态空间方程

状态方程（系统如何演变）：

x(k) = A * x(k-1) + B * u(k) + w(k)

观测方程（如何测量状态）：

z(k) = H * x(k) + v(k)

其中：

• x(k) - 第k时刻的系统状态向量
• A - 状态转移矩阵
• B - 控制输入矩阵
• u(k) - 控制输入
• w(k) - 过程噪声（协方差Q）
• z(k) - 测量值
• H - 观测矩阵
• v(k) - 测量噪声（协方差R）

2.2.2 五个核心方程

预测阶段：

1. 状态预测：

   x'(k) = A * x(k-1) + B * u(k)

2. 协方差预测：

   P'(k) = A * P(k-1) * A^T + Q

更新阶段：

3. 卡尔曼增益：

   K(k) = P'(k) * H^T * (H * P'(k) * H^T + R)^(-1)

4. 状态更新：

   x(k) = x'(k) + K(k) * (z(k) - H * x'(k))

5. 协方差更新：

   P(k) = (I - K(k) * H) * P'(k)

2.3 参数调节说明

• Q（过程噪声协方差）：反映系统模型的不确定性

  • Q越大，表示越不信任模型预测，更依赖测量值
  • Q越小，表示模型很准确，滤波器响应较慢

• R（测量噪声协方差）：反映传感器测量的不确定性

  • R越大，表示测量值不可靠，更依赖模型预测
  • R越小，表示测量很准确，滤波器响应较快

三、STM32实现详细步骤

3.1 一维卡尔曼滤波器实现

对于单变量系统（如温度测量），我们首先实现一个简单的一维卡尔曼滤波器。

3.1.1 数据结构定义

// kalman.h
#ifndef __KALMAN_H
#define __KALMAN_H

#include "stm32f1xx_hal.h"

// 一维卡尔曼滤波器结构体
typedef struct {
    float x;        // 状态估计值
    float P;        // 估计误差协方差
    float Q;        // 过程噪声协方差
    float R;        // 测量噪声协方差
    float K;        // 卡尔曼增益
} KalmanFilter_1D;

// 函数声明
void Kalman_Init_1D(KalmanFilter_1D *kf, float Q, float R, float initial_value);
float Kalman_Update_1D(KalmanFilter_1D *kf, float measurement);

#endif

3.1.2 核心算法实现

// kalman.c
#include "kalman.h"

/**
 * @brief  初始化一维卡尔曼滤波器
 * @param  kf: 卡尔曼滤波器指针
 * @param  Q: 过程噪声协方差
 * @param  R: 测量噪声协方差
 * @param  initial_value: 初始状态值
 */
void Kalman_Init_1D(KalmanFilter_1D *kf, float Q, float R, float initial_value)
{
    kf->x = initial_value;  // 初始状态
    kf->P = 1.0f;           // 初始估计误差协方差
    kf->Q = Q;              // 过程噪声协方差
    kf->R = R;              // 测量噪声协方差
    kf->K = 0.0f;           // 初始卡尔曼增益
}

/**
 * @brief  一维卡尔曼滤波更新
 * @param  kf: 卡尔曼滤波器指针
 * @param  measurement: 测量值
 * @return 滤波后的估计值
 */
float Kalman_Update_1D(KalmanFilter_1D *kf, float measurement)
{
    // 预测步骤
    // 状态预测（一维简单模型，状态不变）
    float x_predict = kf->x;
    
    // 协方差预测
    float P_predict = kf->P + kf->Q;
    
    // 更新步骤
    // 计算卡尔曼增益
    kf->K = P_predict / (P_predict + kf->R);
    
    // 状态更新
    kf->x = x_predict + kf->K * (measurement - x_predict);
    
    // 协方差更新
    kf->P = (1.0f - kf->K) * P_predict;
    
    return kf->x;
}

3.2 二维卡尔曼滤波器实现

对于更复杂的系统（如加速度计+陀螺仪融合），需要使用多维卡尔曼滤波。

// kalman_2d.h
#ifndef __KALMAN_2D_H
#define __KALMAN_2D_H

#include "stm32f1xx_hal.h"

// 二维卡尔曼滤波器结构体（用于角度估计）
typedef struct {
    float angle;      // 角度估计
    float bias;       // 陀螺仪零偏估计
    float P[2][2];    // 估计误差协方差矩阵
    float Q_angle;    // 角度过程噪声
    float Q_bias;     // 零偏过程噪声
    float R_measure;  // 测量噪声
    float dt;         // 采样时间
} KalmanFilter_2D;

void Kalman_Init_2D(KalmanFilter_2D *kf, float Q_angle, float Q_bias, 
                    float R_measure, float dt);
float Kalman_Update_2D(KalmanFilter_2D *kf, float new_angle, float new_rate);

#endif

// kalman_2d.c
#include "kalman_2d.h"

/**
 * @brief  初始化二维卡尔曼滤波器
 * @param  kf: 卡尔曼滤波器指针
 * @param  Q_angle: 角度过程噪声协方差
 * @param  Q_bias: 零偏过程噪声协方差
 * @param  R_measure: 测量噪声协方差
 * @param  dt: 采样周期
 */
void Kalman_Init_2D(KalmanFilter_2D *kf, float Q_angle, float Q_bias, 
                    float R_measure, float dt)
{
    kf->angle = 0.0f;
    kf->bias = 0.0f;
    kf->Q_angle = Q_angle;
    kf->Q_bias = Q_bias;
    kf->R_measure = R_measure;
    kf->dt = dt;
    
    // 初始化协方差矩阵
    kf->P[0][0] = 1.0f;
    kf->P[0][1] = 0.0f;
    kf->P[1][0] = 0.0f;
    kf->P[1][1] = 1.0f;
}

/**
 * @brief  二维卡尔曼滤波更新（角度和角速度融合）
 * @param  kf: 卡尔曼滤波器指针
 * @param  new_angle: 加速度计测得的角度
 * @param  new_rate: 陀螺仪测得的角速度
 * @return 融合后的角度估计
 */
float Kalman_Update_2D(KalmanFilter_2D *kf, float new_angle, float new_rate)
{
    // 预测步骤
    // 状态预测：角度 = 上次角度 + (角速度 - 零偏) * dt
    float rate = new_rate - kf->bias;
    kf->angle += kf->dt * rate;
    
    // 协方差预测
    kf->P[0][0] += kf->dt * (kf->dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle);
    kf->P[0][1] -= kf->dt * kf->P[1][1];
    kf->P[1][0] -= kf->dt * kf->P[1][1];
    kf->P[1][1] += kf->Q_bias * kf->dt;
    
    // 更新步骤
    // 计算创新（测量残差）
    float innovation = new_angle - kf->angle;
    
    // 计算创新协方差
    float S = kf->P[0][0] + kf->R_measure;
    
    // 计算卡尔曼增益
    float K[2];
    K[0] = kf->P[0][0] / S;
    K[1] = kf->P[1][0] / S;
    
    // 状态更新
    kf->angle += K[0] * innovation;
    kf->bias += K[1] * innovation;
    
    // 协方差更新
    float P00_temp = kf->P[0][0];
    float P01_temp = kf->P[0][1];
    
    kf->P[0][0] -= K[0] * P00_temp;
    kf->P[0][1] -= K[0] * P01_temp;
    kf->P[1][0] -= K[1] * P00_temp;
    kf->P[1][1] -= K[1] * P01_temp;
    
    return kf->angle;
}

四、传感器应用实践

4.1 案例一：温度传感器数据滤波

4.1.1 硬件连接

使用DS18B20或DHT11温度传感器连接到STM32。

4.1.2 完整示例代码

// main.c - 温度传感器滤波示例
#include "main.h"
#include "kalman.h"

KalmanFilter_1D temp_filter;

// 模拟温度读取函数（替换为实际传感器读取）
float Read_Temperature(void)
{
    // 这里应该调用实际的传感器读取函数
    // 例如：return DS18B20_Read_Temp();
    static float real_temp = 25.0f;
    // 模拟噪声
    float noise = ((float)(rand() % 100) - 50.0f) / 50.0f;
    return real_temp + noise;
}

int main(void)
{
    HAL_Init();
    SystemClock_Config();
    
    // 初始化卡尔曼滤波器
    // Q=0.01: 过程噪声较小（温度变化慢）
    // R=0.5: 测量噪声中等
    Kalman_Init_1D(&temp_filter, 0.01f, 0.5f, 25.0f);
    
    while(1)
    {
        // 读取原始温度
        float raw_temp = Read_Temperature();
        
        // 卡尔曼滤波
        float filtered_temp = Kalman_Update_1D(&temp_filter, raw_temp);
        
        // 输出结果
        printf("Raw: %.2f°C, Filtered: %.2f°C\r\n", raw_temp, filtered_temp);
        
        HAL_Delay(100);  // 100ms采样周期
    }
}

4.1.3 参数调优建议

温度传感器特点：

• 温度变化缓慢，Q值应该较小（0.001~0.01）
• 传感器精度不同，R值需要根据数据手册调整
• 建议先记录一段静态数据，计算方差作为R的初始值

4.2 案例二：MPU6050姿态解算

4.2.1 硬件连接

MPU6050通过I2C连接到STM32：

• SCL -> PB6
• SDA -> PB7
• VCC -> 3.3V
• GND -> GND

4.2.2 MPU6050驱动配置

// mpu6050.h
#ifndef __MPU6050_H
#define __MPU6050_H

#include "stm32f1xx_hal.h"

typedef struct {
    float accel_x, accel_y, accel_z;  // 加速度 (g)
    float gyro_x, gyro_y, gyro_z;     // 角速度 (°/s)
    float temp;                        // 温度 (°C)
} MPU6050_Data;

HAL_StatusTypeDef MPU6050_Init(I2C_HandleTypeDef *hi2c);
HAL_StatusTypeDef MPU6050_Read_All(I2C_HandleTypeDef *hi2c, MPU6050_Data *data);
void MPU6050_Calculate_Angle(MPU6050_Data *data, float *roll, float *pitch);

#endif

// mpu6050.c
#include "mpu6050.h"
#include <math.h>

#define MPU6050_ADDR 0xD0
#define WHO_AM_I_REG 0x75
#define PWR_MGMT_1_REG 0x6B
#define ACCEL_XOUT_H_REG 0x3B

HAL_StatusTypeDef MPU6050_Init(I2C_HandleTypeDef *hi2c)
{
    HAL_StatusTypeDef ret;
    uint8_t check;
    uint8_t data;
    
    // 检查设备ID
    ret = HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, 1000);
    if(ret != HAL_OK || check != 0x68) return HAL_ERROR;
    
    // 唤醒MPU6050
    data = 0x00;
    ret = HAL_I2C_Mem_Write(hi2c, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &data, 1, 1000);
    if(ret != HAL_OK) return HAL_ERROR;
    
    return HAL_OK;
}

HAL_StatusTypeDef MPU6050_Read_All(I2C_HandleTypeDef *hi2c, MPU6050_Data *data)
{
    uint8_t raw_data[14];
    HAL_StatusTypeDef ret;
    
    // 读取14字节原始数据
    ret = HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, raw_data, 14, 1000);
    if(ret != HAL_OK) return HAL_ERROR;
    
    // 解析加速度数据
    int16_t accel_raw_x = (int16_t)(raw_data[0] << 8 | raw_data[1]);
    int16_t accel_raw_y = (int16_t)(raw_data[2] << 8 | raw_data[3]);
    int16_t accel_raw_z = (int16_t)(raw_data[4] << 8 | raw_data[5]);
    
    // 解析陀螺仪数据
    int16_t gyro_raw_x = (int16_t)(raw_data[8] << 8 | raw_data[9]);
    int16_t gyro_raw_y = (int16_t)(raw_data[10] << 8 | raw_data[11]);
    int16_t gyro_raw_z = (int16_t)(raw_data[12] << 8 | raw_data[13]);
    
    // 转换为实际物理量
    data->accel_x = accel_raw_x / 16384.0f;  // ±2g
    data->accel_y = accel_raw_y / 16384.0f;
    data->accel_z = accel_raw_z / 16384.0f;
    
    data->gyro_x = gyro_raw_x / 131.0f;      // ±250°/s
    data->gyro_y = gyro_raw_y / 131.0f;
    data->gyro_z = gyro_raw_z / 131.0f;
    
    return HAL_OK;
}

void MPU6050_Calculate_Angle(MPU6050_Data *data, float *roll, float *pitch)
{
    // 从加速度计计算角度
    *roll = atan2f(data->accel_y, data->accel_z) * 57.3f;  // 转换为角度
    *pitch = atan2f(-data->accel_x, sqrtf(data->accel_y * data->accel_y + 
                    data->accel_z * data->accel_z)) * 57.3f;
}

4.2.3 姿态融合主程序

// main.c - MPU6050姿态解算
#include "main.h"
#include "mpu6050.h"
#include "kalman_2d.h"

I2C_HandleTypeDef hi2c1;
KalmanFilter_2D kalman_roll, kalman_pitch;
MPU6050_Data mpu_data;

void SystemClock_Config(void);
void MX_I2C1_Init(void);

int main(void)
{
    HAL_Init();
    SystemClock_Config();
    MX_I2C1_Init();
    
    // 初始化MPU6050
    if(MPU6050_Init(&hi2c1) != HAL_OK) {
        Error_Handler();
    }
    
    // 初始化卡尔曼滤波器
    // Q_angle=0.001: 角度变化过程噪声
    // Q_bias=0.003: 陀螺仪零偏过程噪声
    // R_measure=0.03: 加速度计测量噪声
    // dt=0.01: 10ms采样周期
    Kalman_Init_2D(&kalman_roll, 0.001f, 0.003f, 0.03f, 0.01f);
    Kalman_Init_2D(&kalman_pitch, 0.001f, 0.003f, 0.03f, 0.01f);
    
    uint32_t last_time = HAL_GetTick();
    
    while(1)
    {
        // 读取MPU6050数据
        if(MPU6050_Read_All(&hi2c1, &mpu_data) == HAL_OK)
        {
            // 计算当前时间间隔
            uint32_t current_time = HAL_GetTick();
            float dt = (current_time - last_time) / 1000.0f;
            last_time = current_time;
            
            // 更新采样周期
            kalman_roll.dt = dt;
            kalman_pitch.dt = dt;
            
            // 从加速度计计算角度
            float accel_roll, accel_pitch;
            MPU6050_Calculate_Angle(&mpu_data, &accel_roll, &accel_pitch);
            
            // 卡尔曼滤波融合
            float filtered_roll = Kalman_Update_2D(&kalman_roll, accel_roll, mpu_data.gyro_x);
            float filtered_pitch = Kalman_Update_2D(&kalman_pitch, accel_pitch, mpu_data.gyro_y);
            
            // 输出结果
            printf("Roll: %.2f°, Pitch: %.2f°\r\n", filtered_roll, filtered_pitch);
        }
        
        HAL_Delay(10);  // 100Hz采样率
    }
}

void MX_I2C1_Init(void)
{
    hi2c1.Instance = I2C1;
    hi2c1.Init.ClockSpeed = 400000;
    hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
    hi2c1.Init.OwnAddress1 = 0;
    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
    hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
    
    if (HAL_I2C_Init(&hi2c1) != HAL_OK) {
        Error_Handler();
    }
}

4.3 案例三：超声波测距滤波

// 超声波测距卡尔曼滤波示例
#include "main.h"
#include "kalman.h"

KalmanFilter_1D distance_filter;

// 超声波测距函数（HC-SR04）
float Read_Distance_HC_SR04(void)
{
    uint32_t time;
    float distance;
    
    // 发送10us触发信号
    HAL_GPIO_WritePin(TRIG_GPIO_Port, TRIG_Pin, GPIO_PIN_SET);
    delay_us(10);
    HAL_GPIO_WritePin(TRIG_GPIO_Port, TRIG_Pin, GPIO_PIN_RESET);
    
    // 等待回响信号
    while(HAL_GPIO_ReadPin(ECHO_GPIO_Port, ECHO_Pin) == GPIO_PIN_RESET);
    time = 0;
    
    // 测量高电平持续时间
    while(HAL_GPIO_ReadPin(ECHO_GPIO_Port, ECHO_Pin) == GPIO_PIN_SET) {
        time++;
        delay_us(1);
        if(time > 30000) break;  // 超时保护
    }
    
    // 计算距离 (cm)
    distance = (time * 0.034f) / 2.0f;
    
    return distance;
}

int main(void)
{
    HAL_Init();
    SystemClock_Config();
    
    // 初始化卡尔曼滤波器
    // Q=0.1: 距离变化较快
    // R=5.0: 超声波测量噪声较大
    Kalman_Init_1D(&distance_filter, 0.1f, 5.0f, 100.0f);
    
    while(1)
    {
        float raw_distance = Read_Distance_HC_SR04();
        float filtered_distance = Kalman_Update_1D(&distance_filter, raw_distance);
        
        printf("Distance - Raw: %.1f cm, Filtered: %.1f cm\r\n", 
               raw_distance, filtered_distance);
        
        HAL_Delay(50);
    }
}

五、调试与优化

5.1 参数调优方法

5.1.1 经验法则

1. Q和R的初始值选择：

   • 记录静态数据，计算方差作为R的初始值
   • Q通常设置为R的1/10到1/100

2. 观察滤波效果：

   • Q/R比值过大：滤波器响应快，但可能跟随噪声
   • Q/R比值过小：滤波器平滑，但响应慢

5.1.2 自适应调整

// 自适应卡尔曼滤波（简单版）
float Kalman_Update_Adaptive(KalmanFilter_1D *kf, float measurement)
{
    // 计算创新（预测误差）
    float innovation = measurement - kf->x;
    
    // 根据创新大小动态调整R
    if(fabsf(innovation) > 3.0f * sqrtf(kf->R)) {
        // 测量值异常，增大R减小权重
        kf->R *= 1.5f;
    } else {
        // 测量值正常,逐渐恢复R
        kf->R *= 0.99f;
        if(kf->R < 0.1f) kf->R = 0.1f;
    }
    
    // 正常卡尔曼更新
    return Kalman_Update_1D(kf, measurement);
}

5.2 性能优化

5.2.1 定点运算优化

对于计算资源有限的MCU，可以使用定点数代替浮点数：

// 定点卡尔曼滤波器（Q16格式）
typedef struct {
    int32_t x;      // 状态 (Q16)
    int32_t P;      // 协方差 (Q16)
    int32_t Q;      // 过程噪声 (Q16)
    int32_t R;      // 测量噪声 (Q16)
} KalmanFilter_Fixed;

#define Q16_ONE (1 << 16)

int32_t float_to_q16(float f) {
    return (int32_t)(f * Q16_ONE);
}

float q16_to_float(int32_t q) {
    return (float)q / Q16_ONE;
}

int32_t Kalman_Update_Fixed(KalmanFilter_Fixed *kf, int32_t measurement)
{
    // 协方差预测
    int64_t P_predict = (int64_t)kf->P + kf->Q;
    
    // 卡尔曼增益 K = P_predict / (P_predict + R)
    int64_t K = (P_predict << 16) / (P_predict + kf->R);
    
    // 状态更新
    int64_t innovation = measurement - kf->x;
    kf->x += (int32_t)((K * innovation) >> 16);
    
    // 协方差更新
    kf->P = (int32_t)(((Q16_ONE - K) * P_predict) >> 16);
    
    return kf->x;
}

5.2.2 中断优化

// 在定时器中断中进行高频采样
void TIM2_IRQHandler(void)
{
    if(__HAL_TIM_GET_FLAG(&htim2, TIM_FLAG_UPDATE))
    {
        __HAL_TIM_CLEAR_FLAG(&htim2, TIM_FLAG_UPDATE);
        
        // 快速读取传感器
        float raw_data = Read_Sensor_Fast();
        
        // 卡尔曼滤波
        filtered_data = Kalman_Update_1D(&filter, raw_data);
        
        data_ready_flag = 1;
    }
}

5.3 常见问题处理

5.3.1 滤波器发散

现象：估计值偏离真实值越来越远

原因：

• P矩阵数值下溢变为0
• Q值设置过小
• 模型与实际不符

解决方案：

// 添加协方差下限保护
void Kalman_Update_Safe(KalmanFilter_1D *kf, float measurement)
{
    // 正常更新...
    
    // 防止协方差过小
    if(kf->P < 0.0001f) {
        kf->P = 0.001f;
    }
}

5.3.2 响应过慢

解决方案：增大Q值或减小R值

// 动态调整以加快响应
if(fabsf(measurement - kf->x) > threshold) {
    kf->Q *= 2.0f;  // 临时增大Q
}

六、性能对比与测试

6.1 滤波效果对比

滤波方法	响应时间	平滑度	计算量	适用场景
均值滤波	慢	高	低	静态测量
中值滤波	中	中	中	脉冲噪声
一阶低通	快	低	低	简单场景
卡尔曼滤波	快	高	中	动态系统

6.2 测试代码示例

// 性能测试
void Performance_Test(void)
{
    uint32_t start_time, end_time;
    float test_data[1000];
    
    // 测试卡尔曼滤波耗时
    start_time = DWT_Get_Cycle_Count();
    for(int i = 0; i < 1000; i++) {
        test_data[i] = Kalman_Update_1D(&filter, i);
    }
    end_time = DWT_Get_Cycle_Count();
    
    uint32_t cycles = end_time - start_time;
    float time_us = cycles / (SystemCoreClock / 1000000.0f);
    
    printf("1000次滤波耗时: %.2f us, 平均: %.3f us/次\r\n", 
           time_us, time_us / 1000.0f);
}

七、进阶应用

7.1 扩展卡尔曼滤波（EKF）

对于非线性系统，需要使用扩展卡尔曼滤波：

// EKF基本框架
typedef struct {
    float x[N];           // 状态向量
    float P[N][N];        // 协方差矩阵
    // ... 其他参数
} EKF;

void EKF_Predict(EKF *ekf, float (*f)(float*), float (*F)(float*))
{
    // 使用非线性函数f预测状态
    float x_pred[N];
    for(int i = 0; i < N; i++) {
        x_pred[i] = f(ekf->x);
    }
    
    // 计算雅可比矩阵F
    // 更新协方差...
}

7.2 互补滤波对比

对于某些应用（如MPU6050），互补滤波是更简单的替代方案：

// 互补滤波器
float Complementary_Filter(float accel_angle, float gyro_rate, 
                          float dt, float alpha)
{
    static float angle = 0.0f;
    
    // alpha通常取0.98
    angle = alpha * (angle + gyro_rate * dt) + (1.0f - alpha) * accel_angle;
    
    return angle;
}

八、总结

8.1 核心要点

1. 算法选择：

   • 简单系统使用一维卡尔曼滤波
   • 复杂系统使用多维卡尔曼滤波
   • 非线性系统考虑EKF或UKF

2. 参数调优：

   • Q反映对模型的信任程度
   • R反映对测量的信任程度
   • 需要根据实际应用反复调试

3. 性能优化：

   • 资源受限时使用定点运算
   • 合理使用中断和DMA
   • 添加异常处理机制

8.2 实用建议

• 先在PC上用MATLAB验证算法
• 使用串口输出数据到上位机分析
• 记录调试过程中的参数变化
• 建立参数配置文件便于调整

8.3 参考资源

• 《Kalman Filtering: Theory and Practice Using MATLAB》
• STM32 HAL库官方文档
• 各传感器数据手册
• MPU6050 DMP固件参考

九、完整工程结构

KalmanFilter_Project/
├── Core/
│   ├── Inc/
│   │   ├── main.h
│   │   ├── kalman.h
│   │   ├── kalman_2d.h
│   │   └── mpu6050.h
│   └── Src/
│       ├── main.c
│       ├── kalman.c
│       ├── kalman_2d.c
│       └── mpu6050.c
├── Drivers/
│   └── STM32F1xx_HAL_Driver/
└── README.md

通过本文的学习，你应该能够在STM32项目中成功应用卡尔曼滤波算法，解决各种传感器数据噪声问题。记住，没有完美的参数，只有适合你应用的参数，多实验、多调试是成功的关键！

---

作者注：本文代码已在STM32F103C8T6上测试通过，使用HAL库开发。如有问题欢迎在评论区讨论交流！