commit 20a2b774c85cd738c9417f2e573dda772e297dc8
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Mar 15 22:46:46 2024 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Mar 16 12:04:33 2024 -0700
tree f2476d35fd50271a7803df98e440b63ba44f4b3c
parent fda3f8d1702b9177f0a2e47f3cfc91d237b68e12

Fix handling of encoder initialization in HybridEkf

We were handling it oddly---we should not have been indicating that the
encoders were initialized when we first initialized the state.

Also, add a force_dt parameter to be used as mitigation for some other
timing issues in the y2024 localizer.

Change-Id: I866355188ca88402532b2a00a7dcfe3ace348edf
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

frc971/control_loops/drivetrain/hybrid_ekf.h

diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 2f89a0c..b931e52 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -2,6 +2,7 @@
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_HYBRID_EKF_H_
 
 #include <chrono>
+#include <optional>
 
 #include "Eigen/Dense"
 
@@ -306,22 +307,27 @@
   // Currently, we use the drivetrain config for modelling constants
   // (continuous time A and B matrices) and for the noise matrices for the
   // encoders/gyro.
-  HybridEkf(const DrivetrainConfig<double> &dt_config)
+  // If force_dt is set, then all predict steps will use a dt of force_dt.
+  // This can be used in situations where there is no reliable clock guiding
+  // the measurement updates, but the source is coming in at a reasonably
+  // consistent period.
+  HybridEkf(const DrivetrainConfig<double> &dt_config,
+            std::optional<std::chrono::nanoseconds> force_dt = std::nullopt)
       : dt_config_(dt_config),
         velocity_drivetrain_coefficients_(
             dt_config.make_hybrid_drivetrain_velocity_loop()
                 .plant()
-                .coefficients()) {
+                .coefficients()),
+        force_dt_(force_dt) {
     InitializeMatrices();
   }
 
   // Set the initial guess of the state. Can only be called once, and before
-  // any measurement updates have occured.
+  // any measurement updates have occurred.
   void ResetInitialState(::aos::monotonic_clock::time_point t,
                          const State &state, const StateSquare &P) {
     observations_.clear();
     X_hat_ = state;
-    have_zeroed_encoders_ = true;
     P_ = P;
     observations_.PushFromBottom({
         t,
@@ -393,10 +399,8 @@
       newstate(kAngularError) = 0.0;
       newstate(kLongitudinalVelocityOffset) = 0.0;
       newstate(kLateralVelocity) = 0.0;
-      ResetInitialState(t, newstate, P_);
-      // We need to set have_zeroed_encoders_ after ResetInitialPosition because
-      // the reset clears have_zeroed_encoders_...
       have_zeroed_encoders_ = true;
+      ResetInitialState(t, newstate, P_);
     }
 
     Output z(left_encoder, right_encoder, gyro_rate);
@@ -658,6 +662,9 @@
 
   void PredictImpl(Observation *obs, std::chrono::nanoseconds dt, State *state,
                    StateSquare *P) {
+    if (force_dt_.has_value()) {
+      dt = force_dt_.value();
+    }
     // Only recalculate the discretization if the timestep has changed.
     // Technically, this isn't quite correct, since the discretization will
     // change depending on the current state. However, the slight loss of
@@ -728,6 +735,7 @@
   State X_hat_;
   StateFeedbackHybridPlantCoefficients<2, 2, 2, double>
       velocity_drivetrain_coefficients_;
+  std::optional<std::chrono::nanoseconds> force_dt_;
   StateSquare A_continuous_;
   StateSquare Q_continuous_;
   StateSquare P_;