卡尔曼滤波算法 C语言实现

unsigned long KalmanFilter(unsigned long inData)
{
    static float xdata kalman = 0; //上次卡尔曼值(估计出的最优值)
    static float xdata p = 10;
    float xdata q = 0.001; //q:过程噪声
    float xdata r = 0.001; //r:测量噪声
    float xdata kg = 0; //kg:卡尔曼增益

    p += q;
    kg = p / ( p + r ); //计算卡尔曼增益
    kalman = kalman + (kg * (inData - kalman)); //计算本次滤波估计值
    p = (1 - kg) * p; //更新测量方差
    
    return (unsigned long)kalman; //返回估计值
}