From 9b3c936af725353b1da97cc32aacef623d0e9d07 Mon Sep 17 00:00:00 2001 From: Donovan Baarda Date: Tue, 21 Oct 2014 13:33:31 +1100 Subject: Set (accel|noise)_variance from (accel|noise)_stddev settings. Use better estimates for filter settings giving less filtering. Fix broken old reference to target_camera_position. --- ftnoir_filter_kalman/ftnoir_filter_kalman.h | 6 ++++-- ftnoir_filter_kalman/kalman.cpp | 9 ++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h index 8ede4c7d..f6224530 100755 --- a/ftnoir_filter_kalman/ftnoir_filter_kalman.h +++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h @@ -25,8 +25,10 @@ public: FTNoIR_Filter(); void reset(); void filter(const double *input, double *output); - double accel_variance; - double noise_variance; + // Set accel_stddev assuming moving 0.0->100.0 in dt=0.2 is 3 stddevs: (100.0*4/dt^2)/3. + const double accel_stddev = (100.0*4/(0.2*0.2))/3.0; + // TODO(abo): make noise_stddev a UI setting 0.0->10.0 with 0.1 resolution. + const double noise_stddev = 1.0; cv::KalmanFilter kalman; double last_input[6]; QElapsedTimer timer; diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp index a03aad3c..c5a600af 100644 --- a/ftnoir_filter_kalman/kalman.cpp +++ b/ftnoir_filter_kalman/kalman.cpp @@ -16,10 +16,6 @@ FTNoIR_Filter::FTNoIR_Filter() { // the following was written by Donovan Baarda // https://sourceforge.net/p/facetracknoir/discussion/1150909/thread/418615e1/?limit=25#af75/084b void FTNoIR_Filter::reset() { - // Set accel_variance for moving 0.0->1.0 in dt=0.1. - accel_variance = 400.0f; - // TODO(abo): make noise_variance a UI setting 0.0->1.0. - noise_variance = 0.1; // Setup kalman with state (x) is the 6 tracker outputs then // their 6 corresponding velocities, and the measurement (z) is // the 6 tracker outputs. @@ -42,6 +38,7 @@ void FTNoIR_Filter::reset() { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1); + double accel_variance = accel_stddev * accel_stddev; double a = dt * dt * accel_variance; // dt^2 * accel_variance. double b = 0.5 * a * dt; // (dt^3)/2 * accel_variance. double c = 0.5 * b * dt; // (dt^4)/4 * accel_variance. @@ -59,6 +56,7 @@ void FTNoIR_Filter::reset() { 0, 0, 0, 0, b, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, a); cv::setIdentity(kalman.measurementMatrix); + double noise_variance = noise_stddev * noise_stddev; cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar::all(noise_variance)); cv::setIdentity(kalman.errorCovPost, cv::Scalar::all(accel_variance * 1e4)); for (int i = 0; i < 6; i++) { @@ -80,6 +78,7 @@ void FTNoIR_Filter::filter(const double* input, double *output) for (int i = 0; i < 6 && !new_input; i++) new_input = (input[i] != last_input[i]); // Update the transitionMatrix and processNoiseCov for dt. + double accel_variance = accel_stddev * accel_stddev; double a = dt * dt * accel_variance; // dt^2 * accel_variance. double b = 0.5 * a * dt; // (dt^3)/2 * accel_variance. double c = 0.5 * b * dt; // (dt^4)/4 * accel_variance. @@ -96,7 +95,7 @@ void FTNoIR_Filter::filter(const double* input, double *output) if (new_input) { cv::Mat measurement(6, 1, CV_64F); for (int i = 0; i < 6; i++) { - measurement.at(i) = target_camera_position[i]; + measurement.at(i) = input[i]; // Save last_input for detecting new tracker input. last_input[i] = input[i]; } -- cgit v1.2.3