最近研究STM32的自平衡小車，發現有兩座必過的大山，一為卡爾曼濾波，二為PID算法。 網上看了很多關于卡爾曼濾波的代碼，感覺寫得真不咋地。一怒之下，自己重寫，不廢話，貼代碼 [pre lang="C" line="1" file="kalman.h"]/** ??****************************************************************************** ??* @file? ? kalman.h ??* @author??willieon ??* @version V0.1 ??* @date? ? January-2015 ??* @brief? ?卡爾曼濾波算法? ??*? ? ? ?? ??* ??****************************************************************************** ??* @attention ??*本人對卡爾曼的粗略理解：以本次測量角速度（陀螺儀測量值）的積分得出的角度值 ??* 與上次最優角度值的方差產生一個權重來衡量本次測量角度（加速度測量值） ??* 與上次最優角度值，從而產生新的最優角度值。好吧，比較拗口，有誤處忘指正。 ??* ??****************************************************************************** ??*/ #ifndef __KALMAN_H__ #define __KALMAN_H__ #define Q_angle? ? ? ? ? ? ? ? ? ? ? ? 0.001? ? ? ? 角度過程噪聲的協方差 #define Q_gyro? ? ? ? ? ? ? ? ? ? ? ? 0.003? ? ? ? 角速度過程噪聲的協方差 #define R_angle? ? ? ? ? ? ? ? ? ? ? ? 0.5? ? ? ? ? ? ? ? 測量噪聲的協方差（即是測量偏差） #define dt? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 0.01? ? ? ? ? ? ? ? ? ? ? ? 卡爾曼濾波采樣頻率 #define C_0? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 1 /**************卡爾曼運算變量定義********************** * ***由于卡爾曼為遞推運算，結構體需定義為全局變量 ***在實際運用中只需定義一個KalmanCountData類型的變量即可 ***無需用戶定義多個中間變量，簡化函數的使用 */ typedef struct { ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? Q_bias;? ? ? ? ? ? ? ? 最優估計值的偏差，即估計出來的陀螺儀的漂移量 ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? Angle_err;? ? ? ? ? ? ? ? 實測角度與陀螺儀積分角度的差值 ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? PCt_0;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?? ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? PCt_1;? ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? E;? ? ? ? ? ? ? ? ? ? ? ? 計算的過程量 ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? K_0;? ? ? ? ? ? ? ? ? ? ? ? 含有卡爾曼增益的另外一個函數，用于計算最優估計值 ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? K_1;? ? ? ? ? ? ? ? ? ? ? ? 含有卡爾曼增益的函數，用于計算最優估計值的偏差 ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? t_0;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?? ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? t_1; ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? Pdot[4];? ? ? ? ? ? ? ? Pdot[4] = {0,0,0,0};過程協方差矩陣的微分矩陣 ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? PP[2][2];? ? ? ? ? ? ? ? PP[2][2] = { { 1, 0 },{ 0, 1 } };協方差(covariance) ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? Angle_Final;? ? ? ? 后驗估計最優角度值（即系統處理最終值） ? ? ? ? float? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? Gyro_Final;? ? ? ? 后驗估計最優角速度值 }KalmanCountData; void Kalman_Filter(float Accel,? ? ? ? float Gyro ,KalmanCountData * Kalman_Struct); void Kalman_Filter_Init(KalmanCountData * Kalman_Struct); #endif [/pre] kalman.c [pre lang="C" line="1"??file="kalman.c"] #include "kalman.h" /** ??****************************************************************************** ??* @file? ? void Kalman_Filter_Init(KalmanCountData * Kalman_Struct) ??* @author??willieon ??* @version V0.1 ??* @date? ? January-2015 ??* @brief? ?卡爾曼濾波計算中間量初始化 ??*? ? ? ?? ??* ??****************************************************************************** ??* @attention ??* ??*? ??*? ??* ??****************************************************************************** ??*/ void Kalman_Filter_Init(KalmanCountData * Kalman_Struct) { ? ? ? ? Kalman_Struct -> Angle_err? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> Q_bias? ? ? ? ? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> PCt_0? ? ? ? ? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> PCt_1? ? ? ? ? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> E? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> K_0? ? ? ? ? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> K_1? ? ? ? ? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> t_0? ? ? ? ? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> t_1? ? ? ? ? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> Pdot[0]? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> Pdot[1]? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> Pdot[2]? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> Pdot[3]? ? ? ? ? ? ? ???= 0;? ? ? ?? ? ? ? ? Kalman_Struct -> PP[0][0]? ? ? ? ? ? ? ???= 1; ? ? ? ? Kalman_Struct -> PP[0][1]? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> PP[1][0]? ? ? ? ? ? ? ???= 0; ? ? ? ? Kalman_Struct -> PP[1][1]? ? ? ? ? ? ? ???= 1;? ? ? ?? ? ? ? ? Kalman_Struct -> Angle_Final? ? ? ???= 0; ? ? ? ? Kalman_Struct -> Gyro_Final? ? ? ? ? ? ? ???= 0; } /** ??****************************************************************************** ??* @file? ? void Kalman_Filter(float Accel,? ? ? ? float Gyro ,KalmanCountData * Kalman_Struct) ??* @author??willieon ??* @version V0.1 ??* @date? ? January-2015 ??* @brief? ?卡爾曼濾波計算 ??*? ? ? ?? ??* ??****************************************************************************** ??* @attention ??*? ? ? ? ? ? ? ? Accel:加速度計數據處理后進來的角度值 ??*? ? ? ? ? ? ? ? Gyro :陀螺儀數據處理后進來的角速度值 ??*? ? ? ? ? ? ? ? Kalman_Struct:遞推運算所需要的中間變量，由用戶定義為全局結構體變量 ??*? ? ? ? ? ? ? ? Kalman_Struct -> Angle_Final??為濾波后角度最優值 ??*? ? ? ? ? ? ? ? Kalman_Struct -> Gyro_Final? ?為后驗角度值 ??****************************************************************************** ??*/ void Kalman_Filter(float Accel,? ? ? ? float Gyro ,KalmanCountData * Kalman_Struct) { ? ? ? ? ? ? ? ? //陀螺儀積分角度（先驗估計） ? ? ? ? ? ? ? ? Kalman_Struct -> Angle_Final += (Gyro - Kalman_Struct -> Q_bias) * dt;? ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? //先驗估計誤差協方差的微分 ? ? ? ? ? ? ? ? Kalman_Struct -> Pdot[0] = Q_angle - Kalman_Struct -> PP[0][1] - Kalman_Struct -> PP[1][0];? ? ? ? ? ? ? ? ? Kalman_Struct -> Pdot[1] = - Kalman_Struct -> PP[1][1]; ? ? ? ? ? ? ? ? Kalman_Struct -> Pdot[2] = - Kalman_Struct -> PP[1][1]; ? ? ? ? ? ? ? ? Kalman_Struct -> Pdot[3] = Q_gyro; ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? //先驗估計誤差協方差的積分 ? ? ? ? ? ? ? ? Kalman_Struct -> PP[0][0] += Kalman_Struct -> Pdot[0] * dt;? ? ? ? ? ? ? ? ? ? Kalman_Struct -> PP[0][1] += Kalman_Struct -> Pdot[1] * dt;? ? ? ? ? ? ? ? ? ? Kalman_Struct -> PP[1][0] += Kalman_Struct -> Pdot[2] * dt; ? ? ? ? ? ? ? ? Kalman_Struct -> PP[1][1] += Kalman_Struct -> Pdot[3] * dt; ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? //計算角度偏差 ? ? ? ? ? ? ? ? Kalman_Struct -> Angle_err = Accel - Kalman_Struct -> Angle_Final;? ? ? ?? ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? //卡爾曼增益計算 ? ? ? ? ? ? ? ? Kalman_Struct -> PCt_0 = C_0 * Kalman_Struct -> PP[0][0]; ? ? ? ? ? ? ? ? Kalman_Struct -> PCt_1 = C_0 * Kalman_Struct -> PP[1][0]; ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? Kalman_Struct -> E = R_angle + C_0 * Kalman_Struct -> PCt_0; ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? Kalman_Struct -> K_0 = Kalman_Struct -> PCt_0 / Kalman_Struct -> E; ? ? ? ? ? ? ? ? Kalman_Struct -> K_1 = Kalman_Struct -> PCt_1 / Kalman_Struct -> E; ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? //后驗估計誤差協方差計算 ? ? ? ? ? ? ? ? Kalman_Struct -> t_0 = Kalman_Struct -> PCt_0; ? ? ? ? ? ? ? ? Kalman_Struct -> t_1 = C_0 * Kalman_Struct -> PP[0][1]; ? ? ? ? ? ? ? ? Kalman_Struct -> PP[0][0] -= Kalman_Struct -> K_0 * Kalman_Struct -> t_0;? ? ? ? ? ? ? ??? ? ? ? ? ? ? ? ? Kalman_Struct -> PP[0][1] -= Kalman_Struct -> K_0 * Kalman_Struct -> t_1; ? ? ? ? ? ? ? ? Kalman_Struct -> PP[1][0] -= Kalman_Struct -> K_1 * Kalman_Struct -> t_0; ? ? ? ? ? ? ? ? Kalman_Struct -> PP[1][1] -= Kalman_Struct -> K_1 * Kalman_Struct -> t_1; ? ? ? ? ? ? ? ? Kalman_Struct -> Angle_Final += Kalman_Struct -> K_0 * Kalman_Struct -> Angle_err;? ? ? ???//后驗估計最優角度值 ? ? ? ? ? ? ? ? Kalman_Struct -> Q_bias? ? ? ? += Kalman_Struct -> K_1 * Kalman_Struct -> Angle_err;? ? ? ? ? ? ? ???//更新最優估計值的偏差 ? ? ? ? ? ? ? ? Kalman_Struct -> Gyro_Final? ?= Gyro - Kalman_Struct -> Q_bias;? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ???//更新最優角速度值 } [/pre] 代碼可以放在實際工程中使用，也可以用VS等C編譯工具進行實驗學習。在VS中的main()實例使用如下 [pre lang="C" line="1" file="main.c"] #include "kalman.h" #include "stdio.h" #include "stdlib.h" void main(void) { ? ? ? ? KalmanCountData k; ? ? ? ? //定義一個卡爾曼運算結構體 ? ? ? ? Kalman_Filter_Init(&k); ? ? ? ? //講運算變量初始化 ? ? ? ? int m,n;? ?? ??? ? ?? ? ? ? for(int a = 0;a<80;a++) ? ? ? ? //測試80次 ? ? ? ? { ? ? ? ? ? ? ? ? //m,n為1到100的隨機數 ? ? ? ? ? ? ? ? m = 1+ rand() %100; ? ? ? ? ? ? ? ? n = 1+ rand() %100; ? ?? ?? ?? ?? ? //卡爾曼濾波，傳遞2個測量值以及運算結構體 ? ? ? ?? ? ? ? ? Kalman_Filter((float)m,(float)n,&k); ? ? ? ? ? ? ? ? //打印結果 ? ? ? ? ? ? ? ? printf("%d and %d is %f - %f\r\n",m,n,k.Angle_Final,k.K_0); ? ? ? ?? ? ? ? ? } } [/pre]