STM32应用(六)一阶卡尔曼滤波代码和简单应用

STM32应用(五)基于输入捕获的超声波HC-SR04模块使用

1.一阶卡尔曼滤波代码实现

1.1 Kalman滤波代码

1.1.1 Kalman.c文件

#include "Kalman.h"

void Kalman\_Init()
{
	kfp.Last_P = 1;			
	kfp.Now_P = 0;		
	kfp.out = 0;			
	kfp.Kg = 0;		
	kfp.Q = 0;
	kfp.R = 0.01;
}

/\*\*
 \*卡尔曼滤波器
 \*@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;
}

1.1.2 Kalman.h文件

#ifndef Kalman\_H
#define Kalman\_H

#include "main.h"

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

void Kalman\_Init(void);
float KalmanFilter(Kalman \*kfp,float input);

extern Kalman kfp;

#endif

2.一阶卡尔曼滤波代码应用

2.1 在源程序中加入.c/.h文件

在Keil中添加新的文件

2.2 Kalman应用

/\* USER CODE BEGIN Includes \*/
#include "Kalman.h"
/\* USER CODE END Includes \*/

/\* USER CODE BEGIN PV \*/
extern Kalman kfp;
/\* USER CODE END PV \*/
  /\* USER CODE BEGIN 2 \*/
	Kalman\_Init();
  /\* USER CODE END 2 \*/
  while (1)
  {
		KalmanFilter(&kfp,supersonic.distance);
		printf("%f",supersonic.distance);
		HAL\_Delay(100);
  }

2.3 Kalman在稳定CPU温度传感器读数效果展示

STM32F407内部温度传感器实验

//参数配置
void Kalman\_Init()
{
	
	kfp.Last_P = 1;			
	kfp.Now_P = 0;		
	kfp.out = 0;			
	kfp.Kg = 0;		
	kfp.Q = 0;
	kfp.R = 0.01;
}

while：
		float Last_ADC_Value;
		HAL\_ADC\_Start(&hadc1);
		Last_ADC_Value=HAL\_ADC\_GetValue(&hadc1);
		HAL\_Delay(5);
		Last_ADC_Value=KalmanFilter(&kfp,Last_ADC_Value);
		Tempture_Value=Last_ADC_Value\*(3.3/4096);			//电压值
		Tempture_Value=(Tempture_Value-0.76)/0.0025+25;
		printf("%f\r\n", Tempture_Value);
		HAL\_Delay(10);

程色波动的数据是没有添加Kalman滤波的效果，浅蓝色的数据是添加后的效果。

2.4 Kalman在稳定红外传感器读数效果展示

稳定ADC读取红外测距模块的数值
我大致放在据障碍物8cm左右的位置。

//参数配置
void Kalman\_Init()
{
	
	kfp.Last_P = 1;			
	kfp.Now_P = 0;		
	kfp.out = 0;			
	kfp.Kg = 0;		
	kfp.Q = 0;
	kfp.R = 0.1;
}
while：
		HAL\_ADC\_Start(&hadc1);
		ADC_Value=HAL\_ADC\_GetValue(&hadc1);
		ADC_Value=3\*ADC_Value/4095;
		ADC_Value=KalmanFilter(&kfp,ADC_Value);
		printf("%f\r\n", ADC_Value);
		HAL\_Delay(10);

程色波动的数据是没有添加Kalman滤波的效果，浅蓝色的数据是添加后的效果。

3.一阶卡尔曼滤波代码调参

在理解公式的基础上再进行调参是事半功倍的。

3.1 直观上将数学公式与代码一一对应

除了输出不是我们在调参时考虑的参数外，其他五个参数都相互影响。

3.2 纸质版个人理解

字太丑了呜呜呜…

3.3 参考理解

以下内容摘自：卡尔曼滤波中关键参数的调整