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/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();
}