【STM32代码移植问题3：卡尔曼滤波（一阶）】

参考 https://www.bilibili.com/video/BV1Rh41117MT/?p=4&vd_source=362e9cf243ad251a712b82a029f5b2a1
一阶卡尔曼滤波代码和简单应用： https://blog.csdn.net/weixin_45751396/article/details/119595886
二阶？： https://blog.csdn.net/weixin_44549777/article/details/124665317

stm32f030

滤波器：

/**
 *¿¨¶ûÂüÂ˲¨Æ÷
 *@param 	Kalman *kfp ¿¨¶ûÂü½á¹¹Ìå²ÎÊý
 *   			float input ÐèÒªÂ˲¨µÄ²ÎÊýµÄ²âÁ¿Öµ£¨¼´´«¸ÐÆ÷µÄ²É¼¯Öµ£©
 *@return Â˲¨ºóµÄ²ÎÊý£¨×îÓÅÖµ£©
 */
float KalmanFilter_1(Kalman *kfp,float input)
{
   //Ô¤²âЭ·½²î·½³Ì£ºkʱ¿Ìϵͳ¹ÀËãЭ·½²î = k-1ʱ¿ÌµÄϵͳЭ·½²î + ¹ý³ÌÔëÉùЭ·½²î
   kfp->Now_P = kfp->Last_P + kfp->Q;
   //¿¨¶ûÂüÔöÒæ·½³Ì£º¿¨¶ûÂüÔöÒæ = kʱ¿Ìϵͳ¹ÀËãЭ·½²î / £¨kʱ¿Ìϵͳ¹ÀËãЭ·½²î + ¹Û²âÔëÉùЭ·½²î£©
   kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
   //¸üÐÂ×îÓÅÖµ·½³Ì£ºkʱ¿Ì״̬±äÁ¿µÄ×îÓÅÖµ = ״̬±äÁ¿µÄÔ¤²âÖµ + ¿¨¶ûÂüÔöÒæ * £¨²âÁ¿Öµ - ״̬±äÁ¿µÄÔ¤²âÖµ£©
   kfp->out = kfp->out + kfp->Kg * (input - kfp->out);//ÒòΪÕâÒ»´ÎµÄÔ¤²âÖµ¾ÍÊÇÉÏÒ»´ÎµÄÊä³öÖµ
   //¸üÐÂЭ·½²î·½³Ì: ±¾´ÎµÄϵͳЭ·½²î¸¶¸ø kfp->LastP ÍþÏÂÒ»´ÎÔËËã×¼±¸¡£
   kfp->Last_P = (1 - kfp->Kg) * kfp->Now_P;
   return kfp->out;
}

结构体定义 、 结构体变量申明（Kalman kfp_1,kfp_2; ）、结构体变量初始化

typedef struct 
{
    float Last_P;//ÉϴιÀËãЭ·½²î ²»¿ÉÒÔΪ0 ! ! ! ! ! 
    float Now_P;//µ±Ç°¹ÀËãЭ·½²î
    float out;//¿¨¶ûÂüÂ˲¨Æ÷Êä³ö
    float Kg;//¿¨¶ûÂüÔöÒæ
    float Q;//¹ý³ÌÔëÉùЭ·½²î
    float R;//¹Û²âÔëÉùЭ·½²î
}Kalman;
Kalman kfp_1,kfp_2;	

void Kalman_Init_1(void)
{
	kfp_1.Last_P = 1;			
	kfp_1.Now_P = 0;		
	kfp_1.out = 0;			
	kfp_1.Kg = 0;		
	kfp_1.Q = 0.000001;
	kfp_1.R = 0.0004;
}
void Kalman_Init_2(void)
{
	kfp_2.Last_P = 1;			
	kfp_2.Now_P = 0;		
	kfp_2.out = 0;			
	kfp_2.Kg = 0;		
	kfp_2.Q = 0.000001;
	kfp_2.R = 0.0004;
}

主函数中调用：

	Kalman_Init_1();
	Kalman_Init_2();
	float diff=0.00，sum=0.00;
	 while (1)
  {
diff = get1（value）;
sum  = get2（value）;
diff = KalmanFilter_1(&kfp_1,diff);
sum  = KalmanFilter_1(&kfp_1,sum  );//可以再定义滤波器2：KalmanFilter_2使得sum  = KalmanFilter_2(&kfp_2,sum  );
  }

其他，如QT

定义：

typedef struct
{
    float Last_P;//上次估算协方差 不可以为0 ! ! ! ! !
    float Now_P;//当前估算协方差
    float out;//卡尔曼滤波器输出
    float Kg;//卡尔曼增益
    float Q;//过程噪声协方差
    float R;//观测噪声协方差
}Kalman;
Kalman kfp2;

/**
 *卡尔曼滤波器
 *@param 	Kalman *kfp 卡尔曼结构体参数
 *   			float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
float KalmanFilter(Kalman *kfp,float input)
{
   //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
   kfp->Now_P = kfp->Last_P + kfp->Q;
   //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
   kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
   //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
   kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
   //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
   kfp->Last_P = (1-kfp->Kg) * kfp->Now_P;
   return kfp->out;
}

主程序：

        kfp2.Last_P = 1;
        kfp2.Now_P = 0;
        kfp2.out = 0;
        kfp2.Kg = 0;
        kfp2.Q = 0.000001;
        kfp2.R = 0.0004;
  		float temperatur,temperature2;
  		temperature=getvalue();
  		temperature2=KalmanFilter(&kfp2,temperature);