目錄

• 代碼
• • kalman.c
  • kalman.h
• 濾波效果

---

很久以前抄的，忘了是從哪弄的了
我把它改成了這種結構體指針傳參的形式，方便在比賽中應用。應用舉例見MSP432 PID 電機調角度、調速。
它曾助力筆者獲2020年電賽省一等獎。

代碼

Q:過程噪聲協方差 Q參數調濾波后的曲線平滑程度，Q越小越平滑;
R:觀測噪聲協方差 R參數調整濾波后的曲線與實測曲線的相近程度，R越小越接近(收斂越快)

kalman.c

/* 卡爾曼濾波器 整理By 乙酸氧鈹 */ #include "kalman.h"double KalmanFilter(Kalman_Typedef *klm, double input) {//預測協方差方程：k時刻系統估算協方差 = k-1時刻的系統協方差 + 過程噪聲協方差klm->Now_P = klm->LastP + klm->Q;//卡爾曼增益方程：卡爾曼增益 = k時刻系統估算協方差 / （k時刻系統估算協方差 + 觀測噪聲協方差）klm->Kg = klm->Now_P / (klm->Now_P + klm->R);//更新最優值方程：k時刻狀態變量的最優值 = 狀態變量的預測值 + 卡爾曼增益 * （測量值 - 狀態變量的預測值）klm->out = klm->out + klm->Kg * (input -klm->out);//因為這一次的預測值就是上一次的輸出值//更新協方差方程: 本次的系統協方差賦給 klm->LastP 為下一次運算準備。klm->LastP = (1-klm->Kg) * klm->Now_P;return (klm->out); }void Kalman_Init(Kalman_Typedef *klm, const double klm_Q, const double klm_R)//溫度klm_Q=0.01 klm_R=0.25 {klm->LastP=0.02; //上次估算協方差klm->Now_P=0; //當前估算協方差klm->out=0; //卡爾曼濾波器輸出klm->Kg=0; //卡爾曼增益klm->Q=klm_Q; //Q:過程噪聲協方差 Q參數調濾波后的曲線平滑程度，Q越小越平滑;klm->R=klm_R; //R:觀測噪聲協方差 R參數調整濾波后的曲線與實測曲線的相近程度，R越小越接近(收斂越快) }

kalman.h

#ifndef __KALMAN_H__ #define __KALMAN_H__typedef struct {/*不用動*/double LastP;//上次估算協方差double Now_P;//當前估算協方差double out;//卡爾曼濾波器輸出double Kg;//卡爾曼增益double Q;double R; }Kalman_Typedef;void Kalman_Init(Kalman_Typedef *klm, const double klm_Q, const double klm_R); double KalmanFilter(Kalman_Typedef *klm, double input);#endif

濾波效果

總結

以上是生活随笔為你收集整理的【电赛】一阶卡尔曼滤波器 滤波效果良好的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。