组合导航的结果分段跳变问题

1 现象

用上海代数律动公司的AlgoT1-3组合导航设备采集数据进行组合导航算法调试，AlgoT1-3机器输出的结果很好很平滑，AlgoT1-3是带GNSS/INS的组合导航设备，另外还有一款更贵一点的带视觉的组合导航AlgoT1，效果会更好一些，产品型号可以在官网上查看(上海代数律动技术有限公司)。但是我用采集下来的原始数据自己处理，发现会出现结果分段跳变的现象。以AlgoT1-3的结果作为真值来进行对比，效果如下：

然后，对比了同一时刻的GGA结果和组合结果（由于定位结果不在同一个参考点，忽略整体偏移，仅仅关注连续性）：

从上面可以看到组合结果跟着GGA结果的有周期相关性。

2 解决思路

从现象看，因为组合结果的周期性跳动与GGA结果有相关性，判定是GNSS的结果加入带跳。

第一方面，对从上个基准递推过来的INS结果，和当前被GNSS校正的结果，进行进一步融合，而不是直接使用当前被GNSS校正后的结果，可以让结果更平滑一些；

第二方面，被GNSS校正的时候，硬的解算策略需要进行优化；

第三方面，在GNSS参与组合校正状态量后，进行反馈时，进行局部反馈，以让结果更平滑。

3改进后的效果

轨迹图对比：

序列图对比：

从效果上面看，有了比较明显的改善。