在 OpenCV Java 中实现卡尔曼滤波器

Posted 2023-02-21

【中文标题】在 OpenCV Java 中实现卡尔曼滤波器

【英文标题】：Implementing Kalman filter in OpenCV Java

【发布时间】：2016-02-15 10:59:43

【问题描述】：

我正在尝试在 Java 的 OpenCV 程序中实现卡尔曼滤波器。我是 OpenCV 和卡尔曼滤波器的新手。我在 C++ 中找到了一些示例（在 Java 中并不多），这就是我目前所拥有的：

初始化：

    //create kalman filter  
    KalmanFilter kalman = new KalmanFilter(4,2,0,CvType.CV_32F);  
    //set transition matrix
    float[] tM =  1, 0, 1, 0, 
            0, 1, 0, 1,
            0, 0, 1, 0,
            0, 0, 0, 1  ;
    Mat transitionMatrix=new Mat(4,4,CvType.CV_32F,new Scalar(0));
    transitionMatrix.put(0,0,tM);
    kalman.set_transitionMatrix(transitionMatrix);
    //set init measurement
    Mat measurementMatrix = new Mat (2,1, CvType.CV_32F);
    measurementMatrix.setTo(new Scalar(0));
    kalman.set_measurementMatrix(measurementMatrix);

    //Set state matrix
    Mat statePre = new Mat(4,1, CvType.CV_32F);
    statePre.put(1, 1, 300);
    statePre.put(2, 1, 200);
    statePre.put(3, 1, 0);
    statePre.put(4, 1, 0);
    kalman.set_statePre(statePre);


    //Process noise Covariance matrix
    Mat processNoiseCov=Mat.eye(4,4,CvType.CV_32F);
    processNoiseCov=processNoiseCov.mul(processNoiseCov,1e-1);
    kalman.set_processNoiseCov(processNoiseCov);

    //Measurement noise Covariance matrix: reliability on our first measurement
    Mat measurementNoiseCov=Mat.eye(4,4,CvType.CV_32F);
    measurementNoiseCov=measurementNoiseCov.mul(measurementNoiseCov,1e-1);
    kalman.set_measurementNoiseCov(measurementNoiseCov);

    Mat id2=Mat.eye(4,4,CvType.CV_32F);
    id2=id2.mul(id2,0.1);
    kalman.set_errorCovPost(id2);

对于每个视频帧：

    prediction= kalman.predict();
    predictPt.x = prediction.get(1,1)[0];
    predictPt.y = prediction.get(2,1)[0];

...新测量..

   measurementMatrix.put(1, 1, center.x);
   measurementMatrix.put(2, 1, center.y);
   measPt.x=center.x;
   measPt.y=center.y;

   Mat estimated = kalman.correct(measurementMatrix);
   statePt.x=estimated.get(1, 1)[1];
   statePt.y= estimated.get(2, 1)[1];

问题是我得到一个空预测，我看不出得到它的原因。有人知道我的代码有什么问题吗？我真的很感激任何帮助！

谢谢！

【参考方案1】：

您得到一个空预测，因为您既没有插入也没有访问正确的元素。

第一：

statePre.put(0, 1, 300); //statePre.put(1, 1, 300);
statePre.put(1, 1, 200); //statePre.put(2, 1, 200);
statePre.put(2, 1, 0); //statePre.put(3, 1, 0);
statePre.put(3, 1, 0); // statePre.put(4, 1, 0);

预测：

prediction= kalman.predict();
predictPt.x = prediction.get(0,0)[0]; //predictPt.x = prediction.get(1,1)[0];
predictPt.y = prediction.get(1,0)[0]; //predictPt.y = prediction.get(2,1)[0];

最后：

measurementMatrix.put(0, 0, center.x); // measurementMatrix.put(1, 1, center.x);
measurementMatrix.put(1, 0, center.y); //measurementMatrix.put(2, 1, center.y);
measPt.x=center.x;
measPt.y=center.y;

Mat estimated = kalman.correct(measurementMatrix);
statePt.x=estimated.get(0,0)[0];
statePt.y= estimated.get(1,0)[0];

另外，你的measurementMatrix应该是这样的

 Mat measurementMatrix = Mat.eye(2,4, CvType.CV_32F);