如何使用卡尔曼滤波器预测测量之间的 gps 位置

Posted 2023-02-21

【中文标题】如何使用卡尔曼滤波器预测测量之间的 gps 位置

【英文标题】：How to use a Kalman Filter to predict gps positions in between meassurements

【发布时间】：2014-09-06 16:07:15

【问题描述】：

我研究了OpenCV卡尔曼滤波器的实现，做了一些基本的鼠标指针模拟，了解了基础。我似乎错过了在我的应用程序中使用它的一些关键点，希望这里有人可以提供一个小例子。

使用具有速度和位置的简单模型：

KF.statePre.at<float>(0) = mouse_info.x;
KF.statePre.at<float>(1) = mouse_info.y;
KF.statePre.at<float>(2) = 0;
KF.statePre.at<float>(3) = 0;
KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);

setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-2));
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(KF.errorCovPost, Scalar::all(.1));

我可以做一个预测

Mat prediction = KF.predict();

我可以改正

Mat estimated = KF.correct(measurement);

循环执行此操作我不完全理解预测、估计和测量的含义。

因为测量是用某些设备测量的“真实”值。让我们说 GPS 纬度和经度。 我认为这个视频展示了一些有趣的想法。 https://www.youtube.com/watch?v=EQD0PH09Jvo。它使用以 1hz 更新的 GPS 测量单元，然后使用卡尔曼滤波器以 10hz 的速率预测值。

这样的设置看起来如何？下面的例子是这样做的吗？

Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();

Mat estimated = KF.correct(measurement);
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat prediction = KF.predict();
Mat estimated = KF.correct(measurement);

我不确定是否可以预测 9 个值，然后作为第 10 个值提供测量值。

我有一些我想测试的记录数据。 记录的数据是文件中 1hz 的 gps 数据，其中每一行是：timestamp:lat:lng，并且在单独的文件中以 15hz 记录一系列事件：timestamp:eventdata。

我想使用卡尔曼滤波器来模拟数据收集并根据 1hz 测量值预测所有事件数据时间戳的位置。因为使用 opencv 执行此操作的完整示例会很好，但仅在上述问题上使用 predict 和 correct 的起始指针也是可以接受的。

更新

我试了一下。

int main()

    char * log = "C:/dev/sinnovations/opencv_examples/kalman_tracking/data/geosamples.txt";
    char * log1 = "C:/dev/sinnovations/opencv_examples/kalman_tracking/data/geosamples1.txt";

    ifstream myReadFile(log);
    ofstream myfile(log1);

    myfile.precision(15);

    KalmanFilter KF(4, 2, 0,CV_64F);
    Mat_<double> state(4, 1);
    Mat_<double> processNoise(4, 1, CV_64F);
    Mat_<double> measurement(2, 1); measurement.setTo(Scalar(0));

    KF.statePre.at<double>(0) = 0;
    KF.statePre.at<double>(1) = 0;
    KF.statePre.at<double>(2) = 0;
    KF.statePre.at<double>(3) = 0;

    KF.transitionMatrix = (Mat_<double>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); // Including velocity
    KF.processNoiseCov = (cv::Mat_<double>(4, 4) << 0.2, 0, 0.2, 0, 0, 0.2, 0, 0.2, 0, 0, 0.3, 0, 0, 0, 0, 0.3);

    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
    setIdentity(KF.errorCovPost, Scalar::all(.1));

    std::vector < cv::Point3d >  data;
    while (!myReadFile.eof())
    
        double  ms;
        double lat, lng, speed;
        myReadFile >> ms;
        myReadFile >> lat;
        myReadFile >> lng;
        myReadFile >> speed;
        data.push_back(cv::Point3d(ms, lat, lng));
    

    std::cout << data.size() << std::endl;
    for (int i = 1, ii = data.size(); i < ii; ++i)
    
        const cv::Point3d & last = data[i-1];
        const cv::Point3d & current = data[i];
        double steps = current.x - last.x;
        std::cout << "Time between Points:" << current.x - last.x << endl;
        std::cout << "Measurement:" << current << endl;

        measurement(0) = last.y;
        measurement(1) = last.z;
        
        Mat estimated = KF.correct(measurement);
        std::cout << "Estimated: " << estimated.t() << endl;
        Mat prediction = KF.predict();
        for (int j = 0; j < steps; j+=100)
            prediction = KF.predict();  
            myfile << (long)((last.x - data[0].x) + j) << " " << prediction.at<double>(0) << " " << prediction.at<double>(1) << endl;
               
        std::cout << "Prediction: " << prediction.t() << endl << endl;
    

    return 0;

但是缺少一些东西，因为结果可以在图片中看到。红圈是记录值，蓝线是 lat lng 值的第一个坐标的预测值：

我不认为我处理观察和预测值的时间值的方式是正确的。

【参考方案1】：

让我们首先检查一下您在模型中对世界的看法：

float dt = 1;
KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, dt, 0,
                                            0, 1, 0, dt,
                                            0, 0, 1, 0,
                                            0, 0, 0, 1);

在这里，我已经修改了您的转换矩阵，以更明确地说明您已经编写了一个固定的时间步长 dt = 1。换句话说，x_next = x + dt * x_vel。很多对卡尔曼滤波器的解释暗示F（转移矩阵的典型名称）是一个常数。在这种情况下，您需要在时间步长发生变化时随时更新 F 的时间相关条款。

setIdentity(KF.errorCovPost, Scalar::all(.1));

您已将状态向量的误差协方差初始化为I * 0.1。这意味着您每个初始猜测的方差都是 0.1。方差写成“sigma squared”，因为它是标准差的平方。我们在这里使用方差是因为两个随机方差之和的方差是它们的方差之和（简而言之：方差相加）。

因此，您每个初始猜测的标准差为sqrt(0.1) ~= 0.3。因此，在所有情况下，您都说 68% 的时间初始输入在 +/-0.3 单位内，95% 的时间在 +/-0.6 单位内。如果您对这种飞跃感到困惑，请研究 正态分布。

这些陈述合理吗？您确切地知道您的初始鼠标位置（我假设），因此 x 和 y 的初始误差协方差可能更接近 0。尽管 95% 确信您的初始位置在 +/-0.6 像素内非常接近。您还说鼠标以 +/-0.6 像素/dt 的速度移动。假设您的实际 dt 大约是 1/60 秒。这意味着您有 95% 的把握确信鼠标的移动速度低于 0.6*60 = 36 pixels/sec，后者需要大约一分钟才能穿过典型的显示器。所以你很有信心，鼠标的初始速度很慢。

为什么这些事情如此重要？为什么我花了这么多时间在他们身上？因为卡尔曼滤波器的唯一“魔力”是它会根据每个误差的比例对您的测量结果与您的预测状态进行加权。您的卡尔曼滤波器仅与您的误差估计一样好。

您可以非常轻松地改进鼠标速度的方差估计：只需记录一组典型的鼠标移动并计算速度的方差（这很简单：它只是与平均值的平方差的平均值。在这种情况下，由于您在状态初始化中将平均值强制为0，因此在您的测量中强制执行该假设是有意义的，因此您只需在采样期间取平方速度的平均值）。

setIdentity(KF.processNoiseCov, Scalar::all(1e-2));

这里你又初始化了一个对角矩阵。让我们来评估一下您的说法：在一段时间忽略实际鼠标位置后，您对系统的信念已被削弱。添加方差（见上文），因此卡尔曼滤波器只需添加processNoiseCov（通常为Q）来表示该侵蚀。所以在考虑到你对鼠标移动速度的了解之后，你有 95% 的把握你的预测位置在+/-2 * sqrt(1e-2) = +/-0.2 像素内。因此，对于用户所做的事情一无所知（但：我们还没有进入测量步骤），我们非常确定我们的恒速模型。并且由于相同的逻辑适用于 95% 确定速度在 0.2 像素/dt 内没有变化（这可能比物理上可能的更平滑的鼠标运动）你告诉卡尔曼滤波器你真的确定模型是对的，应该很受信任。

setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));

除了指出这一点之外，我不会分解它：

实际测量的鼠标位置的噪声协方差高于过程的噪声协方差。换句话说，您认为鼠标沿直线（估计）移动的次数多于您认为的鼠标位置。 您对自己的实际鼠标位置的准确性的信念没有达到应有的程度：您没有使用诸如 GPS 或相机跟踪之类的“模糊”功能。您将获得实际的无噪声鼠标坐标。您的真实测量NoiseCovariance（通常为R）为0。

现在设置R=0有什么影响？这意味着KF.correct(measurement) 步骤中位置的错误比例将 100% 归因于您的模型。因此K（卡尔曼增益）将进行 100% 的测量，并用新值替换您的 x、y 鼠标位置。不过，这基本上是正确的。如果您用 GPS 替换鼠标，那么您的 R != 0 并且您会将一些新状态与一些旧状态混合在一起。但是在您的实际系统中（除非您故意注入模拟噪声），您确切地知道您的实际鼠标位置。如果您添加模拟噪声，您可以将其精确方差放入 R 中，您将得到最佳状态估计。

因此，就状态估计而言，进行 100% 的测量听起来很无聊。但是卡尔曼滤波器的神奇之处之一是知道确切的 位置 将反向传播到其他项（在这种情况下，估计速度），因为滤波器知道您的 transitionMatrix（又名 @ 987654340@) 所以它知道坏的速度状态如何影响（现在已知的坏）位置状态。

现在解决您的具体问题：

这样的设置看起来如何？下面的例子是这样做的吗？

它似乎适用于该特定实现。通常，您在预测 + 正确（通常称为“更新”）循环中运行卡尔曼滤波器。最终的输出状态实际上是校正后的后验估计。在某些实现中，预测函数可能根本不会更新最终状态，这会破坏您的循环。

其他人已经指出了你的双重预测。

您会在文献中发现，您的方法并没有被很普遍地描述。这主要是因为在 60 年代和 70 年代，在状态估计方面做了大量工作，当时以快速运行过滤器的成本太高了。相反，这个想法是仅以较慢的速率（在您的示例中为 1 Hz）更新过滤器。这是通过在位置（以及在高动态系统中的速度和加速度）中使用 errors 的状态向量并仅以慢速执行预测 + 更新步骤，同时连续使用预测的误差校正来完成的以最快的速度。这种形式称为间接卡尔曼滤波器。您还会发现论文认为这是陈旧的、货物崇拜的编程，其结果略逊于 direct 方法。即使是真的，它也不会改变任何关于位置跟踪主题的谷歌搜索很可能使用间接形式，这种形式更难理解。

【参考方案2】：

在进入 j 的循环之前，您正在调用 predict()。因此，在循环中打印到 myfile 之前，您已经完成了 2 次 predict() 调用。这会导致测量值附近的蓝线出现扭结。我会在循环之前删除调用。

预测步骤有时称为过滤器的传播，它可以更清楚地说明实际发生的情况：及时向前移动过滤器。在您的情况下，过滤器每次传播 100 毫秒的 1 步。