如何使用卡尔曼滤波器来预测测量间的GPS位置 [英] How to use a Kalman Filter to predict gps positions in between meassurements

问题描述

我研究了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））< 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预测= KF.predict（）; 
  

我可以改正

  Mat估计= KF.correct（测量）; 
  

在循环中这样做并不完全理解预测，估计和度量的含义。 / p>

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

这样的设置将如何显示？下面的例子是这样做的吗？

  Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 
 Mat估计= KF.correct（测量）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat预测= KF.predict（）; 
 Mat估计= KF.correct（测量）; 
  

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

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

我想使用卡尔曼滤波器来模拟数据收集，并基于1hz测量结果预测所有事件数据时间戳的位置。因为使用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>状态（4,1）; 
 Mat_< double> processNoise（4,1，CV_64F）; 
 Mat_< double>测量（2,1）; measurement.setTo（标量（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））< 0，0，0，1）; //包括速度
 KF.processNoiseCov =（cv :: Mat_< double>（4,4）<0.2,0,0.2,0,0,0.2,0,0,0,0,0,0,0,0 ，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>数据; 
 while（！myReadFile.eof（））
 {
 double ms; 
双lat，lng，speed; 
 myReadFile>>女士; 
 myReadFile>>土地增值税; 
 myReadFile>> LNG; 
 myReadFile>>速度; 
 data.push_back（cv :: Point3d（ms，lat，lng））; 
} 
 
 std :: cout<< data.size（）<<的std :: ENDL; 
 for（int i = 1，ii = data.size（）; i  {
 const cv :: Point3d& last = data [i-1]; 
 const cv :: Point3d&当前=数据[i]; 
 double steps = current.x  -  last.x; 
 std :: cout<< 点之间的时间：<< current.x  -  last.x<< ENDL; 
 std :: cout<< 测量：<<当前<< ENDL; 
 
 measurement（0）= last.y; 
 measurement（1）= last.z; 
 
 Mat估计= KF.correct（测量）; 
 std :: cout<< 估计：<<估计.t（）<< ENDL; 
 Mat预测= KF.predict（）; 
 for（int j = 0; j  prediction = KF.predict（）; 
 myfile<< （long）（（last.x_data [0] .x）+ j）<< << prediction.at< double>（0）<< << prediction.at< double>（1）<< ENDL; 
} 
 std :: cout<< 预测：<< prediction.t（）<< endl<< ENDL; 
} 
 
返回0; 
} 
  

但在图片中可以看到结果。红色圆圈是记录值，蓝色线是经度值的第一个坐标的预测值：

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

解决方案

让我们先来看看您在模型中对世界的看法：

  float dt = 1; 
 KF.transitionMatrix =（Mat_ （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。方差写为西格玛平方，因为它是标准偏差的平方。我们在这里使用方差，因为两个随机方差之和的方差是其方差之和（简称：方差添加）的好性质。

因此，每个初始猜测的标准差为 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像素/秒的速度移动，这需要大约一分钟的时间才能穿过典型的显示器。所以你非常有信心，最初的鼠标速度是非常缓慢的。

为什么这些事情非常重要？我为什么花这么多时间在他们身上？因为卡尔曼滤波器的唯一魔力就是它将根据每个误差的比例来衡量您预测的状态。您的卡尔曼滤波器的性能与您的误差估计值一样好。

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

  setIdentity（KF.processNoiseCov，Scalar :: all（1e-2））; 
  

这里再次初始化了一个对角矩阵。让我们来评估一下你的说法：在忽略了的实际鼠标位置之后，你对系统的信念就受到了侵蚀。 Variances add（参见上文），所以卡尔曼滤波器可以添加 processNoiseCov （通常 Q ）来表示侵蚀。所以在考虑到由于速度引起的鼠标移动的知识之后，95％确定你的预测位置在 +/- 2 * sqrt（1e-2） = +/- 0.2 像素。因此，不知道用户做了什么（然而：我们还没有进入测量阶段），我们非常确定我们的恒速模型。而且由于同样的逻辑认为95％的速度在0.2pixels / dt内保持不变（这可能是平滑的鼠标运动，而不是物理上的可能），你告诉卡尔曼滤波器，你确实真的

  setIdentity（KF.measurementNoiseCov，Scalar :: all（1e- 1））; 
  

我不打算将此分解，除非指出：

1. 实际测量的鼠标位置的噪声协方差高于过程的噪声协方差。换句话说，您相信鼠标在直线（估计）线中的移动超过了您相信所放置的鼠标位置。 您对您的实际的鼠标位置并不像它应该那样强壮：你不是在使用诸如GPS或照相机跟踪等模糊的东西。您得到了实际无噪声鼠标坐标。您的真实measurementNoiseCovariance（通常 R ）为0。

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

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

现在解决您的具体问题：

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

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

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

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

I have studied the OpenCV Kalman filter implementation and done some basic mouse pointer simulations and understand the basic. I seem to miss a few key points for using it in my application and was hoping someone here can provide a little example.

Using a simple model with velocity and position:

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

i can do a prediction

Mat prediction = KF.predict();

and i can do a correction

Mat estimated = KF.correct(measurement);

Doing this in a loop i dont fully understand what the meaning of prediction, estimate and measurement is.

Ofcause a measurement is a "truth" value meassured with some equitment. Lets say GPS latitude and longitude. I think this video shows some interesting ideas. https://www.youtube.com/watch?v=EQD0PH09Jvo. It is using a GPS measure unit that updates at 1hz and then it uses a kalman filter to predict the value at a 10 hz rate instead.

How would such a setup look? Is the following example the way to do it?

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

I am unsure if one can predict 9 values and then as the 10th provide a measurement.

