Add sanity check on timestep length to HybridEkf
If we receive an unusually long time-step, that will virtually always
indicate some sort of error. As such, ignore the samples rather than
allowing the integrator in the prediction to wind things up.
Change-Id: Ice2cbd350800aeaa496cf15e3a3a1a6b0c9cf8fb
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index bd358ca..4fd2d43 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -160,6 +160,11 @@
// offsets decay, in seconds.
static constexpr double kVelocityOffsetTimeConstant = 1.0;
static constexpr double kLateralVelocityTimeConstant = 1.0;
+ // The maximum allowable timestep--we use this to check for situations where
+ // measurement updates come in too infrequently and this might cause the
+ // integrator and discretization in the prediction step to be overly
+ // aggressive.
+ static constexpr std::chrono::milliseconds kMaxTimestep{20};
// Inputs are [left_volts, right_volts]
typedef Eigen::Matrix<Scalar, kNInputs, 1> Input;
// Outputs are either:
@@ -559,7 +564,7 @@
State *state, StateSquare *P) {
*state = obs->X_hat;
*P = obs->P;
- if (dt.count() != 0) {
+ if (dt.count() != 0 && dt < kMaxTimestep) {
PredictImpl(obs, dt, state, P);
}
if (!(obs->h && obs->dhdx)) {
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index bdfdd4e..48b8b5f 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -406,6 +406,19 @@
}
}
+// Tests that if we have an unusually large gap between measurement updates that
+// nothing crazy happens (a previous implementation had a bug where large
+// time-steps would mess up the prediction step due to how we approximated
+// things).
+TEST_F(HybridEkfTest, ExtraLongUpdateTime) {
+ Input U;
+ U << 0.0, 0.0, 0.1, 0.1;
+ ekf_.RawUpdateEncodersAndGyro(0.0, 0.0, 0.0, U, t0_ + dt_config_.dt);
+ ekf_.RawUpdateEncodersAndGyro(0.0, 0.0, 0.0, U,
+ t0_ + std::chrono::seconds(1000));
+ EXPECT_LT(ekf_.X_hat().norm(), 10.0) << ekf_.X_hat();
+}
+
// Tests encoder/gyro updates when we have some errors in our estimate.
TEST_F(HybridEkfTest, PerfectEncoderUpdateConverges) {
// In order to simulate modelling errors, we add an angular_error and start