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內部溫度傳感器實驗

//引數配置
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值會收斂成一個最小的估計方差矩陣，這個時候的卡爾曼增益也是最優的，所以這個值只是影響初始收斂速度，