代码收藏家 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.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文件
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温度传感器读数效果展示
//参数配置
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 参考理解
以下内容摘自：卡尔曼滤波中关键参数的调整
Q值为过程噪声，越小系统越容易收敛，我们对模型预测的值信任度越高；但是太小则容易发散，如果Q为零，那么我们只相信预测值；Q值越大我们对于预测的信任度就越低，而对测量值的信任度就变高；如果Q值无穷大，那么我们只信任测量值；
R值为测量噪声，太小太大都不一定合适。R太大，卡尔曼滤波响应会变慢，因为它对新测量的值的信任度降低；越小系统收敛越快，但过小则容易出现震荡；R值的改变主要是影响卡尔曼的收敛速度。
测试时可以先将Q从小往大调整，将R从大往小调整；先固定一个值去调整另外一个值，看收敛速度与波形输出。
系统中还有一个关键值P，它是误差协方差初始值，表示我们对当前预测状态的信任度，它越小说明我们越相信当前预测状态；它的值决定了初始收敛速度，一般开始设一个较小的值以便于获取较快的收敛速度。随着卡尔曼滤波的迭代，P的值会不断的改变，当系统进入稳态之后P值会收敛成一个最小的估计方差矩阵，这个时候的卡尔曼增益也是最优的，所以这个值只是影响初始收敛速度。
