利用GPS测量校正砰击漂移误差

我试图找出如何使用GPS测量纠正SLAM方法引入的漂移误差，我在欧几里得3d空间中有两个点集，在固定的时间点上拍摄：

红色数据集由GPS引入，不包含漂移误差，而蓝色数据集基于SLAM算法，随时间漂移

---

这个想法是，SLAM在短距离上是准确的，但最终会漂移，而GPS在长距离上是准确的，在短距离上是不准确的。所以我想弄清楚如何将SLAM数据与GPS进行融合，以获得两种测量的最佳精度。至少该如何解决这个问题？

由于您的GPS看起来很有局部偏差，我假设它成本低，并且不使用任何校正技术，例如，它不是差分的。您可能知道，GPS误差不是高斯误差。中的家伙们展示了一种建模GPS噪声的好方法，即

v+eps

，其中

v

是一个局部恒定的“偏差”向量（通常在几米内是恒定的，然后或多或少地平滑或突然变化），而

eps

是高斯噪声

根据这些信息，一种选择是使用基于Kalman的融合，例如，将GPS噪声和偏差添加到状态向量中，并适当定义过渡方程，然后像使用普通EKF一样进行处理。请注意，如果我们忽略Kalman的预测步骤，这大致相当于最小化形式的误差函数

   measurement_constraints + some_weight * GPS_constraints 

这给了你一个更为艰难的第二选择。例如，如果您的SLAM是可视的，您可以使用重投影误差的平方和（即束调整误差）作为测量约束，并将您的GPS约束定义为

|x-x{GPS}|

，其中

x

是

2d

或

3d

GPS位置（您可能希望使用低成本GPS忽略海拔高度）

如果你的SLAM是基于视觉和特征点的（你没有说你使用的是哪种类型的SLAM，所以我假设是最广泛使用的类型），那么与上述任何一种方法的融合都可能导致“更大的损失”。你进行了一次突然、猛烈的修正，并增加了重投影误差。这意味着你在SLAM的跟踪中失去了内线。因此你必须重新对点进行三角测量，等等。另外，请注意，尽管我上面链接的论文给出了一个GPS误差模型，但它不是一个非常精确的模型，并且假设GPS误差的分布ROR是单峰的（EKF所必需的），对我来说似乎有点冒险

所以，我认为一个好的选择是使用障碍项优化。基本上，这个想法是：因为你不知道如何对GPS误差建模，假设你在本地SLAM中有更多的信心，并最小化函数

S（x）

捕获SLAM重建的质量。注意

x_opt

最小的

S

。然后，与GPS数据融合，只要它不会恶化

S（x_opt）

超过给定的阈值。从数学上讲，您希望最小化

some_coef/(thresh - S(X))  + ||x-x_{gps}||

您可以使用

x_opt

初始化最小化。对于

S

来说，一个很好的选择是束调整误差，因为通过不降低它，可以防止更大的损失。在Literature中还有

S

的其他选择，但它们通常意味着减少计算时间，并在准确性方面增加很少

与EKF不同，EKF没有很好的概率解释，但在实践中产生了非常好的结果（我也用它与GPS以外的其他东西进行了融合，效果很好）。例如，你可以看到它解释了如何彻底实现这一点，如何设置阈值，等等

---

希望这有帮助。如果你在我的答案中发现不准确/错误，请毫不犹豫地告诉我。

因为你的GPS看起来是非常局部偏差的，我假设它是低成本的，并且不使用任何校正技术，例如，它不是差分的。正如你可能知道的，GPS误差不是高斯分布的。中的人显示了一个好的结果GPS噪声建模方法为

v+eps

，其中

v

是一个局部恒定的“偏差”矢量（通常在几米内保持恒定，然后或多或少平稳或突然变化），而

eps

是高斯噪声

根据这些信息，一种选择是使用基于卡尔曼的融合，例如，将GPS噪声和偏差添加到状态向量，并适当定义过渡方程，然后像处理普通EKF一样进行。请注意，如果忽略卡尔曼的预测步骤，这大致相当于最小化形式

   measurement_constraints + some_weight * GPS_constraints 

这给了你一个更严格的第二个选择。例如，如果你的SLAM是可视的，你可以只使用重投影误差的平方和（即束调整误差）作为测量约束，并将你的GPS约束定义为

|x-x{GPS}||

其中

x

是

2d

或

3d

GPS位置（您可能希望使用低成本GPS忽略高度）

如果你的SLAM是基于视觉和特征点的（你没有说你使用的是哪种类型的SLAM，所以我假设是最广泛使用的类型），那么与上述任何一种方法的融合都可能导致“更大的损失”。你进行了一次突然、猛烈的修正，并增加了重投影误差。这意味着你在SLAM的跟踪中失去了内线。因此你必须重新对点进行三角测量，等等。另外，请注意，尽管我上面链接的论文给出了一个GPS误差模型，但它不是一个非常精确的模型，并且假设GPS误差的分布ROR是单峰的（EKF所必需的），对我来说似乎有点冒险

所以，我认为一个好的选择是使用障碍项优化。基本上，这个想法是：因为你不知道如何对GPS误差建模，假设你在SLA中有更多的信心