localization

介绍

​ 一个融合定位的项目。

2022.08.19 --这次做到就当练手了，后续会想写一个大的融合定位项目。

复习了一下 Quaternion kinematics for the error-state Kalman filter ，结合他人的代码，自己实现了一套非常简单的IMU+GPS的融合定位项目。

​ 这次的主要工作：

​ 1、重构。目前这套代码---包括前端主要是收集数据、交给后段处理数据，后段中包含三种model，分别完成初始化、预测、修正的工作。

​ 2、IMU的静态初始化。之前学习VINS，仅仅接触了动态的初始化、这次又学习了一下静态的初始化。具体包括利用向量的叉乘完成初始姿态的初始化 以及 利用施密特正交完成初始化。

​ 3、ESKF。一年前学习的ESKF，这次进行了复现。理论知识可参考 高博 。IMU的解算以及误差的传递就不过多介绍。

​ 大体流程为：

​ 1、预测。完成IMU的解算，由k-1时刻获得k时刻的状态两p,q,v。此外还要用Fk更新获得dx。

​ 2、修成。利用gps数据修正，修正的也是误差量dx，然后将dx添加入p、q、v、ba、bg中。

关于观测方程的推倒--初始化时，gps_buf中会存储一定数量的位置信息，最后求取平均值作为初始位置LLA，以后的每条gps数据都会以LLA为坐标原点，而状态量P初始化为0，所以需要推倒误差状态的观测方程。