一、问题与仿真
假设小车在一个方向上以 2cm/s2 的加速度运动了 100s，使用加速度计和GPS测量小车位置。GPS定位误差为高斯分布，方差为4m；加速度计的误差也为高斯分布，方差为0.01m/s2，并且由于加速度计放置不是完全水平的，有 0.03m/s2 的偏移。采用卡尔曼滤波，融合加速度计和GPS数据，估计小车的位置与速度。
如果只使用GPS数据估计，卡尔曼滤波器MATLAB实现（从一维到三维）一文给出了基本理论与测试效果。如果只使用加速度计数据，理论上系统是不可观的，也即不能稳定估计小车的速度，偏移误差将不断累积。
在小车运动中，已知加速度计数据和GPS定位数据，加速度计和GPS数据都有一定误差，如何融合数据，估算出小车的位置与速度？融合传感器数据真的能提高数据精确度吗？本文将作简单测试验证。
二、系统模型与卡尔曼滤波
2.1 系统模型
2.2 卡尔曼滤波公式
在此例子中，各变量取值如下
可见，在预测过程使用加速度计获取的数据，在滤波过程使用GPS数据。
三、仿真分析
位移结果如下，滤波后方差变为原来 1/10.
速度估计结果如下图。图中蓝色点为使用滤波后位移直接差分得到的结果，但是误差很大。其实，这样是不科学的，因为随着采样频率增加，单位时间内位移 dt 滤波的误差不会变化，但是 v=ds/dt，dt 随采样频率减小，误差会越大。而采用卡尔曼滤波的结果，误差较小。
四、实测数据与仿真
4.1 仿真分析
将大疆无人机带出去，获取加速度度与GPS数据，以及飞控计算的速度与位置。将使用自己写的卡尔曼滤波器计算位置与速度，与大疆的结果对比。
结果如下
可见，只使用加速度积分计算速度与位置，会随着时间不断漂移。使用加速度与GPS数据融合，能够很好地避免数据漂移。
从图中还可见，DJI的GPS并没有很大的误差，几乎在20cm以内，很可能将原始GPS数据滤波。
4.2 补充细节
-
从之前的原理及程序还可以得知，估计位置与速度，并不需要测量速度加入卡尔曼滤波（当然有的话更好），加速度与GPS位移一起就能估算出速度与位移。
-
注意此处的加速度，已经转换到地球坐标系下。直接使用加速度计获取的数据是不行的，加速度计是在机体坐标系下。转换关系为
a e = R a b a_e = R a_b ae=Rab
a b a_b ab 为机体系下加速度，即加速度计测量结果； R R R为机体系到地球系的旋转矩阵； a e a_e ae 为地球系下的加速度。 -
GPS获得经纬度，如果已知两点的经纬度分别为 ( α 1 , β 1 ) , ( α 2 , β 2 ) (\alpha_1, \beta_1), (\alpha_2, \beta_2) (α1,β1),(α2,β2)，采用东北天坐标系，计算其 x y xy xy 之间的位移分别为
d x = ( α 2 − α 1 ) ⋅ r ⋅ cos β 2 d y = ( β 2 − β 1 ) ⋅ r \begin{aligned} &d_x = (\alpha_2 - \alpha_1) \cdot r \cdot \cos\beta_2 \\ &d_y = (\beta_2 - \beta_1) \cdot r \end{aligned} dx=(α2−α1)⋅r⋅cosβ2dy=(β2−β1)⋅r
其中， r = 6378137 r=6378137 r=6378137 为地球半径。
代码与姿态估计参考链接：
791
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
