Kalman filter for C language

先前有幾次在做A/D量測的時候,發現訊號內都會有不定時的雜訊干擾

且這些雜訊是會影響A/D的量測值,在硬體電路沒辦法變更的情況底下

我試著找出解決的辦法,後來發現Kalman filter可以幫助我解決這個問題

希望對於在做A/D量測上要處理雜訊問題的人可以有所幫助。

下面簡單的code是我參照->薛老師 玩玩機器人 內所寫的一篇

Matlab for NXT,經卡爾門濾波前後,陀螺儀數據觀測

內的程式將其改成C code使用。

另外附上kalman filter.c與kalman filter.xlsx兩個檔案

[download id="3"]

一個是C Code檔案,應該不用太多解釋,另外一個.xlsx是可以用來模擬結果的

kfilter

float kalmanFilter(float inData)
    {                        
    static float prevData=0;
    static float p=10, q=0.0001, r=0.05, kGain=0;

    //Kalman filter function start*******************************
    p = p+q;
    kGain = p/(p+r);

    inData = prevData+(kGain*(inData-prevData));

    p = (1-kGain)*p;

	prevData = inData;
    //Kalman filter function stop********************************

    return inData;
    }

補充資料

http://rexkingworld.blogspot.tw/2013/05/kalman-filter.html

8 thoughts on “Kalman filter for C language

  1. 版大好~
    謝謝分享~讓我對於學mcu少點碰撞~
    另外能不能請問我目前正在學使用mcu的濾波這方面
    如果有個實際電路輸入點,要求是1ms 3ms 5ms濾波 要如何透過版大的程式的哪個參數可以修改成我要的1ms或3ms或5ms濾波呢?

  2. Dear Kai,

    我不是很明白您的意思

    1.輸入的訊號是類比訊號還是數位訊號?
    2.1ms或3ms或5ms濾波還是1ms 3ms 5ms濾波同時存在?
    3.另外是否可請教訊號輸入輸出的關係?

  3. 版大~sorry

    剛學mcu和ic 撰寫 頭腦對於這方面的邏輯和知識都很模糊

    1.輸入訊號是類比
    2.輸入總共有16個pin 可以讓使用者自訂每個pin的濾波條件 所以可以讓使用者選pin1是1ms 或者3ms 或者5ms pin2設定成1ms或3ms 或是5ms
    3.現在我適用mcp23s17這個ic當作mcu的擴充pin 外部訊號從mcp23s17輸入,但這個ic接近來的訊號就是0or1了(類比)再透過spi介面回傳到mcu的spi

    所以網路上大部分都是將數位濾波後變成類比 但是如果一近來就是類比 要如何濾波呢?

    剛開始學習mcu 整個概念都沒建立完整,所以描述可能很不明確sorry~

    感謝版大

發表迴響

這個網站採用 Akismet 服務減少垃圾留言。進一步瞭解 Akismet 如何處理網站訪客的留言資料