在Widora_NEO上应用卡尔曼滤波来获得可靠倾角

     在widora openwrt中应用卡尔曼滤波，目的是从加速度计ADXL345和陀螺仪L3G4200D采集的数据中提炼出相对真实可靠的倾角和角速度，通常这些原始数据里面包含有大量的噪音和毛刺，卡尔曼滤波可以有效滤除这些噪音，得到统计意义上的平均值（有义值）, 而其中的毛刺则通过在平滑滤波中设定最大增量值来剔除。

     主要的挑战是大量的矩阵计算，为此特意编写了一组矩阵计算函数，虽然比较粗拙，但是通过编写可以更好地理解卡尔曼滤波的计算原理。我拿网上这个例子来验证这些函数 http://blog.****.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

水平有限，错误在所难免，还望大神们不吝赐教!