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

Question

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

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 返回到您首先通过 kalman.set_measurementMatrix() 调用分配的自身。 (是的，它们是不同的!)测量矩阵是从状态空间到测量空间的(可能是静态的)转换，而测量是不断更新的实际观察结果在循环。这也意味着您的评论“设置初始化测量”是错误的，可能会导致误解。 (是的，opencv KF 命名令人困惑。)您需要添加一个额外的测量矩阵，用于将观察结果传递给 Correct()。

错误消息告诉您 kalman. Correct() 方法内的 gemm() 调用失败，因为尺寸不正确已配置。您传递的是 3x9 矩阵，而它期望的是 3x1。

更新:

我第一次通过您的代码时没有发现它，但是 measurementNoiseCov 矩阵维度也需要更改为 measSizexmeasSize 而不是 stateSizexstateSize 以匹配观察大小。