众所周知，一款好的imu对无人机姿态是多么的重要，曾经我们使用mpu6050、mpu9250等百元级的imu，这种imu在零偏、线性度、白噪声、温飘、轴间对齐、scale线性度等方面都显得力不从心。ardupilot、px4等固件不得不使用大量的低通滤波器来过滤噪声，达到了感觉上飞的还不错的程度。

但是在对无人机姿态要求比较高的情况下，特别是在工业应用中，显然像mpu6050这类IMU是无法满足我们要求的。这次我们就尝试把ADIS16448这款低成本高精度的IMU融合无人机上，当然，因为这款IMU在ardupilot固件中缺乏驱动无法直接使用，加之我手上刚好在做多传感器融合的方案，所以直接就用自己写的tbus-tiny_ekf来做这次的融合，大家如果有时间也可以尝试自己写驱动来测试看看。

---

ADIS16448简介
adis16448，是analog device公司面向市场的千元级低成本10自由度惯性导航产品，内置3轴陀螺仪、3轴加速度计、3轴磁力计、1个气压计。内置的所有传感器都有严格的工厂校准（到手就可以用啦-_-，再也不担心找不到重力矢量）

tips：气压计有点坑，暂时还是推荐使用ms5611比较靠谱

---

一、波形图对比

接下来我们通过波形对比来了解这款高精度IMU（本篇测试皆采用linux实时飞控板ALICE）：

<注：图中绿色为ADIS16448，红色为MPU6050>

静止状态

图一：加速度计白噪声对比

图二：陀螺仪白噪声对比

悬停状态

图三：加速度计白噪声对比

图四：陀螺仪白噪声对比

注：很抱歉，L君暂时无法解释为什么MPU6050 GYRO ACCEL均呈现类似正弦波的波形，也无法解释这种波形到底是好还是不好，不过看起来并非原始数据，有点像是经过了滑动窗口滤波以后呈现的波形，然而反观ADIS呈现出完美的正态分布，这正是EKF想要的数据！

综上图表我们可以看到：

1. 大量滤波下来，imu数据会延时，即本时刻imu数据不反应真实时刻位姿，而是历史状态。
2. 比较大的非线性噪声（非白噪声，由轴间误差、scale误差、帧间非线性误差引起）会对ekf造成比较大的估计偏差，有时甚至会引起ekf发散，再也无法估计状态。
3. 由于EKF都是在一个延迟的时间轴上运行的，所有时间轴向前推移都依赖于IMU的数据，如果IMU数据质量不好，将会引起很大的误差。
4. 为了应对这些问题，不得不对ekf打上茫茫多的patch，造成的代码难以理解和维护。

二、飞行测试视频

测试内容：adis16448 姿态模式飞行测试
测试环境：为了方便，就在室内测试了
测试飞控：linux实时飞控ALICE
测试目的：验证adis16448在tbus-tiny_ekf融合后的姿态实时性、稳定性
测试视频链接：https://www.bilibili.com/video/BV135411t7pm/

三、关于tbus-tiny_ekf

在这里给文中提到的tbus-tiny_ekf做一个解释，它是室内定位系统TSLAM2.0组件之一，主要是为了满足飞控融合多种传感器，比如惯导传感器、视觉、激光、里程计、RTK等多种设备。

tbus-tiny_ekf主要是为了满足这些需求而生：

• 重中之重：我们需要一份以人类能力能够看懂的滤波源码，需要对数学算法以及飞控逻辑做深度隔离。
• 我们需要的滤波器，能够尽可能方便的替换任何类型的传感器，而不应该陷入无穷无尽的函数嵌套。
• 我们的预测函数频率应该足够的高，而不能做：50hz的估计+400hz的控制。 滤波器应该尽量的短小精干，不做没必要的事情。
• 滤波器应该能及时的发现明显的传感器错误，在短时间内不受影响。
• 我们应该能自由操作开启滤波器的个数，方便对一些非常复杂的系统进行适配，比如rtk、视觉、激光、里程计等位置源的融合。

