Need help in resolving error in Kalman filter implementation in Java using opencv

Question

I am implementing a Kalman filter using opencv's Kalman filter implementation for a movement data in 3D (X,Y,Z) coordinates. The model uses acceleration and velocity model for

s = s(0) + v*t + 0.5*a*t^2 

The below code is throwing error at

kalman.correct(measurementMatrix);

E/org.opencv.video: video::correct_10() caught cv::Exception: /build/master_pack-android/opencv/modules/core/src/matmul.cpp:1588: error: (-215) (((flags&GEMM_3_T) == 0 && C.rows == d_size.height && C.cols == d_size.width) || ((flags&GEMM_3_T) != 0 && C.rows == d_size.width && C.cols == d_size.height)) in function void cv::gemm(cv::InputArray, cv::InputArray, double, cv::InputArray, double, cv::OutputArray, int)

Can someone look at the issue mentioned?

public class MovementDirection {

    // Kalman Filter
    private int stateSize = 9; // x_old, v_x, a_x, y_old, v_y, a_y, z_old, v_z, a_z
    private int measSize = 3;  // x_new, y_new, z_new
    private int contrSize = 0;

    private KalmanFilter kalman = new KalmanFilter(stateSize, measSize,contrSize, CV_32F);

    MovementDirection(int depth, int lastXPositionPx, int lastYPositionPx){


        lastDepthCM = zVal;
        lastXPositionCM = xVal; 
        lastYPositionCM = yVal;

        //      1,dT,0.5*(dt*dt),   0,0,0,              0,0,0,
        //      0,1,dT,             0,0,0,              0,0,0,
        //      0,0,1,              0,0,0,              0,0,0,
        //
        //      0,0,0,              1,dT,0.5*(dt*dt),   0,0,0,
        //      0,0,0,              0,1,dT,             0,0,0,
        //      0,0,0,              0,0,1,              0,0,0,
        //
        //      0,0,0,              0,0,0,              1,dT,0.5*(dt*dt),
        //      0,0,0,              0,0,0,              0,1,dT,
        //      0,0,0,              0,0,0,              0,0,1,

        kalman.set_transitionMatrix(Mat.eye(stateSize,stateSize,CV_32F));

        //Set state matrix
        Mat statePre = new Mat(stateSize,1, CV_32F);
        statePre.put(0,0,lastXPositionCM); //x 0.05 CM/millisecond
        statePre.put(3,0,lastYPositionCM); //y 0.05 CM/millisecond
        statePre.put(6,0,lastDepthCM); //z 0.05 CM/millisecond
        kalman.set_statePre(statePre);

        //set init measurement
        Mat measurementMatrix = Mat.eye(measSize,stateSize, CV_32F);
        kalman.set_measurementMatrix(measurementMatrix);

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

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

        Mat errorCovPost = Mat.eye(stateSize,stateSize,CV_32F);
        errorCovPost = errorCovPost.mul(errorCovPost,0.1);
        kalman.set_errorCovPost(errorCovPost);

        Mat statePost = new Mat(stateSize,1, CV_32F);
        statePost.put(0,0,lastXPositionCM); //x 0.05 CM/millisecond
        statePost.put(1,0,lastYPositionCM); //y 0.05 CM/millisecond
        statePost.put(2,0,lastDepthCM); //z 0.05 CM/millisecond
        kalman.set_statePost(statePost);
    }


    public double[] predictDistance(long lastDetectionTime, long currentTime){
        double[] distanceArray = new double[3];
        long timeDiffMilliseconds =  Math.abs(currentTime - lastDetectionTime);

        Mat transitionMatrix = Mat.eye(stateSize,stateSize,CV_32F);
        transitionMatrix.put(0,1,timeDiffMilliseconds);
        transitionMatrix.put(0,2,0.5*timeDiffMilliseconds*timeDiffMilliseconds);
        transitionMatrix.put(1,2,timeDiffMilliseconds);

        transitionMatrix.put(3,4,timeDiffMilliseconds);
        transitionMatrix.put(3,5,0.5*timeDiffMilliseconds*timeDiffMilliseconds);
        transitionMatrix.put(4,5,timeDiffMilliseconds);

        transitionMatrix.put(6,7,timeDiffMilliseconds);
        transitionMatrix.put(6,8,0.5*timeDiffMilliseconds*timeDiffMilliseconds);
        transitionMatrix.put(7,8,timeDiffMilliseconds);
        kalman.set_transitionMatrix(transitionMatrix);


        Mat prediction = kalman.predict();
        distanceArray[0] = prediction.get(0, 0)[0]; // xVal
        distanceArray[1] = prediction.get(3, 0)[0]; // yVal
        distanceArray[2] = prediction.get(6, 0)[0]; // zVal
        return distanceArray;
    }

    //private void kalmanCorrection(int xVal, int yVal, int zVal){
    //    measurementMatrix.put(0,0,xVal);
    //    measurementMatrix.put(1,0,yVal);
    //    measurementMatrix.put(2,0,zVal);
    //    kalman.correct(measurementMatrix);
    //}

  private void kalmanCorrection(int xVal, int yVal, int zVal){
        Mat actualObservations = new Mat(measSize,1, CV_32F);
        actualObservations.put(0,0,xVal);
        actualObservations.put(1,0,yVal);
        actualObservations.put(2,0,zVal);
        kalman.correct(actualObservations);
  }

}

Answer 1

kalman.correct() takes a measurement, but you're passing in the KalmanFilter's own measurementMatrix back into itself that you first assigned via the kalman.set_measurementMatrix() call. (Yes, they're different!) The measurementMatrix is a (probably static) transform from state space to measurement space, whereas measurements are your actual observations that get continually update in the loop. This also means your comment "set init measurement" is wrong and likely leading to the misunderstanding. (Yes, the opencv KF naming is confusing.) You need to add an additional measurement matrix for passing the observations to correct().

The error message is telling you that the gemm() call inside the kalman.correct() method is failing because the dimensions aren't right for how it has been configured. You're passing in a 3x9 matrix where it is expecting a 3x1.

UPDATE:

I didn't catch it the first time through your code, but the measurementNoiseCov matrix dimensions also need to be changed to measSizexmeasSize instead of stateSizexstateSize in order to match the observation size.