如何使用卡尔曼滤波器来预测介于两者之间的gps位置

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);

在循环中这样做我不完全理解预测,估计和测量的含义.

因为衡量是一个"真实"的价值,可以用一些公平来衡量.让我们说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值的第一个坐标的预测值:

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

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.方差写成"sigma平方",因为它是标准差的平方.我们在这里使用方差,因为两个随机方差之和的方差是它们的方差之和(简而言之:方差加)的良好性质.

因此,每个初始猜测的标准偏差是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.2pixels/DT(这可能是平滑的鼠标移动比在物理上是可能的),你说的是卡尔曼滤波器,你是内保持不变真正确保模型是正确的,它应该值得信赖.

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

我不会打破这个,除了指出:

1. 实际测量的鼠标位置的噪声协方差高于过程的噪声协方差.换句话说,您相信鼠标在直线(估计)线上移动的次数超过您认为的鼠标位置.
2. 您对实际鼠标位置准确性的信念并不像应有的那样强烈:您没有使用像GPS或相机跟踪这样的"模糊"的东西.您将获得实际的无噪音鼠标坐标.您的真实measureNoiseCovariance(通常R)为0.

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

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

现在解决您的具体问题:

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

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

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

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