I am some logged data that I would like to test with. The logged data is gps data at 1hz in a file where each row is : timestamp:lat:lng and seperatly a series of events are logged at 15hz in a seperate file : timestamp: eventdata.

I would like to use the kalman filter to simulate the data gathering and predict a position of all the eventdata timestamps based on the 1hz measurements. Ofcause a full example of doing this with opencv would be nice, but just the starting pointers on the questions above with predict and correct is also acceptable.

update

I gave it a try.

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

but there is missing something as the results can be seen in the picture. Red circles are logged value and blue line are predicted values for the first coordinate of the lat lng values:

I dont think the way I am handling the time values for observations and predicting values are correct.

解决方案

Let's first examine what you've said about the world in your model:

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

Here I've modified your transition matrix to be more explicit about the fact that you have coded a fixed timestep of dt = 1. In other words, x_next = x + dt * x_vel. A lot of explanations of the Kalman filter imply that F (the typical name for the transition matrix) is a constant. In this case you'd need to update the time-dependent terms of F any time your timestep changed.

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

You've initialized the error covariance of your state vector to I * 0.1. That means the variance of each of your initial guesses is 0.1. Variance is written as "sigma squared" because it's the square of the standard deviation. We use variance here because of the nice property that the variance of the sum of two random variances is the sum of their variances (in short: variances add).

So, the standard deviation of each of your initial guesses is sqrt(0.1) ~= 0.3. So in all cases you are saying that 68% of the time your initial inputs are within +/-0.3 units and 95% of the time within +/-0.6 units. If that leap is confusing to you, go study the normal distribution.

Are those reasonable statements? You know your initial mouse position exactly (I assume) so your initial error covariance for x and y are probably closer to 0. Although being 95% confident that your initial position is within +/-0.6 pixels is pretty close. You are also saying that the mouse is moving at +/-0.6 pixels/dt. Let's assume your actual dt was something like 1/60th of a second. That means you're 95% confident that the mouse is moving at a rate slower than 0.6*60 = 36 pixels/sec, which takes around a minute to cross a typical monitor. So you're very confident that the initial mouse speed is very slow.

Why do these things matter so much? Why did I spend so much time on them? Because the only "magic" of the Kalman filter is that it's going to weight your measurements against your predicted state based on the proportion of error in each. Your Kalman filter is only as good as your error estimates.

You could improve your variance estimate for the mouse velocity very easily: Just log a bunch of typical mouse movement and compute the variance of the velocities (it's trivial: it's just the average of the squared differences from the mean. In this case, since you are forcing the mean to 0 in your state initialization, it makes sense to force that assumption on your measurement, so you'd just be taking the average of the squared velocities over your sample period).

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

Here again you have initialized a diagonal matrix. Let's evaluate your claim: Having ignored the actual mouse position for a while your belief about the system has eroded. Variances add (see above) so the Kalman filter can just add in the processNoiseCov (usually Q) to represent that erosion. So after taking into account your knowledge of mouse movement due to velocity, you're 95% sure that your predicted position is within +/-2 * sqrt(1e-2) = +/-0.2 pixels. So not knowing anything about what the user has done (yet: we're not at the measurement step yet) we are pretty damn sure about our constant velocity model. And since the same logic holds about being 95% sure velocity is unchanged within 0.2pixels/dt (which might be smoother mouse motion than is physically possible) you are telling the Kalman filter that you are really sure that the model is right, and it should be trusted a lot.

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

I'm not going to break this down other than to point out that:

1. Your noise covariance for your actual measured mouse position is higher than your noise covariance for the process. In other words, you believe the mouse moves in a straight (estimated) line more than you believe the mouse positions you're putting in.
2. Your belief in the accuracy of your actual mouse positions is not as strong as it should be: You're not using something "fuzzy" like GPS or camera tracking. You're given the actual noise free mouse coordinate. Your true measurementNoiseCovariance (typically R) is 0.

Now what is the effect of setting R=0? It means that the proportion of error in the position at the KF.correct(measurement) step is going to be 100% attributed to your model. So K (the Kalman gain) will take 100% of the measurement and replace your x, y mouse position with the new value. That is essentially correct, though. If you replace mouse with GPS then your R != 0 and you will mix some new state with some old state. But in your actual system (unless you intentionally inject simulated noise) you know your actual mouse position exactly. If you add simulated noise you can put its exact variance into R and you will get optimal state estimation as promised.

So taking 100% of the measurement sounds pretty boring in terms of state estimation. But one of the magic bits of the Kalman filter is that knowing the exact position will back-propagate into the other terms (in this case, estimated velocity) because the filter knows your transitionMatrix (aka F) so it knows how the bad velocity state influenced the (now known bad) position state.

Now to address your specific question:

How would such a setup look? Is the following example the way to do it?

It appears to work for that particular implementation. Normally you run a Kalman filter in a loop of predict + correct (usually called "update"). The final output state is actually the posterior estimation from after the correction. In some implementations the predict function might not update the final state at all, which would break your loop.

Someone else already pointed out your double predict.

What you will find in the literature is that your approach isn't very commonly described. This is mainly because a huge amount of work was done on state estimation in the 60s and 70s, when the cost of running the filter at the fast rate was too much. Instead, the idea was to update the filter only at the slow rate (1 Hz in your example). This is done by using a state vector of errors in position (and velocity and acceleration, in high dynamic systems) and only doing the predict + update step at the slow rate, meanwhile using the predicted error corrections continuously at the fast rate. This form is called an Indirect Kalman Filter. You will also find papers arguing that this is archaic, cargo-cult programming which gives slightly inferior results to the direct approach. Even if true, it doesn't change the fact that any google search on the topic of position tracking is very likely to use the indirect form, which is harder to understand.

这篇关于如何使用卡尔曼滤波器来预测测量间的GPS位置的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！