IMU和GPS数据融合估计位置与速度（MATLAB实现）

简介假设小车在一个方向上以 2cm/s2 的加速度运动了 100s，使用加速度计和GPS测量小车位置。GPS定位误差为高斯分布，方差为4m；加速度计的误差也为高斯分布，方差为0.01m/s2，并且由于加速度计放置不是完全水平的，有 0.03m/s2 的偏移。采用卡尔曼滤波，融合加速度计和GPS数据，估计小车的位置与速度。1. 问题如果只使用GPS数据估计，卡尔曼滤波器MATLAB实现（从...

大强强小强强

20554人浏览 · 2020-04-27 22:08:23

大强强小强强 · 2020-04-27 22:08:23 发布

一、问题与仿真

假设小车在一个方向上以 2cm/s² 的加速度运动了 100s，使用加速度计和GPS测量小车位置。GPS定位误差为高斯分布，方差为4m；加速度计的误差也为高斯分布，方差为0.01m/s²，并且由于加速度计放置不是完全水平的，有 0.03m/s² 的偏移。采用卡尔曼滤波，融合加速度计和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_b$ 为机体系下加速度，即加速度计测量结果； $R$ 为机体系到地球系的旋转矩阵； $a_e$ 为地球系下的加速度。
GPS获得经纬度，如果已知两点的经纬度分别为 $(\alpha_1, \beta_1), (\alpha_2, \beta_2)$ ，采用东北天坐标系，计算其 $x y$ 之间的位移分别为
$\begin{aligned} &d_x = (\alpha_2 - \alpha_1) \cdot r \cdot \cos\beta_2 \\ &d_y = (\beta_2 - \beta_1) \cdot r \end{aligned}$
其中， $r = 6378137$ 为地球半径。