我试图弄清楚如何使用 GPS 测量来纠正由 SLAM 方法引入的漂移误差，我在固定时刻在欧几里得 3d 空间中有两个点集：
红色数据集由 GPS 引入，没有漂移误差，蓝色数据集基于 SLAM 算法，随时间漂移。
我们的想法是 SLAM 在短距离上准确但最终会漂移，而 GPS 在长距离上准确而在短距离上不准确。因此，我想弄清楚如何将 SLAM 数据与 GPS 融合，以使两种测量都具有最佳精度。至少如何解决这个问题？
【问题讨论】：
标签： gps kalman-filter sensor-fusion slam slam-algorithm
由于您的 GPS 看起来非常偏向本地，我假设它是低成本的并且不使用任何校正技术，例如它不是差分的。您可能知道，GPS 误差不是高斯误差。 this paper 中的人表明，模拟 GPS 噪声的一种好方法是 v+eps，其中 v 是一个局部恒定的“偏置”向量（它通常保持几米不变，然后或多或少平滑地变化或突然）和eps 是高斯噪声。
鉴于此信息，一种选择是使用基于卡尔曼的融合，例如您将 GPS 噪声和偏差添加到状态向量，并适当地定义您的转移方程并按照您的方式进行一个普通的EKF。注意，如果我们忽略卡尔曼的预测步骤，这大致相当于最小化形式的误差函数
measurement_constraints + some_weight * GPS_constraints
这为您提供了更直接的第二选择。例如，如果您的 SLAM 是可视的，您可以只使用平方重投影误差之和（即束调整误差）作为测量约束，并将 GPS 约束定义为||x- x_{gps}||，其中@ 987654329@ 是2d 或3d GPS 位置（您可能希望忽略使用低成本 GPS 的高度）。
如果您的 SLAM 是基于视觉和特征点的（您没有真正说明您使用的是哪种类型的 SLAM，所以我假设是最广泛使用的类型），那么与上述任何方法的融合都可能导致“内部损失” ”。你做了一个突然的、猛烈的修正，并增加了重投影误差。这意味着您在 SLAM 的跟踪中丢失了内点。所以你必须重新对点进行三角测量，等等。另外，请注意，尽管我在上面链接到的论文提供了 GPS 误差模型，但它不是一个非常准确的模型，并且假设 GPS 误差的分布是单峰的（EKF 所必需的）对我来说似乎有点冒险.
所以，我认为一个不错的选择是使用障碍项优化。基本上，这个想法是这样的：因为你真的不知道如何对 GPS 误差建模，假设你对本地的 SLAM 更有信心，并最小化一个函数S(x)，它可以捕获你的 SLAM 重建的质量。注意x_opt 是S 的最小化器。然后，与 GPS 数据融合，只要它不会恶化S(x_opt) 超过给定阈值。从数学上讲，您希望最小化
some_coef/(thresh - S(X)) + ||x-x_{gps}||
然后您将使用x_opt 初始化最小化。 S 的一个不错的选择是捆绑调整错误，因为通过不降低它，您可以防止内部损失。文献中还有其他S 的选择，但它们通常是为了减少计算时间并在准确性方面增加一点。
这与 EKF 不同，它没有很好的概率解释，但在实践中产生了非常好的结果（我也将它用于与 GPS 以外的其他事物融合，并且效果很好）。例如，您可以查看this excellent paper，它解释了如何彻底实现这一点，如何设置阈值等。
希望这会有所帮助。如果您在我的回答中发现不准确/错误，请随时告诉我。
【讨论】：