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>
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_;
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 2119c7d..96868cb 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -378,6 +378,14 @@
State true_X = ekf_.X_hat();
Input U;
U << -1.0, 5.0, 0.0, 0.0;
+ // Give one good update before starting to move things so that we initialize
+ // the encoders at zero.
+ ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
+ true_X(StateIdx::kRightEncoder, 0),
+ (true_X(StateIdx::kRightVelocity, 0) -
+ true_X(StateIdx::kLeftVelocity, 0)) /
+ dt_config_.robot_radius / 2.0,
+ U, t0_);
for (int ii = 0; ii < 100; ++ii) {
true_X = Update(true_X, U, false);
ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
@@ -423,6 +431,14 @@
// small they are negligible.
Input U;
U << 10.0, 5.0, 0.0, 0.0;
+ // Give one good update before starting to move things so that we initialize
+ // the encoders at zero.
+ ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
+ true_X(StateIdx::kRightEncoder, 0),
+ (true_X(StateIdx::kRightVelocity, 0) -
+ true_X(StateIdx::kLeftVelocity, 0)) /
+ dt_config_.robot_radius / 2.0,
+ U, t0_);
for (int ii = 0; ii < 100; ++ii) {
true_X = Update(true_X, U, false);
ekf_.RawUpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),