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

问题描述

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

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

KF.statePre.at(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_(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));

我可以做一个预测

垫预测 = KF.predict();

我可以改正

Mat 估计 = KF.correct(measurement);

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

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

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

垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫估计 = KF.正确(测量)；垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫预测 = KF.predict();垫估计 = KF.正确(测量)；

我不确定是否可以预测 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);卡尔曼滤波器 KF(4, 2, 0,CV_64F);垫子_<双>状态(4, 1)；垫子_<双>processNoise(4, 1, CV_64F);垫子_<双>测量(2, 1);测量.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) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);//包括速度KF.processNoiseCov = (cv::Mat_(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));标准::向量 <cv::Point3d >数据;而(！myReadFile.eof()){双毫秒；双纬度、液化天然气、速度；我的读取文件>>多发性硬化症;我的读取文件>>纬度；我的读取文件>>液化天然气；我的读取文件>>速度;data.push_back(cv::Point3d(ms, lat, lng));}std::cout <<数据.size() <<标准::endl;for (int i = 1, ii = data.size(); i < ii; ++i){const cv::Point3d &最后=数据[i-1]；const cv::Point3d &当前=数据[i]；双步 = current.x - last.x;std::cout <<点之间的时间:"<<当前.x - 最后.x <<结束；std::cout <<测量:"<<当前<<结束；测量(0)=last.y；测量(1)=last.z；垫估计 = KF.正确(测量)；std::cout <<估计:"<<估计.t() <<结束；垫预测 = KF.predict();for (int j = 0; j <步长; j+=100){预测 = KF.predict();我的文件 <<(long)((last.x - data[0].x) + j) <<""<

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

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

解决方案

让我们先来看看你在你的模型中所说的世界:

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.方差写成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 像素/秒，这需要大约一分钟才能穿过典型的显示器.所以你很有信心，鼠标的初始速度很慢.

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

您可以非常轻松地改进鼠标速度的方差估计:只需记录一组典型的鼠标移动并计算速度的方差(这很简单:它只是与平均值的平方差的平均值.在这种情况下，由于您在状态初始化中强制平均值为 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));

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

1. 实际测量的鼠标位置的噪声协方差高于过程的噪声协方差.换句话说，您认为鼠标沿直线(估计)移动的次数多于您认为的鼠标位置.
2. 您对自己的实际鼠标位置的准确性并不像应有的那么坚定:您没有使用模糊"的东西，例如 GPS 或相机跟踪.您将获得实际的无噪声鼠标坐标.你真正的measurementNoiseCovariance(通常是R)是0.

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

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

现在解决您的具体问题:

<块引用>

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

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

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

您会在文献中发现，您的方法并没有被很普遍地描述.这主要是因为在 60 年代和 70 年代在状态估计方面做了大量工作，当时以快速运行过滤器的成本太高了.相反，这个想法是仅以较慢的速率(在您的示例中为 1 Hz)更新过滤器.这是通过在位置(以及在高动态系统中的速度和加速度)中使用 errors 的状态向量并仅以慢速执行预测 + 更新步骤来完成的，同时连续使用预测的误差校正以最快的速度.这种形式称为间接卡尔曼滤波器.您还会发现论文认为这是陈旧的、货物崇拜的编程，其结果略逊于 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屋！