Improve handling of absent encoder values
This is the first year that we have relied on the network to forward
encoder values to the localizer, which makes it so that we can no longer
rely on encoder values being available. Improve our behavior in these
cases to prevent us from destroying the localizer's state too much when
we lose encoders/network.
Change-Id: I664d331a8249873987402a819a38ad75e70fc4ff
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 b931e52..0a41c55 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -364,8 +364,9 @@
// A utility function for specifically updating with encoder and gyro
// measurements.
- void UpdateEncodersAndGyro(const Scalar left_encoder,
- const Scalar right_encoder, const Scalar gyro_rate,
+ void UpdateEncodersAndGyro(const std::optional<Scalar> left_encoder,
+ const std::optional<Scalar> right_encoder,
+ const Scalar gyro_rate,
const Eigen::Matrix<Scalar, 2, 1> &voltage,
const Eigen::Matrix<Scalar, 3, 1> &accel,
aos::monotonic_clock::time_point t) {
@@ -377,8 +378,8 @@
}
// Version of UpdateEncodersAndGyro that takes a input matrix rather than
// taking in a voltage/acceleration separately.
- void RawUpdateEncodersAndGyro(const Scalar left_encoder,
- const Scalar right_encoder,
+ void RawUpdateEncodersAndGyro(const std::optional<Scalar> left_encoder,
+ const std::optional<Scalar> right_encoder,
const Scalar gyro_rate, const Input &U,
aos::monotonic_clock::time_point t) {
// Because the check below for have_zeroed_encoders_ will add an
@@ -392,24 +393,38 @@
// wpilib_interface, then we can get some obnoxious initial corrections
// that mess up the localization.
State newstate = X_hat_;
- newstate(kLeftEncoder) = left_encoder;
- newstate(kRightEncoder) = right_encoder;
+ have_zeroed_encoders_ = true;
+ if (left_encoder.has_value()) {
+ newstate(kLeftEncoder) = left_encoder.value();
+ } else {
+ have_zeroed_encoders_ = false;
+ }
+ if (right_encoder.has_value()) {
+ newstate(kRightEncoder) = right_encoder.value();
+ } else {
+ have_zeroed_encoders_ = false;
+ }
newstate(kLeftVoltageError) = 0.0;
newstate(kRightVoltageError) = 0.0;
newstate(kAngularError) = 0.0;
newstate(kLongitudinalVelocityOffset) = 0.0;
newstate(kLateralVelocity) = 0.0;
- have_zeroed_encoders_ = true;
ResetInitialState(t, newstate, P_);
}
- Output z(left_encoder, right_encoder, gyro_rate);
+ Output z(left_encoder.value_or(0.0), right_encoder.value_or(0.0),
+ gyro_rate);
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
R.setZero();
R.diagonal() << encoder_noise_, encoder_noise_, gyro_noise_;
CHECK(H_encoders_and_gyro_.has_value());
- Correct(z, &U, nullptr, &H_encoders_and_gyro_.value(), R, t);
+ CHECK(H_gyro_only_.has_value());
+ LinearH *H = &H_encoders_and_gyro_.value();
+ if (!left_encoder.has_value() || !right_encoder.has_value()) {
+ H = &H_gyro_only_.value();
+ }
+ Correct(z, &U, nullptr, H, R, t);
}
// Sundry accessor:
@@ -740,6 +755,7 @@
StateSquare Q_continuous_;
StateSquare P_;
std::optional<LinearH> H_encoders_and_gyro_;
+ std::optional<LinearH> H_gyro_only_;
Scalar encoder_noise_, gyro_noise_;
Eigen::Matrix<Scalar, kNStates, kNInputs> B_continuous_;
@@ -920,13 +936,14 @@
{
Eigen::Matrix<Scalar, kNOutputs, kNStates> H_encoders_and_gyro;
H_encoders_and_gyro.setZero();
+ // Gyro rate is just the difference between right/left side speeds:
+ H_encoders_and_gyro(2, kLeftVelocity) = -1.0 / diameter;
+ H_encoders_and_gyro(2, kRightVelocity) = 1.0 / diameter;
+ H_gyro_only_.emplace(H_encoders_and_gyro);
// Encoders are stored directly in the state matrix, so are a minor
// transform away.
H_encoders_and_gyro(0, kLeftEncoder) = 1.0;
H_encoders_and_gyro(1, kRightEncoder) = 1.0;
- // Gyro rate is just the difference between right/left side speeds:
- H_encoders_and_gyro(2, kLeftVelocity) = -1.0 / diameter;
- H_encoders_and_gyro(2, kRightVelocity) = 1.0 / diameter;
H_encoders_and_gyro_.emplace(H_encoders_and_gyro);
}
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index 0986037..14bc2ef 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -41,7 +41,9 @@
template <typename T>
std::optional<Eigen::Vector2d> GetPosition(
T &fetcher, aos::monotonic_clock::time_point now) {
- fetcher.Fetch();
+ if (!fetcher.Fetch()) {
+ return std::nullopt;
+ }
const bool stale =
(fetcher.get() == nullptr) ||
(fetcher.context().monotonic_event_time + std::chrono::milliseconds(10) <
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index 4d21e3b..8c3a239 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -31,9 +31,9 @@
// [left_voltage, right_voltage]
Eigen::Vector2d VoltageOrZero(aos::monotonic_clock::time_point now);
- // Returns the latest drivetrain encoder values (in meters), or nullopt if no
- // position message is available (or if the message is stale).
- // Returns encoders as [left_position, right_position]
+ // Returns the latest drivetrain encoder values (in meters), or nullopt if
+ // there has been no new encoder reading since the last call. Returns encoders
+ // as [left_position, right_position]
std::optional<Eigen::Vector2d> Encoders(aos::monotonic_clock::time_point now);
// Returns true if either there is no JoystickState message available or if
diff --git a/y2024/localizer/localizer.cc b/y2024/localizer/localizer.cc
index f8c1079..b8e982e 100644
--- a/y2024/localizer/localizer.cc
+++ b/y2024/localizer/localizer.cc
@@ -313,10 +313,6 @@
Eigen::Vector3d gyro, Eigen::Vector3d accel) {
std::optional<Eigen::Vector2d> encoders = utils_.Encoders(sample_time_orin);
last_encoder_readings_ = encoders;
- // Ignore invalid readings; the HybridEkf will handle it reasonably.
- if (!encoders.has_value()) {
- return;
- }
VLOG(1) << "Got encoders";
if (t_ == aos::monotonic_clock::min_time) {
t_ = sample_time_orin;
@@ -331,8 +327,12 @@
// convenient for debugging.
down_estimator_.Predict(gyro, accel, dt);
const double yaw_rate = (dt_config_.imu_transform * gyro)(2);
- ekf_.UpdateEncodersAndGyro(encoders.value()(0), encoders.value()(1), yaw_rate,
- utils_.VoltageOrZero(sample_time_orin), accel, t_);
+ ekf_.UpdateEncodersAndGyro(
+ encoders.has_value() ? std::make_optional<double>(encoders.value()(0))
+ : std::nullopt,
+ encoders.has_value() ? std::make_optional<double>(encoders.value()(1))
+ : std::nullopt,
+ yaw_rate, utils_.VoltageOrZero(sample_time_orin), accel, t_);
SendStatus();
}