卡尔曼算法

只看该作者 · 2024-11-18 14:04

/********************************************************************

*@函数名：KalmanFilter(float newdata)

*@功  能：卡尔曼滤波

*@形  参：newdata 新数据 

*@返回值：滤波后数据

*@备  注：要定义过程噪声和测量噪声

********************************************************************/

 

/*

        Q: 过程噪声，Q增大，动态响应变快，收敛稳定性变坏

        R: 测量噪声，R增大，动态响应变慢，收敛稳定性变好

        其中p的初值可以随便取，但是不能为0（为0的话卡尔曼滤波器就认为已经是最优滤波器了）

  q,r的值需要我们试出来，讲白了就是(买的破温度计有多破，以及你的超人力有多强)

  r参数调整滤波后的曲线与实测曲线的相近程度，r越小越接近。

  q参数调滤波后的曲线平滑程度，q越小越平滑。

*/

//过程噪音  

#define P_Q   0.1

//测量噪声

#define M_R   0.01

float KalmanFilter(float newdata)

{

   static float lastdata=0;

   //其中p的初值可以随便取，但是不能为0（为0的话卡尔曼滤波器就认为已经是最优滤波器了）

   static float p=0.01;

   float        q = P_Q, r = M_R, kGain = 0;

        

   p = p+q;

   kGain = p/(p+r);  //卡尔曼滤波系数

   newdata = lastdata + (kGain*(newdata-lastdata));

   p = (1-kGain)*p;

   lastdata = newdata;  

   return newdata;

}

只看该作者 · 2024-11-18 18:54

卡尔曼滤波算法。