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

asked 2019-06-16 04:57:00 -0500

Kafan gravatar image

updated 2019-06-17 01:49:10 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

wait,

    kalman.set_measurementMatrix(measurementMatrix);

and:

     kalman.correct(measurementMatrix);

that's not the same matrix, the latter is a measurement in the same dims as your prediction.

berak gravatar imageberak ( 2019-06-16 08:55:42 -0500 )edit
1

@berak Updated the declaration and initlization of measurementMatrix inside constructor of class and also updated kalmanCorrection method by passing a 3x1 matrix with actualObservations. But it is still throwing me error of 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)

Kafan gravatar imageKafan ( 2019-06-17 01:47:43 -0500 )edit