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

0.前言

可以先进行这两篇的理解和配置再开始一阶卡尔曼滤波的实践。
STM32应用(三)一阶卡尔曼滤波原理和ADC读取红外测距模块的数值
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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

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);
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

程色波动的数据是没有添加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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

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

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

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

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

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

3.2 纸质版个人理解

字太丑了呜呜呜…

3.3 参考理解

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

Q值为过程噪声，越小系统越容易收敛，我们对模型预测的值信任度越高；但是太小则容易发散，如果Q为零，那么我们只相信预测值；Q值越大我们对于预测的信任度就越低，而对测量值的信任度就变高；如果Q值无穷大，那么我们只信任测量值；

R值为测量噪声，太小太大都不一定合适。R太大，卡尔曼滤波响应会变慢，因为它对新测量的值的信任度降低；越小系统收敛越快，但过小则容易出现震荡；R值的改变主要是影响卡尔曼的收敛速度。

测试时可以先将Q从小往大调整，将R从大往小调整；先固定一个值去调整另外一个值，看收敛速度与波形输出。

系统中还有一个关键值P，它是误差协方差初始值，表示我们对当前预测状态的信任度，它越小说明我们越相信当前预测状态；它的值决定了初始收敛速度，一般开始设一个较小的值以便于获取较快的收敛速度。随着卡尔曼滤波的迭代，P的值会不断的改变，当系统进入稳态之后P值会收敛成一个最小的估计方差矩阵，这个时候的卡尔曼增益也是最优的，所以这个值只是影响初始收敛速度。