自己造轮子-基于Ceres的GNSS-INS松耦合组合导航算法

之前硕士期间的研究课题涉及到GNSS-INS组合导航，入门看的武汉大学牛老师团队的KF-GINS，不得不说，大组的东西还是很棒的，很适合组合导航入门，KF-GINS主要是基于ESKF的GNSS/INS松耦合组合导航系统，博主就不在这里对ESKF和松耦合进行过多的阐述了。本篇主要针对基于Ceres的松耦合组合导航系统的设计与实现，那么直接开造~

Ceres

Ceres这个工具对于slamer应该不算陌生，主要用来设计基于优化问题，那么对于松耦合的GNSS/INS组合导航系统，其实也可以参考基于ESKF的思路来设计基于Ceres的融合方案；

使用工具

本篇博客对于gnss和imu的数据主要使用的是开源方针算法“gnss-ins-sim”

1.ubuntu18.04
2.ros-melodic
3.gnss-ins-sim

融合方案设计

松耦合，那么也就是将ins解算得到的PVQ与gnss提供的PVQ进行融合，然后根据建立的运动学模型和观测来反向估计对应传感器的噪声，本篇博客使用的观测有gnss提供的速度以及位置信息，那么涉及到的融合就只有速度和位置的融合；

位置的融合

1.残差函数
$$error=P_g-P_i$$
下标i表示imu的递推解算得到的P，g表示由gnss提供的P观测；
2.残差的雅可比矩阵
$$J=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

速度的融合

1.残差函数
$$error=V_g-V_i$$
下标i表示imu的递推解算得到的P，g表示由gnss提供的P观测；
2.残差的雅可比矩阵
$$J=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

加速度计的零偏估计

这块其实与上面的速度融合相似，只是需要对残差函数做更进一步的细化：
对于ins的速度更新公式：
![在这里插入图片描述](https://i-blog.csdnimg.cn/direct/d7fe0d5fe51a49388afef7b936c3da2f.png
$$v_k=v_{k-1}+(\frac{R_{w{b}_k}{a}_k+R_{w{b}_{k-1}}{a}_{k-1}}{2}-g)(t_k-t_{k-1})$$
上述公式基于中值法做的速度递推

残差公式

$$error=V_g-V_k$$

雅可比矩阵

这里我没有手推过，就直接用的Ceres的自动求导了…

代码实现

我把代码提到我自己的github了，有兴趣的各位可以去看看，其实对于零偏的估计这块，我自己有点拿不准，但是最终估计出来的效果确实也接近我自己手动设置的加速度计的零偏，还望各位大佬能够不吝赐教，有错误的地方能够大方指点一下，我在这里谢过每一位读过我博客的各位了~
github仓库-选择master分支
如果代码能够帮到各位读者，还望能够star一下哟~

开源工具包使用参考

深蓝学院-多传感器融合定位-第六讲