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

文章目录

  • 0.前言
  • 1.一阶卡尔曼滤波代码实现
    • 1.1 Kalman滤波代码
      • 1.1.1 Kalman.c文件
      • 1.1.2 Kalman.h文件
  • 2.一阶卡尔曼滤波代码应用
    • 2.1 在源程序中加入.c/.h文件
    • 2.2 Kalman应用
    • 2.3 Kalman在稳定CPU温度传感器读数效果展示
    • 2.4 Kalman在稳定红外传感器读数效果展示
  • 3.一阶卡尔曼滤波代码调参
    • 3.1 直观上将数学公式与代码一一对应
    • 3.2 纸质版个人理解
    • 3.3 参考理解

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.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内部温度传感器实验
STM32应用(六)一阶卡尔曼滤波代码和简单应用_第1张图片

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

whilefloat 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左右的位置。
STM32应用(六)一阶卡尔曼滤波代码和简单应用_第2张图片

//参数配置
void Kalman_Init()
{
	
	kfp.Last_P = 1;			
	kfp.Now_P = 0;		
	kfp.out = 0;			
	kfp.Kg = 0;		
	kfp.Q = 0;
	kfp.R = 0.1;
}
whileHAL_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 直观上将数学公式与代码一一对应

STM32应用(六)一阶卡尔曼滤波代码和简单应用_第3张图片
除了输出不是我们在调参时考虑的参数外,其他五个参数都相互影响。

3.2 纸质版个人理解

STM32应用(六)一阶卡尔曼滤波代码和简单应用_第4张图片
STM32应用(六)一阶卡尔曼滤波代码和简单应用_第5张图片
字太丑了呜呜呜…

3.3 参考理解

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

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

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

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

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

你可能感兴趣的:(STM32CubeMx,stm32,算法,嵌入式硬件,传感器)