在widora openwrt中應用卡爾曼濾波，目的是從加速度計ADXL345和陀螺儀L3G4200D採集的數據中提煉出相對真實可靠的傾角和角速度，通常這些原始數據裏面包含有大量的噪音和毛刺，卡爾曼濾波可以有效濾除這些噪音，得到統計意義上的平均值（有義值）, 而其中的毛刺則通過在平滑濾波中設定最大增量值來剔除。
主要的挑戰是大量的矩陣計算，爲此特意編寫了一組矩陣計算函數，雖然比較粗拙，但是通過編寫可以更好地理解卡爾曼濾波的計算原理。我拿網上這個例子來驗證這些函數 http://blog.csdn.net/zhangwenyu111/article/details/17034813 ，結果一致； 將其修改成3x1的觀測矩陣，結果也一致。
另一個難點就是在Openwrt(非實時)系統中通過輪詢(而非中斷)來採樣傳感器的數據，這造成了採樣週期的不固定，給卡爾曼濾波引入了額外的噪音或非線性因數。 我採用每次將上一次所計週期時長更新到F矩陣中的方法。
原始的傾角數據是通過X和Z軸加速度比值的反正切計算獲得，角速度值直接用L3G4200D讀取的數據乘以對應的係數。 在應用卡爾曼濾波之前先通過了滑動平均數濾波(MAF)，主要是剔除那些可惡的毛刺信號（如何產生的？）。
卡爾曼濾波器僅有2個參變量，直接把它寫成了一個函數float_KalmanFilter(), 在程序中不斷循環讀取傳感器的數據，計算傾角和角速度，並將他們傳入卡爾曼濾波器，然後獲得濾波後的結果，在如此反覆循環中，卡爾曼濾波的神奇功能就出現了...
源碼: https://github.com/widora/ctest/blob/master/widora_BiRoll/wbiroll.c
水平有限，錯誤在所難免，還望大神們不吝賜教!