忽然感觉TBUS牛逼，真的是深钻了一些算法，真正解决了些问题，单靠IMU实现定位都做出来了，牛逼。
最新的他们好像是用中心差分卡尔曼滤波了，可以看到他们在状态估计上花了很大的力气。
转载自：https://mp.weixin.qq.com/s/m-oBzlFWMuzRkwJrM1J0wg
原创 凌拓智能TBUS TBUS社区 2020-04-09
新年新气象，疫情过去后tbus的第一弹
By L君
>>>速览目录
一、TSLAM（室内定位导航系统）全面升级，跨入了2.0的时代
二、ADIS16448低成本高精度imu已完美匹配ALICE飞控
三、tslam2.0组件----tbus-tiny_ekf 开源
四、未来展望
一、TSLAM（室内定位导航系统）全面升级，跨入了2.0的时代
回顾TSLAM介绍及应用场景请点击：
原创无人机室内无源定位方案——TSLAM
TSLAM多场景测试
1.加入了对位姿先验的支持，意味着你可以不单单使用激光测距来定位了，与其他slam一样，轮式里程计，视觉里程计，加速度计均可以作为slam匹配器的先验，让slam更精确，可以排除大量的匹配错误，让slam更平滑。（此点针对tslam sdk）
2.加入了对后验位姿的准确度判定，如果判定得到有明显的匹配错误，则丢弃当前帧定位与建图，明显减少了在场景特征稀少的区域出现位姿发散的问题。
3.增加了对向下激光测距的支持，得益于tslam sdk的时间轴对齐机制，保证了实时厘米级的高度估计。
4.增加了对tbus-tiny_ekf的支持（后文有详细介绍）。
二、ADIS16448低成本高精度imu已完美匹配ALICE飞控
ADIS16448简介
adis16448，是analog device公司面向市场的千元级低成本10自由度惯性导航产品，内置3轴陀螺仪、3轴加速度计、3轴磁力计、1个气压计。内置的所有传感器都有严格的工厂校准（到手就可以用啦-_-，再也不担心我找不到重力矢量）
tips：气压计有点坑，暂时还是推荐使用ms5611比较靠谱
众所周知，一款好的imu对无人机姿态是如此的重要，曾经我们使用mpu6050、mpu9250等百元级的imu，这种imu在零偏、线性度、白噪声、温飘、轴间对齐、scale线性度等方面都显得力不从心。ardupilot、px4等固件不得不使用大量的低通滤波器来过滤噪声，达到了感觉上飞的还不错的程度。
接下来我们通过波形对比来了解这款高精度IMU（本文测试皆采用linux实时飞控板——ALICE）：
<注：图中绿色为ADIS16448，红色为MPU6050>
1
静止状态
【图1】加速度计白噪声对比
【图2】陀螺仪白噪声对比
2
悬停状态
【图3】加速度计白噪声对比
【图4】陀螺仪白噪声对比
注：很抱歉，L君暂时无法解释为什么MPU6050 GYRO ACCEL均呈现类似正弦波的波形，也无法解释这种波形到底是好还是不好，不过看起来并非原始数据，有点像是经过了滑动窗口滤波以后呈现的波形，然而反观ADIS呈现出完美的正态分布，这正是EKF想要的数据！
综上图表我们可以看到：
1、大量滤波下来，imu数据会延时，即本时刻imu数据不反应真实时刻位姿，而是历史状态。
2、比较大的非线性噪声（非白噪声，由轴间误差、scale误差、帧间非线性误差引起）会对ekf造成比较大的估计偏差，有时甚至会引起ekf发散，再也无法估计状态。
3、由于EKF都是在一个延迟的时间轴上运行的，所有时间轴向前推移都依赖于IMU的数据，如果IMU数据质量不好，将会引起很大的误差。
4、为了应对这些问题，不得不对ekf打上茫茫多的patch，造成的代码难以理解和维护。
想看实际飞行效果？接下来给大家上机实测一下：
-adis16448 姿态模式飞行测试-
（表现姿态的稳定与实时）
如何获得此款IMU的开源套件？
大家可通过以下TB链接了解更多产品信息和购买本IMU套件：
复制后用浏览器打开：
https://item.taobao.com/item.htm?id=615281436269
购买后可以获得：
官方imu adis16448一个
alice_wrapper底板一个
alice_wrapper底板源代码一份
官方数据手册、alice_wrapper数据手册、alice_wrapper快速上手手册
alice飞控一个（注意，默认套件包含ALICE飞控，并做出厂适配，提供相应技术支持；若使用其它飞控，可能无法提供相应技术支持，不保证imu能直接使用。）
tbus-tiny_ekf c++源码一份
ardupilot-tbus-tiny_ekf源码一份
tbus-tiny_ekf matlab源码一份(此项可能会延迟一段时间，正在优化中)
三、tslam2.0组件----tbus-tiny_ekf 开源
从tbus决定深耕室内定位至今，已经一年了。在创业之初完全不敢想象会有这么多的企业院校研究所愿意支持我们，在此我们表示真诚的感谢。
在这一年的辛苦耕耘下，我们发现，ardupilot官方的navekf2(3)已经无法满足我们对定位的需求了，我们急需一款能满足以下要求的EKF：
1.重中之重：tbus需要一份以人类能力能够看懂的滤波源码，需要对数学算法以及飞控逻辑做深度隔离。
2.我们需要的滤波器，能够尽可能方便的替换任何类型的传感器，而不应该陷入无穷无尽的函数嵌套。
3.我们的预测函数频率应该足够的高，而不能做：50hz的估计+400hz的控制。
4.滤波器应该尽量的短小精干，不做没必要的事情。
5.滤波器应该能及时的发现明显的传感器错误，在短时间内不受影响。
6.我们应该能自由操作开启滤波器的个数，方便对一些非常复杂的系统进行适配，比如rtk、视觉、激光、里程计等位置源的融合。
鉴于此，我们对ardupilot的卡尔曼滤波进行了重新打造，tbus-tiny_ekf便应运而生了。
以下是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算法我们将对购买ALICE+adis16448的用户开源，并提供技术支持与以下资料：
如果您使用ardupilot进行无人机应用，我们为您提供了6个wrapper，分别为：
ublox+adis16448+板载气压计
rtk+板载imu
rtk+adis16448
tslam+板载imu+板载气压计
tslam+adis16448+板载气压计
tslam+adis16448+tfmini
如果您不想使用任何附加库制作机器人固件，我们专门为您提供了alice+tiny_ekf_+tslam的完整定位套件，或者选择alice+adis16448+tiny_ekf的精简套件（这一套精简套件是全开源的）。
欢迎扫描以下二维码联系我们了解更多：
话不多说，我们来实测一下：
以下是tbus-tiny_ekf的悬停波形图：
注1：由于图实在是太多了，就放几个代表一下，有兴趣的可以自己做测试，这里只放上DOWN方向位置的滤波后波形，以及DOWN方向新息。
注2：新息越小代表IMU精度越高，最好一条直线全0。
注3：阶梯状是因为我拉了几次油门。
【图5】过EKF后DOWN方向位置
【图6】过EKF后DOWN方向新息
下面，我们分别使用mpu6050和adis16448在定点模式下同时采用1.66hz的定位频率进行飞行测试。当然adis使用了全新的tbus-tiny_ekf，而mpu6050使用最新官方无删减无修改EKF。
mpu6050定点模式飞行测试
adis16448 定点模式飞行测试
可以看到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的空白，并且出现的非线性波形会很大程度的影响最终的估计值。（注意：一个是最多，一个是至少）
四、未来展望
到目前为止，tbus已经完成了全源定位大部分的底层代码构建，我们正在积极努力的打造一款不分室内外，不分时间，不分环境的准确的定位方案。不过这还需要长时间的打磨，希望在未来的某一天，能为在应用上为大家添砖加瓦！
