4d_ObstacleRadarDroneCAN/App/KelmanFilter.c

#include "KelmanFilter.h"
    
float KelmanFilter_1Dimension(float InputData, bool ResetFlag)
{
    static float x_est = 0.0f;   // 状态估计（平滑后的 cy）
    static float P     = 1.0f;   // 估计不确定性
    static float Q     = 1e-3f;  // 过程噪声
    static float R     = 1e-1f;  // 测量噪声
    static bool initialized = false;

    /* 初始化或重置 */
    if (!initialized || ResetFlag)
    {
        x_est = InputData;
        P = 1.0f;
        Q = 1e-3f;
        R = 1e-1f;
        initialized = true;
        return x_est;
    }

    /* 预测*/
    float x_pred = x_est;   // 假设距离不变
    float P_pred = P + Q;   // 不确定性增加

     /* 更新*/
    float K = P_pred / (P_pred + R);  // 卡尔曼增益
    x_est = x_pred + K * (InputData - x_pred);
    P = (1.0f - K) * P_pred;

    return x_est;
}