卡尔曼滤波算法c语言stm32

下面是一个简单的卡尔曼滤波算法C语言实现，可以在STM32上使用：

#include "kalman_filter.h"

float x_est_last = 0; // 上一次的状态估计值
float P_last = 0; // 上一次的状态协方差矩阵

// 卡尔曼滤波算法
float kalman_filter(float mea)
{
    float Q = 0.1; // 过程噪声协方差
    float R = 0.5; // 测量噪声协方差

    // 预测
    float x_temp_est = x_est_last; // 上一次的状态估计值作为本次的初始值
    float P_temp = P_last + Q; // 上一次的状态协方差矩阵加上过程噪声协方差

    // 更新
    float K = P_temp / (P_temp + R); // 卡尔曼增益
    float x_est = x_temp_est + K * (mea - x_temp_est); // 状态估计值
    float P = (1 - K) * P_temp; // 状态协方差矩阵

    // 保存上一次的状态估计值和状态协方差矩阵
    x_est_last = x_est;
    P_last = P;

    return x_est;
}

在使用时，只需要调用`kalman_filter()`函数，传入当前的测量值即可。需要注意的是，过程噪声协方差和测量噪声协方差需要根据具体应用场景进行调整。此外，卡尔曼滤波算法还可以进行优化，例如加入自适应调整因子等，以提高滤波效果。