summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rwxr-xr-xftnoir_filter_kalman/ftnoir_filter_kalman.h11
-rw-r--r--ftnoir_filter_kalman/kalman.cpp35
2 files changed, 22 insertions, 24 deletions
diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
index c2947516..f6224530 100755
--- a/ftnoir_filter_kalman/ftnoir_filter_kalman.h
+++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
@@ -24,12 +24,13 @@ class OPENTRACK_EXPORT FTNoIR_Filter : public IFilter
public:
FTNoIR_Filter();
void reset();
- void filter(const double *target_camera_position,
- double *new_camera_position);
- double accel_variance;
- double noise_variance;
+ void filter(const double *input, double *output);
+ // 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 prev_position[6];
+ double last_input[6];
QElapsedTimer timer;
};
diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp
index ab352faa..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 <abo@minkirri.apana.org.au>
// 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,16 +56,16 @@ 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++) {
- prev_position[i] = 0;
+ last_input[i] = 0;
}
timer.invalidate();
}
-void FTNoIR_Filter::filter(const double* target_camera_position,
- double *new_camera_position)
+void FTNoIR_Filter::filter(const double* input, double *output)
{
// Start the timer if it's not running.
if (!timer.isValid())
@@ -77,11 +74,11 @@ void FTNoIR_Filter::filter(const double* target_camera_position,
auto dt = timer.restart() / 1000.0f;
// Note this is a terrible way to detect when there is a new
// frame of tracker input, but it is the best we have.
- bool new_target = false;
- for (int i = 0; i < 6 && !new_target; i++)
- new_target = (prev_position[i] != target_camera_position[i]);
-
+ bool new_input = false;
+ 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.
@@ -93,20 +90,20 @@ void FTNoIR_Filter::filter(const double* target_camera_position,
kalman.processNoiseCov.at<double>(i+6,i) = b;
}
// Get the updated predicted position.
- cv::Mat output = kalman.predict();
+ cv::Mat next_output = kalman.predict();
// If we have new tracker input, get the corrected position.
- if (new_target) {
+ if (new_input) {
cv::Mat measurement(6, 1, CV_64F);
for (int i = 0; i < 6; i++) {
- measurement.at<double>(i) = target_camera_position[i];
- // Save prev_position for detecting new tracker input.
- prev_position[i] = target_camera_position[i];
+ measurement.at<double>(i) = input[i];
+ // Save last_input for detecting new tracker input.
+ last_input[i] = input[i];
}
- output = kalman.correct(measurement);
+ next_output = kalman.correct(measurement);
}
- // Set new_camera_position to the output.
+ // Set output to the next_output.
for (int i = 0; i < 6; i++) {
- new_camera_position[i] = output.at<double>(i);
+ output[i] = next_output.at<double>(i);
}
}