Question

我想使用Opencv卡尔曼滤波器实现平滑一些噪声点。所以我试图为它编写一个简单的测试。

我们说我有观察（一点）。我接收新观察的每一帧，我称卡尔曼预测和卡尔曼正确。 opencv卡尔曼滤波器正确后的状态是＆＃34;跟随点＃34;，没关系。

然后让我说我有一个缺失的观察，无论如何我想要更新卡尔曼滤波器并预测新状态。这里我的代码失败了：如果我调用kalman.predict（），则值不再更新。

这是我的代码：

#include <iostream>
#include <vector>
#include <sys/time.h>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>

using namespace cv;
using namespace std;

//------------------------------------------------ convenience method for 
//                                                 using kalman filter with 
//                                                 Point objects
cv::KalmanFilter KF;
cv::Mat_<float> measurement(2,1); 
Mat_<float> state(4, 1); // (x, y, Vx, Vy)

void initKalman(float x, float y)
{
    // Instantate Kalman Filter with
    // 4 dynamic parameters and 2 measurement parameters,
    // where my measurement is: 2D location of object,
    // and dynamic is: 2D location and 2D velocity.
    KF.init(4, 2, 0);

    measurement = Mat_<float>::zeros(2,1);
    measurement.at<float>(0, 0) = x;
    measurement.at<float>(0, 0) = y;


    KF.statePre.setTo(0);
    KF.statePre.at<float>(0, 0) = x;
    KF.statePre.at<float>(1, 0) = y;

    KF.statePost.setTo(0);
    KF.statePost.at<float>(0, 0) = x;
    KF.statePost.at<float>(1, 0) = y; 

    setIdentity(KF.transitionMatrix);
    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov, Scalar::all(.005)); //adjust this for faster convergence - but higher noise
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
    setIdentity(KF.errorCovPost, Scalar::all(.1));
}

Point kalmanPredict() 
{
    Mat prediction = KF.predict();
    Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
    return predictPt;
}

Point kalmanCorrect(float x, float y)
{
    measurement(0) = x;
    measurement(1) = y;
    Mat estimated = KF.correct(measurement);
    Point statePt(estimated.at<float>(0),estimated.at<float>(1));
    return statePt;
}

//------------------------------------------------ main

int main (int argc, char * const argv[]) 
{
    Point s, p;

    initKalman(0, 0);

    p = kalmanPredict();
    cout << "kalman prediction: " << p.x << " " << p.y << endl;
    /* 
     * output is: kalman prediction: 0 0
     *
     * note 1:
     *         ok, the initial value, not yet new observations
     */

    s = kalmanCorrect(10, 10);
    cout << "kalman corrected state: " << s.x << " " << s.y << endl;
    /* 
     * output is: kalman corrected state: 5 5
     *
     * note 2:
     *         ok, kalman filter is smoothing the noisy observation and 
     *         slowly "following the point"
     *         .. how faster the kalman filter follow the point is 
     *            processNoiseCov parameter
     */

    p = kalmanPredict();
    cout << "kalman prediction: " << p.x << " " << p.y << endl;
    /* 
     * output is: kalman prediction: 5 5
     *
     * note 3:
     *         mhmmm, same as the last correction, probabilly there are so few data that
     *         the filter is not predicting anything..
     */

    s = kalmanCorrect(20, 20);
    cout << "kalman corrected state: " << s.x << " " << s.y << endl;
    /* 
     * output is: kalman corrected state: 10 10
     *
     * note 3:
     *         ok, same as note 2
     */

    p = kalmanPredict();
    cout << "kalman prediction: " << p.x << " " << p.y << endl;
    s = kalmanCorrect(30, 30);
    cout << "kalman corrected state: " << s.x << " " << s.y << endl;
    /* 
     * output is: kalman prediction: 10 10
     *            kalman corrected state: 16 16
     *
     * note 4:
     *         ok, same as note 2 and 3
     */    


    /*
     * now let's say I don't received observation for few frames,
     * I want anyway to update the kalman filter to predict 
     * the future states of my system
     *
     */
    for(int i=0; i<5; i++) {
        p = kalmanPredict();
        cout << "kalman prediction: " << p.x << " " << p.y << endl;
    }
    /* 
     * output is: kalman prediction: 16 16
     * kalman prediction: 16 16
     * kalman prediction: 16 16
     * kalman prediction: 16 16
     * kalman prediction: 16 16
     *
     * !!! kalman filter is still on 16, 16..
     *     no future prediction here..
     *     I'm exprecting the point to go further..
     *     why???
     *
     */    

    return 0;
}

我认为这段代码非常能说明我不理解的内容。我试图关注some theory和一些practical example，但仍然不知道如何对未来的位置进行新的预测..

任何人都可以帮助我理解我做错了什么？

Answer 1

对于那些在使用OpenCV卡尔曼过滤方面仍有问题的人

以上发布的代码经过小幅修改后效果很好。您可以设置如下，而不是将转换矩阵设置为Identity。

<强>修改

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

输出

enter image description here

Answer 2

我没有设置过渡和测量矩阵。

我在excellent MATLAB documentation page中找到了这些矩阵的标准状态空间值。

Answer 3

在每次预测之后，您应该将预测状态（statePre）复制到更正状态（statePost）。这也应该用于状态协方差（errorCovPre - ＆gt; errorCovPost）。这可以防止过滤器在未执行更正时陷入状态。原因是predict（）使用statePost中存储的状态值，如果没有调用更正则不会改变。

您的kalmanPredict函数将如下：

Point kalmanPredict() 
{
    Mat prediction = KF.predict();
    Point predictPt(prediction.at<float>(0),prediction.at<float>(1));

    KF.statePre.copyTo(KF.statePost);
    KF.errorCovPre.copyTo(KF.errorCovPost);

    return predictPt;
}

没有新观察的Opencv卡尔曼滤波器预测

3 个答案: