本帖最后由 jackk 于 2014-3-5 13:00 編輯

前段時間在論壇里簡單地發了一些關于kalman的理解。

有很多網友頂貼的，趁著今天休息，整理一下前段時間的工作。

有些理解和說法可能不正確，以此拋磚引玉吧。

1，

在google上搜索卡爾曼濾波，很容易找到以下這個帖子：

http://blog.csdn.net/lanbing510/article/details/8828109

這里面很簡單形象的解釋了kalman的作用。

但是帖子后半段，將kalman回歸到了一大堆數學推理、推導上，對非數學專業，或者數學基礎尤其是概率和隨機過程基礎不好的同學來講實在太過頭痛。

帖子最后用matlab實現了kalman，其中很簡單地用i，j，k來命名，可能不是太妥當，新手理解的時候，腦袋里面要運行著一個很深的堆棧。

當然，原作者毫無疑問對kalman有著極其深刻的理解，才能實現得如此游刃有余。

在此，只是對第一次接觸kalman的同學簡單介紹一下自己的理解。幫得到諸君為好，幫不到請勿噴。

2，

首先，kalman是一個數字濾波器。

我們可以用硬件搭建一個模擬濾波器，將疊加了噪聲的模擬信號輸入到濾波器中，濾波器給出一個響應。

我們把這個響應作為輸入信號踢掉噪聲之后的真值。

當然，可以通過調整濾波器參數，使得響應盡可能接近客觀真值，亦即盡可能多地衰減噪聲。

進一步講，我們用AD將模擬信號數字化之后，因為模擬信號本身包含了噪聲，即使AD沒有誤差，數字化之后的數字量也是含有噪聲的。

況且，不可避免的，還要考慮AD的誤差。我們把這種誤差就叫做測量誤差。

數字化之后，最簡單的，我們可以測100組數據，排序，刪掉前20個，刪掉后20個，剩下60個取均值。這樣會排除了一些偶然誤差。

kalman濾波器和上面的均值方法工作模式類似，只不過他的工作過程比較復雜，通過算法里面的五條公式過后，會很好的給出真值。

網上很多的是關于多維kalman實現。理解多維的比較費勁。

參照網上的一些代碼，實現了一個一維地濾波，對于有C語言基礎的同學來講，理解起來應該很容易了。

3，

百度百科里面有這個帖子：

http://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html

講解的是一維kalman濾波器，但是最后printf出來的都是分立的值，看不出來什么。

我參考那段代碼，改寫成了下面這段代碼，在labwindows里面繪制了一段曲線，效果就很直觀了：

/*-------------------------------------------------------------------------------------------------------------*/

void KalmanFilter(unsigned int ResrcDataCnt,const double *ResrcData,double *FilterOutput,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)

{

unsigned int i;

double R = MeasureNoise_R;

double Q = ProcessNiose_Q;

double x_last = *ResrcData;

double x_mid = x_last;

double x_now;

double p_last = InitialPrediction;

double p_mid ;

double p_now;

double kg;

for(i=0;i

{

x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)

p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪聲

kg=p_mid/(p_mid+R); //kg為kalman filter，R為噪聲

x_now=x_mid+kg*(*(ResrcData+i)-x_mid);//估計出的最優值

p_now=(1-kg)*p_mid;//最優值對應的covariance

*(FilterOutput + i)??= x_now;

p_last = p_now; //更新covariance值

x_last = x_now; //更新系統狀態值

}

}

/*-------------------------------------------------------------------------------------------------------------*/

上面是濾波器部分，然后，我模擬了一段AD的數據，輸給這個濾波器，然后看一下這個濾波器的響應，效果如下所示：

從圖上可以看出，真值為5，,1000組數據，每組隨機疊加10%的噪聲，可以觀察一下kalman的收斂速度和收斂精度。

收斂速度和收斂精度就是kalman的兩個重要衡量指標。

收斂速度和收斂精度是矛盾著的。

4，

參考上面的代碼，我優化了一下之后，運行在了STM32上面：

/*-------------------------------------------------------------------------------------------------------------*/

/*

Q:過程噪聲，Q增大，動態響應變快，收斂穩定性變壞

R:測量噪聲，R增大，動態響應變慢，收斂穩定性變好

*/

double KalmanFilter(const double ResrcData,

double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)

{

double R = MeasureNoise_R;

double Q = ProcessNiose_Q;

static? ? ? ? double x_last;

double x_mid = x_last;

double x_now;

static? ? ? ? double p_last;

double p_mid ;

double p_now;

double kg;

x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)

p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪聲

kg=p_mid/(p_mid+R); //kg為kalman filter，R為噪聲

x_now=x_mid+kg*(ResrcData-x_mid);//估計出的最優值

p_now=(1-kg)*p_mid;//最優值對應的covariance

p_last = p_now; //更新covariance值

x_last = x_now; //更新系統狀態值

return x_now;

}

/*-------------------------------------------------------------------------------------------------------------*/

5，

我是希望用加速度計測出加速度，然后加速度積分得出速度，然后速度積分得位移。

硬件用的freescale的MMA7361，模擬加速度計，STM32 片上AD，如下：洞洞板右上角為MMA7361

LCD上面曲線為加速度計曲線，下面那條線為速度曲線，第三條沒處理。

沒有kalman可以看到加速度那條線很粗：

用了kalman之后，效果如下：

6，

kalman需要根據具體的應用來調整濾波器參數的。

主要是：測量噪聲系數，系統噪聲系數，初始估計。這三個。

我問了一下老師，老師的意思是，這些參數很多時候是經驗值，沒有很好地系統的估計方法。

我個人感覺這是kalman理論發展的方向，估計很多人在研究這個吧。

7，

附件是一個labwindows 仿真的C代碼，一個STM32 的MDK工程代碼，正點原子的板子，和一些文檔。

感覺有個這些，再參考一下網上的資料，應用方面應該沒什么問題了。

至于理論本質，實在力不能及，路漫漫其修遠啊。

8，

聽了小伙伴的意思，換成數字MPU6000了。

吃飯啦，就這樣吧。希望能幫到大家。

我是學生黨，在學習，很多東西做的很粗糙，勿噴。

總結

以上是生活随笔為你收集整理的ad 卡尔曼_卡尔曼滤波算法C语言实现(转2)的全部內容，希望文章能夠幫你解決所遇到的問題。

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