java - 在 OpenCV Java 中实现卡尔曼滤波器

Question

我正尝试在我的 OpenCV 程序中用 Java 实现卡尔曼滤波器。我是 OpenCV 和卡尔曼滤波器的新手。我在 C++ 中找到了一些示例(在 Java 中不多)，这是我目前所拥有的:

初始化:

    //create kalman filter  
    KalmanFilter kalman = new KalmanFilter(4,2,0,CvType.CV_32F);  
    //set transition matrix
    float[] tM = { 1, 0, 1, 0, 
            0, 1, 0, 1,
            0, 0, 1, 0,
            0, 0, 0, 1 } ;
    Mat transitionMatrix=new Mat(4,4,CvType.CV_32F,new Scalar(0));
    transitionMatrix.put(0,0,tM);
    kalman.set_transitionMatrix(transitionMatrix);
    //set init measurement
    Mat measurementMatrix = new Mat (2,1, CvType.CV_32F);
    measurementMatrix.setTo(new Scalar(0));
    kalman.set_measurementMatrix(measurementMatrix);

    //Set state matrix
    Mat statePre = new Mat(4,1, CvType.CV_32F);
    statePre.put(1, 1, 300);
    statePre.put(2, 1, 200);
    statePre.put(3, 1, 0);
    statePre.put(4, 1, 0);
    kalman.set_statePre(statePre);


    //Process noise Covariance matrix
    Mat processNoiseCov=Mat.eye(4,4,CvType.CV_32F);
    processNoiseCov=processNoiseCov.mul(processNoiseCov,1e-1);
    kalman.set_processNoiseCov(processNoiseCov);

    //Measurement noise Covariance matrix: reliability on our first measurement
    Mat measurementNoiseCov=Mat.eye(4,4,CvType.CV_32F);
    measurementNoiseCov=measurementNoiseCov.mul(measurementNoiseCov,1e-1);
    kalman.set_measurementNoiseCov(measurementNoiseCov);

    Mat id2=Mat.eye(4,4,CvType.CV_32F);
    id2=id2.mul(id2,0.1);
    kalman.set_errorCovPost(id2);

对于每个视频帧:

    prediction= kalman.predict();
    predictPt.x = prediction.get(1,1)[0];
    predictPt.y = prediction.get(2,1)[0];

...新测量..

   measurementMatrix.put(1, 1, center.x);
   measurementMatrix.put(2, 1, center.y);
   measPt.x=center.x;
   measPt.y=center.y;

   Mat estimated = kalman.correct(measurementMatrix);
   statePt.x=estimated.get(1, 1)[1];
   statePt.y= estimated.get(2, 1)[1];

问题是我得到了一个空预测，而且我没有看到得到它的原因。有人知道我的代码有什么问题吗？我真的很感激任何帮助!

谢谢!

Answer 1

您得到一个空预测，因为您既没有插入也没有访问正确的元素。

首先:

statePre.put(0, 1, 300); //statePre.put(1, 1, 300);
statePre.put(1, 1, 200); //statePre.put(2, 1, 200);
statePre.put(2, 1, 0); //statePre.put(3, 1, 0);
statePre.put(3, 1, 0); // statePre.put(4, 1, 0);

预测:

prediction= kalman.predict();
predictPt.x = prediction.get(0,0)[0]; //predictPt.x = prediction.get(1,1)[0];
predictPt.y = prediction.get(1,0)[0]; //predictPt.y = prediction.get(2,1)[0];

最后:

measurementMatrix.put(0, 0, center.x); // measurementMatrix.put(1, 1, center.x);
measurementMatrix.put(1, 0, center.y); //measurementMatrix.put(2, 1, center.y);
measPt.x=center.x;
measPt.y=center.y;

Mat estimated = kalman.correct(measurementMatrix);
statePt.x=estimated.get(0,0)[0];
statePt.y= estimated.get(1,0)[0];

还有，你的measurementMatrix应该是这样的

 Mat measurementMatrix = Mat.eye(2,4, CvType.CV_32F);