无新观测的Opencv卡尔曼滤波预测

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

假设我有一个观察（一个观点）。每一帧我接收到新的观测，我称之为Kalman predict和Kalman correct。opencv卡尔曼滤波校正后的状态为“跟随该点”，即ok

然后，假设我有一个遗漏的观测值，我希望不管怎样，卡尔曼滤波器被更新并预测新的状态。这里我的代码失败了：如果我调用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;
}

#包括
#包括
#包括
#包括
#包括
使用名称空间cv；
使用名称空间std；
//------------------------------------------------简易测量方法
//用卡尔曼滤波法
//点对象
cv：：Kalman过滤器KF；
cv：：材料测量（2,1）；
物质状态（4，1）；//（x，y，Vx，Vy）
无效初始值（浮动x，浮动y）
{
//瞬时卡尔曼滤波
//4个动态参数和2个测量参数，
//我的测量是：物体的二维位置，
//动态是：二维位置和二维速度。
KF.init（4,2,0）；
测量=材料：零（2,1）；
在（0，0）=x时的测量值；
（0，0）处的测量值=y；
KF.statePre.seto（0）；
KF.statePre.at（0,0）=x；
KF.statePre.at（1,0）=y；
KF.statePost.seto（0）；
KF.statePost.at（0,0）=x；
KF.statePost.at（1,0）=y；
setIdentity（KF.transitionMatrix）；
setIdentity（KF.measurementMatrix）；
setIdentity（KF.processNoiseCov，Scalar:：all（.005））；//调整此值以加快收敛速度，但噪声更高
setIdentity（KF.measurementNoiseCov，Scalar:：all（1e-1））；
setIdentity（KF.errorCovPost，Scalar:：all（.1））；
}
卡尔曼点（）
{
Mat prediction=KF.predict（）；
点预测点（预测点在（0），预测点在（1））；
返回预测；
}
点kalmanCorrect（浮动x，浮动y）
{
测量值（0）=x；
测量（1）=y；
估计材料=KF.正确（测量）；
点状态点（在（0）处估计，在（1）处估计）；
返回状态；
}
//------------------------------------------------主要
int main（int argc，char*const argv[]
{
点s，p；
initKalman（0,0）；
p=kalmanPredict（）；
cout我没有设置转换和测量矩阵

我在这里找到了这些矩阵的标准状态空间值。
在每次预测之后，您应该将预测状态（statePre）复制到校正状态（statePost）中。这也应该对状态协方差（errorCovPre->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;
}

Point kalmanPredict（）
{
Mat prediction=KF.predict（）；
点预测点（预测点在（0），预测点在（1））；
KF.statePre.copyTo（KF.statePost）；
KF.errorCovPre.copyTo（KF.errorCovPost）；
返回预测；
}
适用于那些在使用OpenCV卡尔曼滤波方面仍有问题的人

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

修改

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

KF.transitionMatrix=*（Mat_U4，4）嗯，这就是原因。@Xisco在查看2.4版本及之后的源代码时，已经完成了将预测状态复制到后验状态的操作。请看，或者有可能在最初提出这个问题时，源代码没有这样做。我在这里添加它，作为与我遇到相同问题的任何人的指针。