Correctly handle corrupted encoder readings in localizer
Also, tune down image corrections and remap channels in replay.
This does not fix the yaw issues.
Change-Id: I8587148db1bd03d12210f951306cca3e30a2cb84
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index 24df19b..b12525b 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -258,11 +258,10 @@
state->model_state += K * (Z - H * state->model_state);
}
-void ModelBasedLocalizer::HandleImu(aos::monotonic_clock::time_point t,
- const Eigen::Vector3d &gyro,
- const Eigen::Vector3d &accel,
- const Eigen::Vector2d encoders,
- const Eigen::Vector2d voltage) {
+void ModelBasedLocalizer::HandleImu(
+ aos::monotonic_clock::time_point t, const Eigen::Vector3d &gyro,
+ const Eigen::Vector3d &accel, const std::optional<Eigen::Vector2d> encoders,
+ const Eigen::Vector2d voltage) {
VLOG(2) << t;
if (t_ == aos::monotonic_clock::min_time) {
t_ = t;
@@ -323,13 +322,17 @@
R.diagonal() << 1e-9, 1e-9, 1e-13;
}
- const Eigen::Matrix<double, kNModelOutputs, 1> Z(encoders(0), encoders(1),
- yaw_rate);
+ const Eigen::Matrix<double, kNModelOutputs, 1> Z =
+ encoders.has_value()
+ ? Eigen::Vector3d(encoders.value()(0), encoders.value()(1), yaw_rate)
+ : Eigen::Vector3d(current_state_.model_state(kLeftEncoder),
+ current_state_.model_state(kRightEncoder),
+ yaw_rate);
if (branches_.empty()) {
VLOG(2) << "Initializing";
- current_state_.model_state(kLeftEncoder) = encoders(0);
- current_state_.model_state(kRightEncoder) = encoders(1);
+ current_state_.model_state(kLeftEncoder) = Z(0);
+ current_state_.model_state(kRightEncoder) = Z(1);
current_state_.branch_time = t;
branches_.Push(current_state_);
}
@@ -389,7 +392,7 @@
current_state_.accel_state = branches_[0].accel_state;
current_state_.model_state = branches_[0].model_state;
current_state_.model_state = ModelStateForAccelState(
- current_state_.accel_state, encoders, yaw_rate);
+ current_state_.accel_state, Z.topRows<2>(), yaw_rate);
} else {
VLOG(2) << "Normal branching";
current_state_.accel_state =
@@ -407,14 +410,15 @@
using_model_ = true;
// Grab the model-based state from back when we stopped diverging.
current_state_.model_state.topRows<kShareStates>() =
- ModelStateForAccelState(branches_[0].accel_state, encoders, yaw_rate)
+ ModelStateForAccelState(branches_[0].accel_state, Z.topRows<2>(),
+ yaw_rate)
.topRows<kShareStates>();
current_state_.accel_state =
AccelStateForModelState(current_state_.model_state);
} else {
// TODO(james): Why was I leaving the encoders/wheel velocities in place?
current_state_.model_state = ModelStateForAccelState(
- current_state_.accel_state, encoders, yaw_rate);
+ current_state_.accel_state, Z.topRows<2>(), yaw_rate);
current_state_.branch_time = t;
}
}
@@ -449,7 +453,7 @@
VLOG(2) << "Input acce " << accel.transpose();
VLOG(2) << "Input gyro " << gyro.transpose();
VLOG(2) << "Input voltage " << voltage.transpose();
- VLOG(2) << "Input encoder " << encoders.transpose();
+ VLOG(2) << "Input encoder " << Z.topRows<2>().transpose();
VLOG(2) << "yaw rate " << yaw_rate;
CHECK(std::isfinite(last_residual_));
@@ -638,7 +642,7 @@
H_model(1, kY) = 1.0;
H_accel(0, kX) = 1.0;
H_accel(1, kY) = 1.0;
- R.diagonal() << 1e-2, 1e-2;
+ R.diagonal() << 1e-0, 1e-0;
const Eigen::Matrix<double, kNModelStates, 2> K_model =
P_model_ * H_model.transpose() *
@@ -966,9 +970,12 @@
output_fetcher_.Fetch();
for (const IMUValues *value : *values.readings()) {
zeroer_.InsertAndProcessMeasurement(*value);
- const Eigen::Vector2d encoders{
- left_encoder_.Unwrap(value->left_encoder()),
- right_encoder_.Unwrap(value->right_encoder())};
+ const std::optional<Eigen::Vector2d> encoders =
+ zeroer_.Faulted()
+ ? std::nullopt
+ : std::make_optional(Eigen::Vector2d{
+ left_encoder_.Unwrap(value->left_encoder()),
+ right_encoder_.Unwrap(value->right_encoder())});
{
const aos::monotonic_clock::time_point pico_timestamp{
std::chrono::microseconds(value->pico_timestamp_us())};
@@ -1016,8 +1023,10 @@
status_builder.add_zeroed(zeroer_.Zeroed());
status_builder.add_faulted_zero(zeroer_.Faulted());
status_builder.add_zeroing(zeroer_status);
- status_builder.add_left_encoder(encoders(0));
- status_builder.add_right_encoder(encoders(1));
+ if (encoders.has_value()) {
+ status_builder.add_left_encoder(encoders.value()(0));
+ status_builder.add_right_encoder(encoders.value()(1));
+ }
if (pico_offset_.has_value()) {
status_builder.add_pico_offset_ns(pico_offset_.value().count());
status_builder.add_pico_offset_error_ns(
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index be14b45..f2cb50e 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -110,7 +110,8 @@
const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
void HandleImu(aos::monotonic_clock::time_point t,
const Eigen::Vector3d &gyro, const Eigen::Vector3d &accel,
- const Eigen::Vector2d encoders, const Eigen::Vector2d voltage);
+ const std::optional<Eigen::Vector2d> encoders,
+ const Eigen::Vector2d voltage);
void HandleTurret(aos::monotonic_clock::time_point sample_time,
double turret_position, double turret_velocity);
void HandleImageMatch(aos::monotonic_clock::time_point sample_time,
diff --git a/y2022/localizer/localizer_replay.cc b/y2022/localizer/localizer_replay.cc
index d328948..08479eb 100644
--- a/y2022/localizer/localizer_replay.cc
+++ b/y2022/localizer/localizer_replay.cc
@@ -59,6 +59,13 @@
// open logfiles
aos::logger::LogReader reader(logfiles, &config.message());
+ reader.RemapLoggedChannel("/localizer",
+ "frc971.controls.LocalizerStatus");
+ reader.RemapLoggedChannel("/localizer",
+ "frc971.controls.LocalizerOutput");
+ reader.RemapLoggedChannel("/localizer",
+ "frc971.controls.LocalizerVisualization");
+
auto factory =
std::make_unique<aos::SimulatedEventLoopFactory>(reader.configuration());
diff --git a/y2022/localizer/localizer_test.cc b/y2022/localizer/localizer_test.cc
index 6e9cd1e..ef14972 100644
--- a/y2022/localizer/localizer_test.cc
+++ b/y2022/localizer/localizer_test.cc
@@ -784,7 +784,7 @@
event_loop_factory_.RunFor(std::chrono::seconds(4));
CHECK(status_fetcher_.Fetch());
ASSERT_TRUE(status_fetcher_->model_based()->using_model());
- EXPECT_TRUE(VerifyEstimatorAccurate(1e-1));
+ EXPECT_TRUE(VerifyEstimatorAccurate(5e-1));
ASSERT_TRUE(status_fetcher_->model_based()->has_statistics());
ASSERT_LT(10,
status_fetcher_->model_based()->statistics()->total_candidates());
@@ -832,7 +832,7 @@
event_loop_factory_.RunFor(std::chrono::seconds(4));
CHECK(status_fetcher_.Fetch());
ASSERT_TRUE(status_fetcher_->model_based()->using_model());
- EXPECT_TRUE(VerifyEstimatorAccurate(1e-1));
+ EXPECT_TRUE(VerifyEstimatorAccurate(5e-1));
ASSERT_TRUE(status_fetcher_->model_based()->has_statistics());
ASSERT_EQ(status_fetcher_->model_based()->statistics()->total_candidates(),
rejected_count +