需要帮助解决 Java 中使用 opencv 的卡尔曼滤波器实现中的错误

Question

我正在为 3D (X,Y,Z) 坐标中的运动数据使用 opencv 的卡尔曼滤波器实现来实现卡尔曼滤波器。该模型使用加速度和速度模型

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

下面的代码在

处抛出错误

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)

有人可以看看提到的问题吗？

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() 需要一个 measurement，但是您将 KalmanFilter 自己的 measurementMatrix 传递回您首先通过 [=14] 分配的自身=] 打电话。（是的，它们是不同的！）measurementMatrix 是一个（可能是静态的）从状态 space 到测量值 space 的转换，而 measurements 是你得到的实际观察结果在循环中不断更新。这也意味着您的评论 "set init measurement" 是错误的，可能会导致误解。（是的，opencv KF 命名令人困惑。）您需要添加一个额外的 measurement 矩阵以将观察结果传递给 correct()。

错误消息告诉您 kalman.correct() 方法中的 gemm() 调用失败，因为尺寸不适合其配置方式。您正在传递一个 3x9 矩阵，它期望一个 3x1.

更新：

我第一次通过你的代码没看出来，但是 measurementNoiseCov 矩阵维度也需要更改为 measSizexmeasSize 而不是 stateSizexstateSize 以匹配观察大小。

需要帮助解决 Java 中使用 opencv 的卡尔曼滤波器实现中的错误

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

java

android

opencv

prediction

kalman-filter