單片機ADC采樣演算法 之 卡爾曼濾波
演算法的核心思想是,根據當前的儀器"測量值" 和上一刻的 “預測量” 和 “誤差”,計算得到當前的最優量.
再預測下一刻的量, 里面比較突出的是觀點是. 把誤差納入計算, 而且分為預測誤差和測量誤差兩種.通稱為 噪聲. 還有一個非常大的特點是,誤差獨立存在, 始終不受測量資料的影響，
下來先了解一個卡爾曼濾波中幾個引數的含義：概率（Probability），隨即變數（Random Variable），高斯或正態分配（Gaussian Distribution）還有State-space Model等等，
關于卡爾曼公式的含義及推導，網上已經有很多文章了，這里不在贅述，直接看C代碼的實作，
/*
R值固定，Q值越大，代表越信任測量值，Q值無窮大，代表只用測量值，
Q值越小，代表越信任模型預測值，Q值為0，代表只用模型預測值，
*/
//引數一
float KalmanFilter( float inData )
{
static float prevData = 0; //上一個資料
static float p = 10, q = 0.001, r = 0.001, kGain = 0; // q 控制誤差 r 控制回應速度
p = p + q;
kGain = p / ( p + r ); //計算卡爾曼增益
inData = prevData + ( kGain * ( inData - prevData ) ); //計算本次濾波估計值
p = ( 1 - kGain ) * p; //更新測量方差
prevData = inData;
return inData; //回傳估計值
}
現在測驗一下卡爾曼濾波的效果，通過函式發生器產生一個鋸齒波，送到單片機的AD口，單片機讀取采集到的AD資料后，經過卡爾曼濾波演算法，然后將采樣的資料和濾波后的資料通過串口發生出來，并在串口波形顯示軟體上顯示，
void main( void )
{
while( 1 )
{
val1 = ReadVol_CH3(); //讀取AD采樣資料
dat = ( float )val1;
dat = KalmanFilter( dat ); //卡爾曼濾波
printf("A%drn",val1); //列印結果
printf("B%2frn",dat);
}
}
現在看一下濾波的結果
藍色曲線為原始采樣的資料曲線，橙色曲線為經過卡爾曼濾波后的曲線，
下面改變Q和R的值在測驗一下濾波效果，
修改后的引數如下
//引數二
unsigned long kalman_filter( unsigned long ADC_Value )
{
float LastData;
float NowData;
float kalman_adc;
static float kalman_adc_old = 0;
static float P1;
static float Q = 0.0003;
static float R = 5;
static float Kg = 0;
static float P = 1;
NowData = ADC_Value;
LastData = kalman_adc_old;
P = P1 + Q;
Kg = P / ( P + R );
kalman_adc = LastData + Kg * ( NowData - kalman_adc_old );
P1 = ( 1 - Kg ) * P;
P = P1;
kalman_adc_old = kalman_adc;
return ( unsigned long )( kalman_adc );
}
測驗波形
藍色曲線為原始采樣的資料曲線，橙色曲線為經過卡爾曼濾波后的曲線，
和第一次測驗的波形圖對比后可以發現，第二次經過卡爾曼濾波后的波形變化非常大，引數改變后鋸齒波被濾成接近于直線了，
可以看到不同的R、Q值會對測量結果有很大的影響，
Q:程序噪聲，Q增大，動態回應變快，收斂穩定性變壞
R:測量噪聲，R增大，動態回應變慢，收斂穩定性變好
具體各個引數的如何選擇，只有在應用中根據測量結果，自己慢慢調整，
轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/231077.html
標籤：其他