Нужна помощь в устранении ошибки в реализации фильтра Калмана в Java с использованием opencv - PullRequest
Новая
       36

Нужна помощь в устранении ошибки в реализации фильтра Калмана в Java с использованием opencv

0 голосов
/ 16 июня 2019

Я реализую фильтр Калмана, используя реализацию фильтра Калмана в 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 ()пойман cv :: Исключение: /build/master_pack-android/opencv/modules/core/src/matmul.cpp:1588: ошибка: (-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)) в функции 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);
  }

}

1 Ответ

1 голос
/ 16 июня 2019

kalman.correct() занимает measurement, но вы передаете собственный measurementMatrix KalmanFilter обратно в себя, который вы сначала присвоили с помощью вызова kalman.set_measurementMatrix(). (Да, они разные!) measurementMatrix - это (возможно, статическое) преобразование из пространства состояний в пространство измерений, тогда как measurement s - это ваши фактические наблюдения, которые постоянно обновляются в цикле. Это также означает, что ваш комментарий «установить начальное измерение» неверен и может привести к недоразумению. (Да, именование opencv KF сбивает с толку.) Вам необходимо добавить дополнительную матрицу measurement для передачи наблюдений в correct().

В сообщении об ошибке сообщается, что вызов gemm() внутри метода kalman.correct() завершается ошибкой, поскольку измерения не соответствуют его настройке. Вы переходите в матрицу 3x9, где она ожидает 3x1.

UPDATE:

Я не уловил его в первый раз через ваш код, но размеры матрицы measurementNoiseCov также необходимо изменить на measSize x measSize вместо stateSize x stateSize, чтобы соответствовать Размер наблюдения.

...