Implementing Kalman filter in OpenCV Java

Question

I'm trying to implement a Kalman filter in my OpenCV program in Java. I'm new to both OpenCV and Kalman Filter. I've found some examples in C++ (not many in Java) and this is what I have so far:

Initialization:

    //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);

For each video frame:

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

...new measurement..

   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];

The problem is that I get a null prediction, and I don't see the reason for getting it. Does somebody know what's wrong with my code? I really appreciate any help!

Thank you!

raullalves raullalves · Accepted Answer · 2017-06-13T15:02:12

You get a null prediction because you are neither inserting nor accessing the correct elements.

First:

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);

Predict:

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];

And finally:

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];

Also, your measurementMatrix should be like this

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

Implementing Kalman filter in OpenCV Java

1 Answers