最近研究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]

總結

以上是生活随笔為你收集整理的平衡小车卡尔曼滤波算法的全部內容，希望文章能夠幫你解決所遇到的問題。

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