卡尔曼滤波器 - 空预测点

Question

我正在尝试在 C++ 中使用 OpenCV 应用卡尔曼滤波器以过滤某些轨道。让它对我有用的第一步是用来自 Points2f 向量的过滤器预测点。

我的代码如下：

cv::KalmanFilter kalmanFilter(4,2,0, CV_32F); 
kalmanFilter.transitionMatrix = transitionMat;
for(int i = 0 ; i < oldTrackeables.size() ; i++)
    for(int j = 0 ; j < oldTrackeables[i].getTrack().size() ; j++)
              {
                  cv::Size msmtSize(2,1);
                  cv::Mat measurementMat(msmtSize, CV_32F);
                  measurementMat.setTo(cv::Scalar(0));
                  measurementMat.at<float>(0) = oldTrackeables[i].getTrack()[j].x;
                  measurementMat.at<float>(1) = oldTrackeables[i].getTrack()[j].y;

                  //Initialisation of the Kalman filter
                  kalmanFilter.statePre.at<float>(0) = (float) oldTrackeables[i].getTrack()[j].x;
                  kalmanFilter.statePre.at<float>(1) = (float) oldTrackeables[i].getTrack()[j].y;
                  kalmanFilter.statePre.at<float>(2) = (float) 2;
                  kalmanFilter.statePre.at<float>(3) = (float) 3;


                 cv::setIdentity(kalmanFilter.measurementMatrix);
                 cv::setIdentity(kalmanFilter.processNoiseCov, cv::Scalar::all(1e-4));
                 cv::setIdentity(kalmanFilter.measurementNoiseCov, cv::Scalar::all(.1));
                 cv::setIdentity(kalmanFilter.errorCovPost, cv::Scalar::all(.1));

                 //Prediction
                 cv::Mat prediction = kalmanFilter.predict();

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

                 cv::Point predictPt(prediction.at<float>(0), prediction.at<float>(1));
                 cv::Point Mc = oldTrackeables[i].getMassCenter();          

                 cv::circle(kalmat, predictPt, 16, cv::Scalar(0,255,0), 3, 2, 1);


                 std::cout<<"prediction : x = " << predictPt.x << " - y = " << predictPt.y <<std::endl;
                 std::cout<<"position captée : x = " << oldTrackeables[i].getTrack()[j].x << " - y = " << oldTrackeables[i].getTrack()[j].y << std::endl;
                 std::cout<<"size of frame : rows = " << frame.rows << " - width = " << frame.cols <<std::endl;
                 std::cout<<"size of kalmat : rows = " << kalmat.rows << " - width = " << kalmat.cols <<std::endl;
                 cv::imshow("kalmat", kalmat);

其中 oldTrackeables[i].getTrack()[j] 只是向量中的一些 Points2f。

跟踪正确，但卡尔曼滤波器没有给出 "correct" 预测值 - 例如，程序显示：预测：x = 0 - y = 0 - position captée : x = 138.29 - y = 161.078 (原点的位置).

我真的一直在寻找答案并尝试了许多不同的方法，但我找不到任何真正帮助我的东西......我找到的更接近的是这个：http://answers.opencv.org/question/24865/why-kalman-filter-keeps-returning-the-same-prediction/ 但是并没有帮我解决问题...

如果你们中的任何人的答案可以帮助我理解问题，我将不胜感激。谢谢。

Answer 1

我认为您缺少测量计算的校正阶段。

Answer 2

首先，我会将所有初始化内容移到循环之外，否则您将覆盖过滤器中的内部状态。同时将 statePre 更改为 statPost

  //Initialisation of the Kalman filter
  kalmanFilter.statePost.at<float>(0) = (float) 0;   
  kalmanFilter.statePost.at<float>(1) = (float) 0;
  kalmanFilter.statePost.at<float>(2) = (float) 2;
  kalmanFilter.statePost.at<float>(3) = (float) 3;

  cv::setIdentity(kalmanFilter.measurementMatrix);
  cv::setIdentity(kalmanFilter.processNoiseCov, cv::Scalar::all(1e-4));
  cv::setIdentity(kalmanFilter.measurementNoiseCov,cv::Scalar::all(.1));
  cv::setIdentity(kalmanFilter.errorCovPost, cv::Scalar::all(.1));

部分：

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

应该删除，因为这是在 predict 阶段内部完成的。

最后，正如@Mozfox 所说，correct 阶段不存在于您提供的循环代码中。添加：

kalmanFilter.predict(measurementMat);

卡尔曼滤波器 - 空预测点

Kalman filter - Null predicted point(s)

c++

opencv

kalman-filter