diff options
author | Stanisław Halik <sthalik@misaki.pl> | 2016-07-08 16:08:16 +0200 |
---|---|---|
committer | GitHub <noreply@github.com> | 2016-07-08 16:08:16 +0200 |
commit | 0f2806bb5bc5ccd9a4d6772fe57563f981f76e18 (patch) | |
tree | 769119e25c6c2ec8ca7116b8f49a053e1d2015b4 /filter-kalman/kalman.cpp | |
parent | 5adc4acd119923278ed346ceb0c899751c5c1a50 (diff) | |
parent | d9a10a3276555e74b7f09e7fe30201c511094ac7 (diff) |
Merge pull request #383 from DaMichel/kalman
Kalman filter
Diffstat (limited to 'filter-kalman/kalman.cpp')
-rw-r--r-- | filter-kalman/kalman.cpp | 16 |
1 files changed, 12 insertions, 4 deletions
diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp index 05731cb6..deaf6321 100644 --- a/filter-kalman/kalman.cpp +++ b/filter-kalman/kalman.cpp @@ -66,7 +66,7 @@ void FTNoIR_Filter::reset() { for (int i = 0; i < 6; i++) { last_input[i] = 0; } - timer.invalidate(); + first_run = true; } void FTNoIR_Filter::filter(const double* input, double *output) @@ -76,16 +76,24 @@ void FTNoIR_Filter::filter(const double* input, double *output) reset(); prev_slider_pos = s.noise_stddev_slider; } - // Start the timer if it's not running. - if (!timer.isValid()) + // Start the timer on first filter evaluation. + if (first_run) + { timer.start(); + first_run = false; + return; + } + // Get the time in seconds since last run and restart the timer. - const double dt = timer.restart() / 1000.; + const double dt = timer.elapsed_seconds(); + timer.start(); + // 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_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. |