我正在使用opencv的Kalman滤波器实现在3D(X，Y，Z)坐标中的运动数据上实现Kalman滤波器。该模型将加速度和速度模型用于
s = s(0) + v*t + 0.5*a*t^2
下面的代码在抛出错误
kalman.correct(measurementMatrix);
有人可以看看提到的问题吗？
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);
}
}
最佳答案
kalman.correct()带有一个measurement，但是您正在将KalmanFilter自己的measurementMatrix传递回给自己，该名称是您首先通过kalman.set_measurementMatrix()调用首先分配的。 (是的，它们是不同的!)measurementMatrix是从状态空间到测量空间的(可能是静态的)转换，而measurement是您在循环中不断更新的实际观察结果。这也意味着您的评论“设置初始测量”是错误的，并且可能导致误解。 (是的，opencv KF的命名令人困惑。)您需要添加一个附加的measurement矩阵，以将观测值传递给correct()。
错误消息告诉您gemm()方法内的kalman.correct()调用失败，因为尺寸与配置方式不符。您传递的是3x9矩阵，该矩阵预期为3x1。
更新:
我没有在您的代码中第一次发现它，但是measurementNoiseCov矩阵的尺寸也需要更改为measSize x measSize而不是stateSize x stateSize才能匹配观察尺寸。