假设小车在一个方向上以 2cm/s2 的加速度运动了 100s，使用加速度计和GPS测量小车位置。GPS定位误差为高斯分布，方差为4m；加速度计的误差也为高斯分布，方差为0.01m/s2，并且由于加速度计放置不是完全水平的，有 0.03m/s2 的偏移。采用卡尔曼滤波，融合加速度计和GPS数据，估计小车的位置与速度。
如果只使用GPS数据估计，卡尔曼滤波器MATLAB实现（从一维到三维）一文给出了基本理论与测试效果。如果只使用加速度计数据，理论上系统是不可观的，也即不能稳定估计小车的速度，偏移误差将不断累积。
在小车运动中，已知加速度计数据和GPS定位数据，加速度计和GPS数据都有一定误差，如何融合数据，估算出小车的位置与速度？融合传感器数据真的能提高数据精确度吗？本文将作简单测试验证。
在此例子中，各变量取值如下
可见，在预测过程使用加速度计获取的数据，在滤波过程使用GPS数据。
位移结果如下，滤波后方差变为原来 1/10.
速度估计结果如下图。图中蓝色点为使用滤波后位移直接差分得到的结果，但是误差很大。其实，这样是不科学的，因为随着采样频率增加，单位时间内位移 dt 滤波的误差不会变化，但是 v=ds/dt，dt 随采样频率减小，误差会越大。而采用卡尔曼滤波的结果，误差较小。
将大疆无人机带出去，获取加速度度与GPS数据，以及飞控计算的速度与位置。将使用自己写的卡尔曼滤波器计算位置与速度，与大疆的结果对比。
结果如下
可见，只使用加速度积分计算速度与位置，会随着时间不断漂移。使用加速度与GPS数据融合，能够很好地避免数据漂移。
从图中还可见，DJI的GPS并没有很大的误差，几乎在20cm以内，很可能将原始GPS数据滤波。
从之前的原理及程序还可以得知，估计位置与速度，并不需要测量速度加入卡尔曼滤波（当然有的话更好），加速度与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 为地球半径。
代码与姿态估计参考链接：
https://m.ke.qq.com/course/5343321?_bid=167&_wv=2147487745&term_id=105522580&taid=13615218132355161&from=share&tuin=5a4b8cb5#from=androidapp
