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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。