summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorDaMichel <mw.pub@welter-4d.de>2016-08-04 13:31:23 +0200
committerDaMichel <mw.pub@welter-4d.de>2016-08-04 15:04:13 +0200
commitb798b0ae5a971bbb43c6f47b667f108a747bfffe (patch)
treedd569c6d5fd95ad39db3a2e8139e8822a9aed1f3
parente1658b7ffaae49085ea8ef501b24b8645b071b85 (diff)
filter/kalman: deadzone tweaks
-rw-r--r--filter-kalman/kalman.cpp17
-rw-r--r--filter-kalman/kalman.h5
2 files changed, 9 insertions, 13 deletions
diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp
index 1d71e3f6..6ed5ca91 100644
--- a/filter-kalman/kalman.cpp
+++ b/filter-kalman/kalman.cpp
@@ -199,8 +199,6 @@ void FTNoIR_Filter::reset()
prev_slider_pos[0] = static_cast<slider_value>(s.noise_pos_slider_value);
prev_slider_pos[1] = static_cast<slider_value>(s.noise_rot_slider_value);
- //minimal_state_var = PoseVector::Constant(std::numeric_limits<double>::max());
-
dz_filter.reset();
}
@@ -238,15 +236,14 @@ void FTNoIR_Filter::filter(const double* input_, double *output_)
{
// Compute deadzone size base on estimated state variance.
- // Given a constant input plus noise, KF should converge to the true (constant) input.
- // This works indeed. That is the output pose becomes very still afte some time.
- // At this point the estimated cov should be minimal. We can use this to
- // calculate the size of the deadzone, so that in the stationary state the
- // deadzone size is zero. Thus the tracking error due to the dz-filter
- // becomes zero.
+ // Given a constant input plus measurement noise, KF should converge to the true input.
+ // This works well. That is the output pose becomes very still afte some time.
+ // The QScaling adaptive filter makes the state cov vary depending on the estimated noise
+ // and the measured noise of the innovation sequence. After a sudden movement it peaks
+ // and then decays asymptotically to some constant value taken in stationary state.
+ // We can use this to calculate the size of the deadzone, so that in the stationary state the
+ // deadzone size is small. Thus the tracking error due to the dz-filter becomes also small.
PoseVector variance = kf.state_cov.diagonal().head(6);
- //minimal_state_var = minimal_state_var.cwiseMin(variance);
- //dz_filter.dz_size = (variance - minimal_state_var).cwiseSqrt() * s.deadzone_scale;
dz_filter.dz_size = variance.cwiseSqrt() * s.deadzone_scale;
}
output = dz_filter.filter(output);
diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
index 3c9466a5..aaed3bc1 100644
--- a/filter-kalman/kalman.h
+++ b/filter-kalman/kalman.h
@@ -91,8 +91,8 @@ struct settings : opts {
value<slider_value> noise_pos_slider_value;
static constexpr double adaptivity_window_length = 0.5; // seconds
- static constexpr double deadzone_scale = 2.;
- static constexpr double deadzone_exponent = 4.0;
+ static constexpr double deadzone_scale = 2;
+ static constexpr double deadzone_exponent = 2.0;
static constexpr double process_sigma_pos = 0.5;
static constexpr double process_simga_rot = 0.5;
@@ -147,7 +147,6 @@ public:
settings s;
KalmanFilter kf;
KalmanProcessNoiseScaler kf_adaptive_process_noise_cov;
- //PoseVector minimal_state_var;
DeadzoneFilter dz_filter;
slider_value prev_slider_pos[2];