Question

为了帮助人们理解我所问的问题，我选择完全改写它。我希望这可以解决它。

我正在以1秒的速率收集gps数据（纬度/经度）。理解这些数据可能不是100％准确，偶尔（1个或多个）数据点远远超出标记，最合适的方法是删除异常点以确定相当准确的过程和车速？这种车辆可以以每小时0到60英里的速度行驶，通常是直线行驶，但也可能会突然转弯（加权值？）。

我为这种困惑道歉，而且因为没有理解已经提出的建议而道歉。

Answer 1

标准算法adjacent_difference将产生一系列迭代器中每个元素之间的差异。因此，如果有5个元素，则会产生4个差异。

这些是我们将要使用的标准库：

#include <vector>
#include <iostream>
#include <iterator>
#include <algorithm>
#include <numeric>

我不知道你的GPS课程会是什么样子。我将假设它是一维的：

class Position
{
public:
    Position() :
    m_position(0)
    {
    }

    Position(int position) :
    m_position(position)
    {
    }

    Position operator-(const Position& other) const
    {
        return Position(m_position - other.m_position);
    }
    operator int() const
    {
        return m_position;
    }
private:
    int m_position;
};

Position abs_sum(const Position& lhs, const Position& rhs)
{
    return Position(abs(int(lhs)) + abs(int(rhs)));
}

把它放在一起：

int main()
{
    using namespace std; // for brevity - don't really do this in your code

    vector<Position> positions;
    positions.push_back(Position(13));
    positions.push_back(Position(23));
    positions.push_back(Position(17));
    positions.push_back(Position(19));

    vector<Position> displacements;

    adjacent_difference(positions.begin(), positions.end(),
                        back_inserter(displacements));

    cout << "Displacements: ";
    copy(displacements.begin(), displacements.end(),
         ostream_iterator<int>(cout, ", "));

    cout << endl;

    int distance = accumulate(displacements.begin(), displacements.end(),
                              0, abs_sum);
    cout << "Total: " << distance << endl;

    return 0;
}

输出：

Displacements: 13, 10, -6, 2, 
Total: 31

Answer 2

问题似乎是格格不入，因为您没有足够的数据。所以，你的GPS正在收集位置，这些基本上是一堆坐标。你问的是“他们是否正确”和“如何使其更加精确”。显然，我们需要更多数据来做到这一点。

在机器人技术中，典型的“另一个数据”是来自其他传感器的数据（例如IMU - 惯性测量单元，实际上是加速度计）或odometry（对电机的命令）。从这两者中，机器人知道它“直行”并且可以校正左/右的任何分支。或者，他们使用计算机视觉算法跟踪“地标”（例如树木或角落），这也提供了关于机器人运动的良好信息。你没有这些。

你拥有的是汽车的物理模型。你知道，在60英里/小时，汽车不可能转90度（这就是为什么问题看起来不适合你，因为你自然知道汽车应该如何表现）。这种约束并不像其他传感器信息那样好，但应该这样做。您可以使用非线性最小二乘法或卡尔曼滤波器。

我不是卡尔曼过滤器的忠实粉丝所以我不会告诉你如何实现它。

使用NLS，您可以将汽车的位置作为图形（不要与绘图混淆）。汽车的每个位置都是一个顶点。每两个相邻的顶点（对应于“前一个和当前位置”）通过“汽车运动定律”约束（边缘）链接。每个顶点也有GPS位置约束，这是一元边缘。

这种图由稀疏矩阵表示。它是雅可比矩阵（或Hessian矩阵），其中值对应于顶点相对于给定系统状态（所有顶点的位置）的约束的推导。在每个步骤中，矩阵大小增加，因为添加了下一个位置。为了保持解决方案的低复杂性，您可以删除旧位置，仅保留最后N个步骤。在每个步骤中，您需要评估物理约束（计算速度和旋转速率并查看它是否合理）并计算雅可比行列/ Hessians和误差向量（当前矢量位置与GPS定位/物理约束的差异向量）运动）。然后你解决这个系统（dx =雅可比行列/误差），产生向量dx，这是顶点位置的差异。你只需将它添加到顶点就可以了。这基本上是高斯 - 牛顿算法。

这不是一件容易实现的事情。有些库可以有效地解决这类图形问题，例如SLAM++，iSAM，GTSAM或g2o。麻烦的是，这些都不适合你开箱即用，因为那里没有实现物理合理性约束（GPS约束也没有，但那只是减法）。您必须实现自己的顶点/边缘类型。

我建议你使用更简单的东西。只需考虑GPS告诉你的差异，计算窗口中位数，看看最后一次测量是否偏离中位数。如果它太远（您将不得不进行实验并查看哪个阈值有效），请不要将该测量用于速度/过程计算（但仍保留用于计算中位数）。这应该是相当准确的，适合您的目的。

如果您将测量数据上传为文本文件，其中lat / lon / timestamp是您在块周围驱动的，我们可以看到编写代码来处理它。

卡尔曼，最小二乘，或

2 个答案: