更新

Question

我研究过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);

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

因为测量是一个真理＆＃34;值得用一些公平来衡量。让我们说GPS纬度和经度。我认为这个视频展示了一些有趣的想法。 https://www.youtube.com/watch?v=EQD0PH09Jvo。它使用的GPS测量单位在1hz时更新，然后使用卡尔曼滤波器以10 hz的速率预测值。

这样的设置怎么样？以下示例是如何做到的？

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测量结果预测所有eventdata时间戳的位置。因为使用opencv这样做的一个完整的例子会很好，但只有上述问题的起始指针，预测和正确也是可以接受的。

更新

我试一试。

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值的第一个坐标的预测值：

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

enter image description here

Answer 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。差异写成＆＃34; sigma平方＆＃34;因为它是标准差的平方。我们在这里使用方差，因为两个随机方差之和的方差是它们的方差之和（简而言之：方差加）的良好性质。

因此，每个初始猜测的标准偏差为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，这需要大约一分钟才能穿过典型的显示器。因此，您非常确信初始鼠标速度非常慢。

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

您可以非常轻松地改善鼠标速度的方差估计：只需记录一堆典型的鼠标移动并计算速度的方差（它是微不足道的：它只是平方的平均值与平均值的差异。在这种情况下，由于您在状态初始化时将平均值强制为0，因此强制对测量强制进行假设是有意义的，因此您只需要取平均值即可。你的样本期间的平方速度）。

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

这里你再次初始化了一个对角矩阵。让我们评估您的声明：忽略一段时间的实际鼠标位置您对系统的信念已被侵蚀。方差加（见上文），因此卡尔曼滤波器只需添加processNoiseCov（通常为Q）来表示侵蚀。因此在考虑到您因速度引起的鼠标移动知识后，您可以95％确定您的预测位置在+/-2 * sqrt(1e-2) = +/-0.2像素范围内。所以不知道用户做了什么（然而：我们还没有进行测量步骤）我们非常确定我们的恒速模型。并且由于同样的逻辑保持95％的确定速度在0.2pixels / dt内保持不变（这可能比物理上可能更平滑的鼠标运动）你告诉卡尔曼滤波器你真的确定模型是正确的，它应该被信任很多。

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

除了指出：

之外，我不打算将其打破

您实际测量的鼠标位置的噪声协方差高于此过程的噪声协方差。换句话说，您相信鼠标在直线（估计）线上移动的次数比您认为的鼠标位置要多。
您对实际鼠标位置的准确性的信念并不像它应该的那样强烈：您没有使用某些东西＆＃34;模糊＆＃34;像GPS或相机跟踪。您已获得实际无噪音鼠标坐标。您的真实measureNoiseCovariance（通常为R）为0。

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

因此，在状态估计方面，100％的测量结果听起来很无聊。但卡尔曼滤波器的一个神奇之处在于知道确切的位置将反向传播到其他项（在这种情况下，估计速度），因为过滤器知道你的transitionMatrix（又名F）所以它知道坏速度状态如何影响（现在已知的坏）位置状态。

现在解决您的具体问题：

这样的设置怎么样？以下示例是如何做到的？

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

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

您将在文献中发现，您的方法并不常见。这主要是因为在60和70年代对状态估计进行了大量的工作，当时快速运行滤波器的成本太高。相反，我们的想法是仅以慢速（在您的示例中为1 Hz）更新滤波器。这是通过在位置上使用错误的状态向量（以及在高动态系统中的速度和加速度）并且仅以慢速执行预测+更新步骤，同时连续使用预测的错误修正来完成的。以极快的速度。此表单称为间接卡尔曼滤波器。你还会发现一些论文认为这是古老的，货物崇拜的编程，它给直接方法带来了稍差的结果。即使是真的，也不会改变这样一个事实，即任何关于位置跟踪主题的谷歌搜索都很可能使用间接形式，这很难理解。

Answer 2

在进入j循环之前调用predict（）。因此，在循环中打印到myfile之前，您已完成2次predict（）调用。这会导致测量附近的蓝线发生扭结。我会在循环之前删除调用。

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

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

更新

2 个答案: