卡尔曼，最小二乘，或答案

【问题标题】：Kalman, least squares, or卡尔曼，最小二乘，或
【发布时间】：2012-04-10 18:15:29
【问题描述】：

为了帮助人们理解我的问题是我在问什么，我选择完全改写它。我希望这能解决问题。

我正在以 1 秒的速度收集 GPS 数据（纬度/经度）。了解这些数据可能不是 100% 准确，并且偶尔有一个（1 个或多个）数据点偏离标准，为了确定相当准确的路线，去除异常点的最合适方法是什么？车辆的速度？这辆车可以以每小时 0-60 英里的速度行驶，通常是直线，但也容易突然转弯（加权值？）。

对于造成的混乱，以及未能理解已经提出的建议，我深表歉意。

【问题讨论】：

“最佳”在哪方面？您是否需要良好的精度/小噪音、小偏差或混叠、快速响应……？它的计算成本有多高？
最适合我的输入和期望的输出。
我不希望它的计算成本很高。至于精度，我很难说。它不必过于精确。即，速度应该精确到几英里每小时，而路线精确到 1 度。除此之外，我真的不知道。这对我来说显然是未知的领域。
您测量的是人的步行速度、高速公路上的汽车还是航天飞机的速度？
更像是高速公路上的汽车。

标签： c++ gps least-squares kalman-filter

【解决方案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

【讨论】：

根据我对问题的重新措辞，你觉得这仍然适用吗？
@Jason 我认为您要求的是完全不同的东西。对不起！

【解决方案2】：

由于您没有足够的数据，因此该问题似乎格式不正确。所以，你的 GPS 正在收集位置，这些基本上是一堆坐标。你在问“它们是否正确”以及“如何使它更精确”。显然，我们需要更多数据才能做到这一点。

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

您所拥有的是汽车的物理模型。你知道，在 60 MPH 时，汽车不可能进行 90 度转弯（这就是为什么这个问题对你来说似乎并不适合，因为你自然知道汽车应该如何表现）。这种约束不如额外的传感器信息好，但它应该这样做。您可以使用非线性最小二乘法或卡尔曼滤波器等。

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

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

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

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

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

如果您将测量数据作为文本文件上传，并带有您在街区周围行驶的纬度/经度/时间戳，我们可以了解如何编写代码来处理它。

【讨论】：