用“卡尔曼滤波器”来处理“加速度计数据”（加速度，陀螺仪调试）

liuxuan163 2009-8-5 14:10 楼主

关键词：SCA3000，ADIS16255，加速度计、陀螺仪数据计算处理算法，卡尔曼滤波器、扩展卡尔曼滤波器。关键词：加速度计和陀螺仪驱动，SCA3000，ADIS16250，ADIS16255，温度、重力加速度、角速度计算方法，SPI总线，SCA3000 模式设置（Mode）、参数设置、读写,ADIS16255读写、参数设置,陀螺仪数据处理 SCA3000，ADIS16255，加速度计、陀螺仪数据计算处理算法、调试结果，航空模型飞行，无人驾驶飞机，三维数据处理，XYZ角度，水平垂直仪器装置。

最近专注于GPS定位的精度算法研究。众所周知GPS有一定的误差，如果GPS的误差在10米范围的话，远超过了车道的宽度，要想定位在车道上，确实是个难度。可以利用固定GPS基站的方式来校准精度，但还有一个问题，芯片提供的定位周期最快只有一秒。就是说，这一秒范围内的轨迹是空白的，当速度达到一定程度的时候，一秒的距离已经很远，甚至已经完成了一个快速小角度偏转动作。那么这样的话精度将会大大降低。

为了填补这一秒中的轨迹空白，于是我们增加了加速度计和陀螺仪芯片，利用两者数据推算轨迹，将轨迹周期提高到13毫秒。陀螺仪内置有DSP功能模块，精度相当的高。但加速度计在物体运动的时候，有很大的噪声。去掉这些噪声是必须的，我们选择了卡尔曼滤波器。经过滤波以后的，使得噪声有了很大的收敛。给数据的准确性提升了一个很大的台阶。

卡尔曼的5个基本公式：
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)

有关其卡尔曼原理这里不再解释，有兴趣的可以搜索来看看。

附图1：
发动机启动后0速度的的加速度波形（上部分和中间部分）：
X轴：红色 Y轴：绿色，上部分为原始的加速度计波形，中间部分绿色的为经过卡尔曼滤波后的波形。

附图2：运动和转弯时的波形（上部分和中间部分）：
X轴：红色 Y轴：绿色，上部分为原始的加速度计波形，中间部分绿色的为经过卡尔曼滤波后的波形。

在进行速度推算时，我们有一个要求，在直线运动情况下“推算速度”无限逼近GPS速度，在转弯时“推算速度”无限接近用加速度数据计算的速度值（相当于测量值）。也就是说我们需要一个系数，当加速度变化越大，其值就越大，反之就越小。于是我们设计了一个公式：
K * K = (A * A) / (A * A + Q * Q)
其中K就是系数因子，A是加数据数据值，Q是加速度数据在0加速运动时的噪声，从经验数据看，我们的芯片在0加速运动时的噪声范围是0～30之间，于是我们取Q值＝20；由于我们要求无限接近，所以在计算K之后，还应再加一个公式：
K = K + (1 - K) * K

我们可以看下图来解释这个公式：
附图2：速度推算系数因子的波形。
纵向K值，横向A(加速度)值
蓝色K值计算步骤为：第1步：√(A * A) / (A * A + Q * Q);
红色K值计算步骤为：第1步：√(A * A) / (A * A + Q * Q)，第2步：K = K + (1 - K) * K；
绿色K值计算步骤为：第1步：√(A * A) / (A * A + Q * Q)，第2步：K = K + (1 - K) * K；第3步：K = K + (1 - K) * K；
('√'是开平方)
黑色虚线：A＝20的界限

这个公式的波形接近于对数波形，从图中我们看出，红色K值波形是最接近我们要求的波形，因为其曲线在A>30以后接近1，就是是无限大，30之前是一个小于1的弧线，刚好符合加速度计的噪声范围要求。





陀螺仪：
由于SCA3000和ADIS16255的SPI时序有些差异，因此只能采取模拟SPI的方式进行通讯。本驱动，下载后可以根据MCU具体的引脚定义进行修改。16255需要进行设置以后，偏差小了很多。SCA3000基本上不需要进行参数设置。

附图1，陀螺数据处理后对比图片：
上部分（绿色）曲线为以陀螺仪数据变化量为纵向刻度，离中线最近的开始，依次为1次平均值滤波，2次平均值滤波，3次平均值滤波，4次平均值滤波。
中间部分为陀螺仪原值曲线，底部部分（红色）为多次平均值滤波后的陀螺仪数据值。
左转弯后再右转弯回到原来方向，不超过5度