From cb58e5a3a4c72eb065daf7d3e1ab04754533f9ee Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Fri, 12 Oct 2012 14:01:36 +0400 Subject: [PATCH] let Kalman handle the missing measurements (bug #1380) --- modules/video/src/kalman.cpp | 6 ++++++ samples/cpp/kalman.cpp | 3 ++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/modules/video/src/kalman.cpp b/modules/video/src/kalman.cpp index b7a24b86a..a86fbf518 100644 --- a/modules/video/src/kalman.cpp +++ b/modules/video/src/kalman.cpp @@ -171,6 +171,9 @@ cvKalmanPredict( CvKalman* kalman, const CvMat* control ) /* P'(k) = temp1*At + Q */ cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1, kalman->error_cov_pre, CV_GEMM_B_T ); + + /* handle the case when there will be measurement before the next predict */ + cvCopy(kalman->state_pre, kalman->state_post); return kalman->state_pre; } @@ -260,6 +263,9 @@ const Mat& KalmanFilter::predict(const Mat& control) // P'(k) = temp1*At + Q gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T); + + // handle the case when there will be measurement before the next predict. + statePre.copyTo(statePost); return statePre; } diff --git a/samples/cpp/kalman.cpp b/samples/cpp/kalman.cpp index 9c6a9001f..f54adec92 100644 --- a/samples/cpp/kalman.cpp +++ b/samples/cpp/kalman.cpp @@ -82,7 +82,8 @@ int main(int, char**) line( img, statePt, measPt, Scalar(0,0,255), 3, CV_AA, 0 ); line( img, statePt, predictPt, Scalar(0,255,255), 3, CV_AA, 0 ); - KF.correct(measurement); + if(theRNG().uniform(0,4) != 0) + KF.correct(measurement); randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at(0, 0)))); state = KF.transitionMatrix*state + processNoise;