在Widora_NEO上应用卡尔曼滤波

目的是通过卡尔曼滤波,从加速度计ADXL345和陀螺仪L3G4200D采集的数据中提炼出相对真实可靠的倾角和角速度,通常这些原始数据里面包含有大量的噪音和毛刺,卡尔曼滤波可以有效滤除这些噪音,得到统计意义上的平均值(有义值), 而其中的毛刺则通过在平滑滤波中设定最大增量值来剔除。
由于对kalman滤波和openwrt系统的理解有限,因此以下分析中难免会有纰漏, 仅供有兴趣的DIY爱好者参考。

1。 卡尔曼滤波基本原理
关于卡尔曼滤波的基本原理,主要参考了网上牛人的这篇博文: http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mjx-eqn-gaussequiv 简明扼要,相对来讲比较容易理解。个人理解其实质就是在已知线性模型中,各状态量数据如果仅包含正态分别噪声,那么通过计算就可以最得到各状态量的最优估计值(有义值)。
借用一下博文中的计算流程图:
!

2。 矩阵运算
卡尔曼滤波的计算最后归结到五个公式,也即五个计算步骤:
(这里没有考虑控制矩阵Bk和控制向量uk)

  1. 状态先验: Yp = F*Y
  2. 方差先验: Pp = FPF’+Q
  3. 增益矩阵: K = PpH’inv(HPpH’+R)
  4. 更新状态(最优): Y = Yp+K*(Z-H*Yp)
  5. 更新方差(最优) : P = (I-K*H)*Pp
    这个五个公式按照上面图片中的流程不断循环计算,输入观测矩阵Z, 输出状态矩阵Y(x,p) 。

为了上面的矩阵计算,特意编写了一组矩阵计算函数,虽然比较粗拙,但是通过编写可以更好地理解卡尔曼滤波的计算原理。源码 https://github.com/widora/ctest/blob/master/L3G4200D/mathwork.h 和 mathwork.c

我拿网上这个例子来验证 http://blog.csdn.net/zhangwenyu111/article/details/17034813 ,结果一致; 将其修改成3x1的观测矩阵,结果也一致。

3。 传感器的连接
将L3G4200D连接到widora_neo的SPI接口,将ADXL345连接到I2C接口,它们的数据输出率(ODR)都设定在800HZ。数据通过轮询的方式进行读取,原始返回数据都是有符号的16位整数,通过乘以对应的系数转化为浮点数。 在它们的初始化函数中进行了必要的偏移检测。
!


!
由于数据不是通过中断的方式进行读取, 造成了采样周期的不固定, 我采用每次将上一次所计周期时长更新到F矩阵中的方法,这给卡尔曼滤波引入了额外的噪音或非线性因数。

4。 应用卡尔曼滤波获得倾角
原始的倾角数据是通过X和Z轴加速度比值的反正切计算获得,角速度值直接用L3G4200D读取的数据乘以对应的系数。 在应用卡尔曼滤波之前先通过了滑动平均数滤波(MAF),主要是剔除那些可恶的毛刺信号(如何产生的?)。
卡尔曼滤波器仅有2个参变量,直接把它写成了一个函数float_KalmanFilter(), 在程序中不断循环读取传感器的数据,计算倾角和角速度,并将他们传入卡尔曼滤波器,然后获得滤波后的结果,在如此反复循环中,卡尔曼滤波的神奇功能就出现了…
!


这里面关键是如何设置R矩阵和Q矩阵的具体数值!这个还需要摸索一段时间。
试验程序 https://github.com/widora/ctest/tree/master/widora_BiRoll