卡尔曼滤波

yanjiang0456 · 发表于 2019-4-24 09:14:18

有哪位仁兄做过基于卡尔曼滤波的温度预测（DS18B20），在STM32战舰开发板上实现过，

yun1003 · 发表于 2019-4-24 09:14:19

/*
**卡尔曼滤波算法
**R+Q=1,R越大滤波效果越好,迟滞性越强；Q越大跟随性越好
*/
INT16U f_kalman_fliter(INT16U z_measure)
{
      double  kg;
      double  x_mid;
      double  x_now;
      double  p_mid;
      double  p_now;

      static double Q=0.1;
      static double  R=0.9;
      static double x_last=0;
      static double p_last=0;

      x_mid=x_last;
      p_mid=p_last+Q;
      kg=p_mid/(p_mid+R);
      x_now=x_mid+kg*(z_measure-x_mid);                      //估计最优值
      p_now=(1-kg)*p_mid;                                                       //最优值对应的协方差

      p_last=p_now;                                                             //更新状态值
      x_last=x_now;

      return (INT16U)x_now;
}