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

/**
  ******************************************************************************
  * @brief  卡尔曼滤波器 函数
  * @param  inData - 输入值
  * @return 滤波后的值
  * @note   r值固定，q值越大，代表越信任测量值，q值无穷大，代表只用测量值。
  *                  q值越小，代表越信任模型预测值，q值为0，代表只用模型预测值。
  *         q:过程噪声，q增大，动态响应变快，收敛稳定性变坏；反之。控制误差 
  *         r:测量噪声，r增大，动态响应变慢，收敛稳定性变好；反之。控制响应速度
  ******************************************************************************
  */
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; //返回估计值
}