鉴于此，我们对ardupilot的卡尔曼滤波进行了重新打造，tbus-tiny_ekf便应运而生了~

tbus-tiny_ekf 19维状态变量

quaterion[4]//姿态

velocity[3]//速度

position[3]//位置

gyro_rate_bias[3]//陀螺仪零偏

accel_bias[3]//加速度计零偏

mag_bias[3]//磁力计零偏

tbus-tiny_ekf 6个更新函数

//水平速度更新

bool update_velocity_ne(float raw_velocity[2], float velocity_noise, float gate,float innovation[2], float varInnov[2], float watcher[2]);

//垂直速度更新

bool update_velocity_d(float raw_velocity, float velocity_noise, float gate,float& innovation, float& varInnov, float& watcher);

//水平位置更新

bool update_position_ne(float raw_position[2], float position_noise, float gate,float innovation[2], float varInnov[2], float watcher[2]);

//垂直位置更新

bool update_position_d(float raw_position, float position_noise, float gate,float& innovation, float& varInnov, float& watcher);

//偏航角更新

bool update_attitude_yaw(float raw_yaw, float yaw_noise, float gate,float& innovation, float& varInnov, float& watcher);

//磁力计更新

bool update_use_mag(float field[3], float mag_decl, float mag_noise, float gate,float& innovation, float& varInnov, float& watcher);

tbus-tiny_ekf 预测模型

悬停波形图

原理说完了，来做一下实际测试
以下是tbus-tiny_ekf的悬停波形图：

---

注：
注1：由于图实在是太多了，就放几个代表一下，有兴趣的可以自己做测试，这里只放上DOWN方向位置的滤波后波形，以及DOWN方向新息。
注2：新息越小代表IMU精度越高，最好一条直线全0。
注3：阶梯状是因为我拉了几次油门。

---

图五：过EKF后DOWN方向位置

图六：过EKF后DOWN方向新息

飞行对比测试视频

我们分别使用mpu6050和adis16448在定点模式下同时采用1.66hz的定位频率进行飞行测试。当然adis使用了全新的tbus-tiny_ekf，而mpu6050使用最新官方无删减无修改EKF。

注意：本次测试要求噪声必须是白噪声（不能是椒盐噪声），所以我们采用室内TSLAM激光定位，如果大家需要室外测试建议使用RTK进行定位。

1、mpu6050定点模式飞行测试视频（室内TSLAM激光定位）
https://www.bilibili.com/video/BV1zK4y1r7uW/

2、adis16448 定点模式飞行测试视频（室内TSLAM激光定位）
https://www.bilibili.com/video/BV1Ne41147MT/

可以看到ardupilot官方EKF+MPU6050就光定住就已经很难了，而tbus-tiny_ekf+adis16448不仅准确的定住了，甚至还可以慢速的走一走，完全没有发散的迹象！

FAQ

1.采用这么低的频率测试有什么意义吗？我们普通GPS也能达到5HZ频率呀？
L君:你应该想想高速飞行的场景，在高速场景下，IMU的数据质量就尤为重要了，一点点的积分误差都会导致估计的不准确。

2.为什么不采用高定位频率再测试呢？
L君：如果采用10HZ的室内满频率，肉眼实在无法看出MPU与ADIS有什么差别。并且，室内slam也是不允许你高速飞行的。

3.为什么一定是1.66HZ呢，其他频率测试不行吗？
L君：其实这个测试频率越低越容易说明问题，但是，如果低于1.66HZ ardupilot官方EKF+MPU6050飞起来那一瞬间，我就可以提控回家了。

4.这个测试说明了什么问题呢？
L君：这个测试说明了，adis完美的弥补了每两次定位信息之间的空白地带，至少在400ms之内的积分是准确无误的，而mpu会差很多，我估计最多只能弥补200ms的空白，并且出现的非线性波形会很大程度的影响最终的估计值。（注意：一个是最多，一个是至少）

---

OK，这次的测试就到这里，继续回去撸代码了…