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;
Mat measurementMatrix = Mat.eye(measSize,stateSize, CV_32F);
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
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);
}
}
