有没有人能推荐轨迹闭合的算法啊。就是把下图的缺口闭合，使其更接近实际

场景描述：

我们有个室外的移动机器人，使用RTK+imu+odom进行定位，建图。在有RTK的情况下，定位效果还是很好的。在无RTK的情况下，依靠odom+imu的惯性导航算法。

具体问题：

如下图，在无RTK的情况下，odom+imu的推算轨迹，和实际轨迹有偏差，我们希望，在重新恢复RTK后，能够修正推算的轨迹，至少使整个轨迹是连续的。没有断开。想请教下有没有什么好的算法推荐下呢？

可获得的数据：

1、有RTK情况下的实际轨迹的位置（x,y） 2、推算轨迹的位置（x,y），包括在恢复RTK后，odom+imu的推算位置，也可以获得

注：直接将2个点连起来的方法不考虑

图片说明：

1 黑线为建图算法输出的机器人轨迹
2 有RTK时的机器人轨迹就是实际轨迹

可以迭代方式消除误差