Unify y2022 localizer into y2022/localizer folder
Somehow ended up creating multiple folders....
Change-Id: Ib8e7e69a7583379caecb9d0639042c77bba039ed
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/localizer/BUILD b/y2022/localizer/BUILD
index c2afa55..e13159f 100644
--- a/y2022/localizer/BUILD
+++ b/y2022/localizer/BUILD
@@ -1,3 +1,125 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
+load("@npm//@bazel/typescript:index.bzl", "ts_library")
+
+ts_library(
+ name = "localizer_plotter",
+ srcs = ["localizer_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ "//frc971/wpilib:imu_plot_utils",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "localizer_output_fbs",
+ srcs = [
+ "localizer_output.fbs",
+ ],
+ gen_reflections = True,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_cc_library(
+ name = "localizer_status_fbs",
+ srcs = [
+ "localizer_status.fbs",
+ ],
+ gen_reflections = True,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs_includes",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+cc_static_flatbuffer(
+ name = "localizer_schema",
+ function = "frc971::controls::LocalizerStatusSchema",
+ target = ":localizer_status_fbs_reflection_out",
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "localizer",
+ srcs = ["localizer.cc"],
+ hdrs = ["localizer.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":localizer_output_fbs",
+ ":localizer_status_fbs",
+ "//aos/containers:ring_buffer",
+ "//aos/events:event_loop",
+ "//aos/time",
+ "//frc971/control_loops:c2d",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//frc971/wpilib:imu_fbs",
+ "//frc971/zeroing:imu_zeroer",
+ "//frc971/zeroing:wrap",
+ "//y2020/vision/sift:sift_fbs",
+ "//y2022:constants",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_binary(
+ name = "localizer_main",
+ srcs = ["localizer_main.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":localizer",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//y2022/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
+cc_test(
+ name = "localizer_test",
+ srcs = ["localizer_test.cc"],
+ data = [
+ "//y2022:aos_config",
+ ],
+ shard_count = 10,
+ deps = [
+ ":localizer",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ "//frc971/control_loops/drivetrain:drivetrain_test_lib",
+ ],
+)
+
+cc_binary(
+ name = "localizer_replay",
+ srcs = ["localizer_replay.cc"],
+ data = [
+ "//y2022:aos_config",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":localizer",
+ ":localizer_schema",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/events/logging:log_writer",
+ "//y2022/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
cc_library(
name = "imu",
srcs = [
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
new file mode 100644
index 0000000..8d4f908
--- /dev/null
+++ b/y2022/localizer/localizer.cc
@@ -0,0 +1,629 @@
+#include "y2022/localizer/localizer.h"
+
+#include "frc971/control_loops/c2d.h"
+#include "frc971/wpilib/imu_batch_generated.h"
+#include "y2022/constants.h"
+
+namespace frc971::controls {
+
+namespace {
+constexpr double kG = 9.80665;
+constexpr std::chrono::microseconds kNominalDt(500);
+
+template <int N>
+Eigen::Matrix<double, N, 1> MakeState(std::vector<double> values) {
+ CHECK_EQ(static_cast<size_t>(N), values.size());
+ Eigen::Matrix<double, N, 1> vector;
+ for (int ii = 0; ii < N; ++ii) {
+ vector(ii, 0) = values[ii];
+ }
+ return vector;
+}
+} // namespace
+
+ModelBasedLocalizer::ModelBasedLocalizer(
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
+ : dt_config_(dt_config),
+ velocity_drivetrain_coefficients_(
+ dt_config.make_hybrid_drivetrain_velocity_loop()
+ .plant()
+ .coefficients()),
+ down_estimator_(dt_config) {
+ CHECK_EQ(branches_.capacity(), static_cast<size_t>(std::chrono::seconds(1) /
+ kNominalDt / kBranchPeriod));
+ if (dt_config_.is_simulated) {
+ down_estimator_.assume_perfect_gravity();
+ }
+ A_continuous_accel_.setZero();
+ A_continuous_model_.setZero();
+ B_continuous_accel_.setZero();
+ B_continuous_model_.setZero();
+
+ A_continuous_accel_(kX, kVelocityX) = 1.0;
+ A_continuous_accel_(kY, kVelocityY) = 1.0;
+
+ const double diameter = 2.0 * dt_config_.robot_radius;
+
+ A_continuous_model_(kTheta, kLeftVelocity) = -1.0 / diameter;
+ A_continuous_model_(kTheta, kRightVelocity) = 1.0 / diameter;
+ A_continuous_model_(kLeftEncoder, kLeftVelocity) = 1.0;
+ A_continuous_model_(kRightEncoder, kRightVelocity) = 1.0;
+ const auto &vel_coefs = velocity_drivetrain_coefficients_;
+ A_continuous_model_(kLeftVelocity, kLeftVelocity) =
+ vel_coefs.A_continuous(0, 0);
+ A_continuous_model_(kLeftVelocity, kRightVelocity) =
+ vel_coefs.A_continuous(0, 1);
+ A_continuous_model_(kRightVelocity, kLeftVelocity) =
+ vel_coefs.A_continuous(1, 0);
+ A_continuous_model_(kRightVelocity, kRightVelocity) =
+ vel_coefs.A_continuous(1, 1);
+
+ A_continuous_model_(kLeftVelocity, kLeftVoltageError) =
+ 1 * vel_coefs.B_continuous(0, 0);
+ A_continuous_model_(kLeftVelocity, kRightVoltageError) =
+ 1 * vel_coefs.B_continuous(0, 1);
+ A_continuous_model_(kRightVelocity, kLeftVoltageError) =
+ 1 * vel_coefs.B_continuous(1, 0);
+ A_continuous_model_(kRightVelocity, kRightVoltageError) =
+ 1 * vel_coefs.B_continuous(1, 1);
+
+ B_continuous_model_.block<1, 2>(kLeftVelocity, kLeftVoltage) =
+ vel_coefs.B_continuous.row(0);
+ B_continuous_model_.block<1, 2>(kRightVelocity, kLeftVoltage) =
+ vel_coefs.B_continuous.row(1);
+
+ B_continuous_accel_(kVelocityX, kAccelX) = 1.0;
+ B_continuous_accel_(kVelocityY, kAccelY) = 1.0;
+ B_continuous_accel_(kTheta, kThetaRate) = 1.0;
+
+ Q_continuous_model_.setZero();
+ Q_continuous_model_.diagonal() << 1e-4, 1e-4, 1e-4, 1e-2, 1e-0, 1e-0, 1e-2,
+ 1e-0, 1e-0;
+
+ P_model_ = Q_continuous_model_ * aos::time::DurationInSeconds(kNominalDt);
+}
+
+Eigen::Matrix<double, ModelBasedLocalizer::kNModelStates,
+ ModelBasedLocalizer::kNModelStates>
+ModelBasedLocalizer::AModel(
+ const ModelBasedLocalizer::ModelState &state) const {
+ Eigen::Matrix<double, kNModelStates, kNModelStates> A = A_continuous_model_;
+ const double theta = state(kTheta);
+ const double stheta = std::sin(theta);
+ const double ctheta = std::cos(theta);
+ const double velocity = (state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
+ A(kX, kTheta) = -stheta * velocity;
+ A(kX, kLeftVelocity) = ctheta / 2.0;
+ A(kX, kRightVelocity) = ctheta / 2.0;
+ A(kY, kTheta) = ctheta * velocity;
+ A(kY, kLeftVelocity) = stheta / 2.0;
+ A(kY, kRightVelocity) = stheta / 2.0;
+ return A;
+}
+
+Eigen::Matrix<double, ModelBasedLocalizer::kNAccelStates,
+ ModelBasedLocalizer::kNAccelStates>
+ModelBasedLocalizer::AAccel() const {
+ return A_continuous_accel_;
+}
+
+ModelBasedLocalizer::ModelState ModelBasedLocalizer::DiffModel(
+ const ModelBasedLocalizer::ModelState &state,
+ const ModelBasedLocalizer::ModelInput &U) const {
+ ModelState x_dot = AModel(state) * state + B_continuous_model_ * U;
+ const double theta = state(kTheta);
+ const double stheta = std::sin(theta);
+ const double ctheta = std::cos(theta);
+ const double velocity = (state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
+ x_dot(kX) = ctheta * velocity;
+ x_dot(kY) = stheta * velocity;
+ return x_dot;
+}
+
+ModelBasedLocalizer::AccelState ModelBasedLocalizer::DiffAccel(
+ const ModelBasedLocalizer::AccelState &state,
+ const ModelBasedLocalizer::AccelInput &U) const {
+ return AAccel() * state + B_continuous_accel_ * U;
+}
+
+ModelBasedLocalizer::ModelState ModelBasedLocalizer::UpdateModel(
+ const ModelBasedLocalizer::ModelState &model,
+ const ModelBasedLocalizer::ModelInput &input,
+ const aos::monotonic_clock::duration dt) const {
+ return control_loops::RungeKutta(
+ std::bind(&ModelBasedLocalizer::DiffModel, this, std::placeholders::_1,
+ input),
+ model, aos::time::DurationInSeconds(dt));
+}
+
+ModelBasedLocalizer::AccelState ModelBasedLocalizer::UpdateAccel(
+ const ModelBasedLocalizer::AccelState &accel,
+ const ModelBasedLocalizer::AccelInput &input,
+ const aos::monotonic_clock::duration dt) const {
+ return control_loops::RungeKutta(
+ std::bind(&ModelBasedLocalizer::DiffAccel, this, std::placeholders::_1,
+ input),
+ accel, aos::time::DurationInSeconds(dt));
+}
+
+ModelBasedLocalizer::AccelState ModelBasedLocalizer::AccelStateForModelState(
+ const ModelBasedLocalizer::ModelState &state) const {
+ const double robot_speed =
+ (state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
+ const double lat_speed = (AModel(state) * state)(kTheta) * long_offset_;
+ const double velocity_x = std::cos(state(kTheta)) * robot_speed -
+ std::sin(state(kTheta)) * lat_speed;
+ const double velocity_y = std::sin(state(kTheta)) * robot_speed +
+ std::cos(state(kTheta)) * lat_speed;
+ return (AccelState() << state(0), state(1), state(2), velocity_x, velocity_y)
+ .finished();
+}
+
+ModelBasedLocalizer::ModelState ModelBasedLocalizer::ModelStateForAccelState(
+ const ModelBasedLocalizer::AccelState &state,
+ const Eigen::Vector2d &encoders, const double yaw_rate) const {
+ const double robot_speed = state(kVelocityX) * std::cos(state(kTheta)) +
+ state(kVelocityY) * std::sin(state(kTheta));
+ const double radius = dt_config_.robot_radius;
+ const double left_velocity = robot_speed - yaw_rate * radius;
+ const double right_velocity = robot_speed + yaw_rate * radius;
+ return (ModelState() << state(0), state(1), state(2), encoders(0),
+ left_velocity, 0.0, encoders(1), right_velocity, 0.0)
+ .finished();
+}
+
+double ModelBasedLocalizer::ModelDivergence(
+ const ModelBasedLocalizer::CombinedState &state,
+ const ModelBasedLocalizer::AccelInput &accel_inputs,
+ const Eigen::Vector2d &filtered_accel,
+ const ModelBasedLocalizer::ModelInput &model_inputs) {
+ // Convert the model state into the acceleration-based state-space and check
+ // the distance between the two (should really be a weighted norm, but all the
+ // numbers are on ~the same scale).
+ // TODO(james): Maybe weight lateral velocity divergence different than
+ // longitudinal? Seems like we tend to get false-positives currently when in
+ // sharp turns.
+ // TODO(james): For off-center gyros, maybe reduce noise when turning?
+ VLOG(2) << "divergence: "
+ << (state.accel_state - AccelStateForModelState(state.model_state))
+ .transpose();
+ const AccelState diff_accel = DiffAccel(state.accel_state, accel_inputs);
+ const ModelState diff_model = DiffModel(state.model_state, model_inputs);
+ const double model_lng_velocity =
+ (state.model_state(kLeftVelocity) + state.model_state(kRightVelocity)) /
+ 2.0;
+ const double model_lng_accel =
+ (diff_model(kLeftVelocity) + diff_model(kRightVelocity)) / 2.0 -
+ diff_model(kTheta) * diff_model(kTheta) * long_offset_;
+ const double model_lat_accel = diff_model(kTheta) * model_lng_velocity;
+ const Eigen::Vector2d robot_frame_accel(model_lng_accel, model_lat_accel);
+ const Eigen::Vector2d model_accel =
+ Eigen::AngleAxisd(state.model_state(kTheta), Eigen::Vector3d::UnitZ())
+ .toRotationMatrix()
+ .block<2, 2>(0, 0) *
+ robot_frame_accel;
+ const double accel_diff = (model_accel - filtered_accel).norm();
+ const double theta_rate_diff =
+ std::abs(diff_accel(kTheta) - diff_model(kTheta));
+
+ const Eigen::Vector2d accel_vel = state.accel_state.bottomRows<2>();
+ Eigen::Vector2d model_vel =
+ AccelStateForModelState(state.model_state).bottomRows<2>();
+ velocity_residual_ = (accel_vel - model_vel).norm() /
+ (1.0 + accel_vel.norm() + model_vel.norm());
+ theta_rate_residual_ = theta_rate_diff;
+ accel_residual_ = accel_diff / 4.0;
+ return velocity_residual_ + theta_rate_residual_ + accel_residual_;
+}
+
+void ModelBasedLocalizer::UpdateState(
+ CombinedState *state,
+ const Eigen::Matrix<double, kNModelStates, kNModelOutputs> &K,
+ const Eigen::Matrix<double, kNModelOutputs, 1> &Z,
+ const Eigen::Matrix<double, kNModelOutputs, kNModelStates> &H,
+ const AccelInput &accel_input, const ModelInput &model_input,
+ aos::monotonic_clock::duration dt) {
+ state->accel_state = UpdateAccel(state->accel_state, accel_input, dt);
+ if (down_estimator_.consecutive_still() > 500.0) {
+ state->accel_state(kVelocityX) *= 0.9;
+ state->accel_state(kVelocityY) *= 0.9;
+ }
+ state->model_state = UpdateModel(state->model_state, model_input, dt);
+ 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) {
+ VLOG(2) << t;
+ if (t_ == aos::monotonic_clock::min_time) {
+ t_ = t;
+ }
+ if (t_ + 2 * kNominalDt < t) {
+ t_ = t;
+ ++clock_resets_;
+ }
+ const aos::monotonic_clock::duration dt = t - t_;
+ t_ = t;
+ down_estimator_.Predict(gyro, accel, dt);
+ // TODO(james): Should we prefer this or use the down-estimator corrected
+ // version? Using the down estimator is more principled, but does create more
+ // opportunities for subtle biases.
+ const double yaw_rate = (dt_config_.imu_transform * gyro)(2);
+ const double diameter = 2.0 * dt_config_.robot_radius;
+
+ const Eigen::AngleAxis<double> orientation(
+ Eigen::AngleAxis<double>(xytheta()(kTheta), Eigen::Vector3d::UnitZ()) *
+ down_estimator_.X_hat());
+ last_orientation_ = orientation;
+
+ const Eigen::Vector3d absolute_accel =
+ orientation * dt_config_.imu_transform * kG * accel;
+ abs_accel_ = absolute_accel;
+
+ VLOG(2) << "abs accel " << absolute_accel.transpose();
+ VLOG(2) << "dt " << aos::time::DurationInSeconds(dt);
+
+ // Update all the branched states.
+ const AccelInput accel_input(absolute_accel.x(), absolute_accel.y(),
+ yaw_rate);
+ const ModelInput model_input(voltage);
+
+ const Eigen::Matrix<double, kNModelStates, kNModelStates> A_continuous =
+ AModel(current_state_.model_state);
+
+ Eigen::Matrix<double, kNModelStates, kNModelStates> A_discrete;
+ Eigen::Matrix<double, kNModelStates, kNModelStates> Q_discrete;
+
+ DiscretizeQAFast(Q_continuous_model_, A_continuous, dt, &Q_discrete,
+ &A_discrete);
+
+ P_model_ = A_discrete * P_model_ * A_discrete.transpose() + Q_discrete;
+
+ Eigen::Matrix<double, kNModelOutputs, kNModelStates> H;
+ Eigen::Matrix<double, kNModelOutputs, kNModelOutputs> R;
+ {
+ H.setZero();
+ R.setZero();
+ H(0, kLeftEncoder) = 1.0;
+ H(1, kRightEncoder) = 1.0;
+ H(2, kRightVelocity) = 1.0 / diameter;
+ H(2, kLeftVelocity) = -1.0 / diameter;
+
+ R.diagonal() << 1e-9, 1e-9, 1e-13;
+ }
+
+ const Eigen::Matrix<double, kNModelOutputs, 1> Z(encoders(0), encoders(1),
+ yaw_rate);
+
+ if (branches_.empty()) {
+ VLOG(2) << "Initializing";
+ current_state_.model_state.setZero();
+ current_state_.accel_state.setZero();
+ current_state_.model_state(kLeftEncoder) = encoders(0);
+ current_state_.model_state(kRightEncoder) = encoders(1);
+ current_state_.branch_time = t;
+ branches_.Push(current_state_);
+ }
+
+ const Eigen::Matrix<double, kNModelStates, kNModelOutputs> K =
+ P_model_ * H.transpose() * (H * P_model_ * H.transpose() + R).inverse();
+ P_model_ = (Eigen::Matrix<double, kNModelStates, kNModelStates>::Identity() -
+ K * H) *
+ P_model_;
+ VLOG(2) << "K\n" << K;
+ VLOG(2) << "Z\n" << Z.transpose();
+
+ for (CombinedState &state : branches_) {
+ UpdateState(&state, K, Z, H, accel_input, model_input, dt);
+ }
+ UpdateState(¤t_state_, K, Z, H, accel_input, model_input, dt);
+
+ VLOG(2) << "oildest accel " << branches_[0].accel_state.transpose();
+ VLOG(2) << "oildest accel diff "
+ << DiffAccel(branches_[0].accel_state, accel_input).transpose();
+ VLOG(2) << "oildest model " << branches_[0].model_state.transpose();
+
+ // Determine whether to switch modes--if we are currently in model-based mode,
+ // swap to accel-based if the two states have divergeed meaningfully in the
+ // oldest branch. If we are currently in accel-based, then swap back to model
+ // if the oldest model branch matches has matched the
+ filtered_residual_accel_ +=
+ 0.01 * (accel_input.topRows<2>() - filtered_residual_accel_);
+ const double model_divergence =
+ branches_.full() ? ModelDivergence(branches_[0], accel_input,
+ filtered_residual_accel_, model_input)
+ : 0.0;
+ filtered_residual_ +=
+ (1.0 - std::exp(-aos::time::DurationInSeconds(kNominalDt) / 0.0095)) *
+ (model_divergence - filtered_residual_);
+ // TODO(james): Tune this more. Currently set to generally trust the model,
+ // perhaps a bit too much.
+ // When the residual exceeds the accel threshold, we start using the inertials
+ // alone; when it drops back below the model threshold, we go back to being
+ // model-based.
+ constexpr double kUseAccelThreshold = 2.0;
+ constexpr double kUseModelThreshold = 0.5;
+ constexpr size_t kShareStates = kNModelStates;
+ static_assert(kUseModelThreshold < kUseAccelThreshold);
+ if (using_model_) {
+ if (filtered_residual_ > kUseAccelThreshold) {
+ hysteresis_count_++;
+ } else {
+ hysteresis_count_ = 0;
+ }
+ if (hysteresis_count_ > 0) {
+ using_model_ = false;
+ // Grab the accel-based state from back when we started diverging.
+ // TODO(james): This creates a problematic selection bias, because
+ // we will tend to bias towards deliberately out-of-tune measurements.
+ 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);
+ } else {
+ VLOG(2) << "Normal branching";
+ current_state_.accel_state =
+ AccelStateForModelState(current_state_.model_state);
+ current_state_.branch_time = t;
+ }
+ hysteresis_count_ = 0;
+ } else {
+ if (filtered_residual_ < kUseModelThreshold) {
+ hysteresis_count_++;
+ } else {
+ hysteresis_count_ = 0;
+ }
+ if (hysteresis_count_ > 100) {
+ 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)
+ .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_.branch_time = t;
+ }
+ }
+
+ // Generate a new branch, with the accel state reset based on the model-based
+ // state (really, just getting rid of the lateral velocity).
+ // By resetting the accel state in the new branch, this tries to minimize the
+ // odds of runaway lateral velocities. This doesn't help with runaway
+ // longitudinal velocities, however.
+ CombinedState new_branch = current_state_;
+ new_branch.accel_state = AccelStateForModelState(new_branch.model_state);
+ new_branch.accumulated_divergence = 0.0;
+
+ ++branch_counter_;
+ if (branch_counter_ % kBranchPeriod == 0) {
+ branches_.Push(new_branch);
+ branch_counter_ = 0;
+ }
+
+ last_residual_ = model_divergence;
+
+ VLOG(2) << "Using " << (using_model_ ? "model" : "accel");
+ VLOG(2) << "Residual " << last_residual_;
+ VLOG(2) << "Filtered Residual " << filtered_residual_;
+ VLOG(2) << "buffer size " << branches_.size();
+ VLOG(2) << "Model state " << current_state_.model_state.transpose();
+ VLOG(2) << "Accel state " << current_state_.accel_state.transpose();
+ VLOG(2) << "Accel state for model "
+ << AccelStateForModelState(current_state_.model_state).transpose();
+ 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) << "yaw rate " << yaw_rate;
+
+ CHECK(std::isfinite(last_residual_));
+}
+
+void ModelBasedLocalizer::HandleReset(aos::monotonic_clock::time_point now,
+ const Eigen::Vector3d &xytheta) {
+ branches_.Reset();
+ t_ = now;
+ using_model_ = true;
+ current_state_.model_state << xytheta(0), xytheta(1), xytheta(2),
+ current_state_.model_state(kLeftEncoder), 0.0, 0.0,
+ current_state_.model_state(kRightEncoder), 0.0, 0.0;
+ current_state_.accel_state =
+ AccelStateForModelState(current_state_.model_state);
+ last_residual_ = 0.0;
+ filtered_residual_ = 0.0;
+ filtered_residual_accel_.setZero();
+ abs_accel_.setZero();
+}
+
+flatbuffers::Offset<AccelBasedState> ModelBasedLocalizer::BuildAccelState(
+ flatbuffers::FlatBufferBuilder *fbb, const AccelState &state) {
+ AccelBasedState::Builder accel_state_builder(*fbb);
+ accel_state_builder.add_x(state(kX));
+ accel_state_builder.add_y(state(kY));
+ accel_state_builder.add_theta(state(kTheta));
+ accel_state_builder.add_velocity_x(state(kVelocityX));
+ accel_state_builder.add_velocity_y(state(kVelocityY));
+ return accel_state_builder.Finish();
+}
+
+flatbuffers::Offset<ModelBasedState> ModelBasedLocalizer::BuildModelState(
+ flatbuffers::FlatBufferBuilder *fbb, const ModelState &state) {
+ ModelBasedState::Builder model_state_builder(*fbb);
+ model_state_builder.add_x(state(kX));
+ model_state_builder.add_y(state(kY));
+ model_state_builder.add_theta(state(kTheta));
+ model_state_builder.add_left_encoder(state(kLeftEncoder));
+ model_state_builder.add_left_velocity(state(kLeftVelocity));
+ model_state_builder.add_left_voltage_error(state(kLeftVoltageError));
+ model_state_builder.add_right_encoder(state(kRightEncoder));
+ model_state_builder.add_right_velocity(state(kRightVelocity));
+ model_state_builder.add_right_voltage_error(state(kRightVoltageError));
+ return model_state_builder.Finish();
+}
+
+flatbuffers::Offset<ModelBasedStatus> ModelBasedLocalizer::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const flatbuffers::Offset<control_loops::drivetrain::DownEstimatorState>
+ down_estimator_offset = down_estimator_.PopulateStatus(fbb, t_);
+
+ const CombinedState &state = current_state_;
+
+ const flatbuffers::Offset<ModelBasedState> model_state_offset =
+ BuildModelState(fbb, state.model_state);
+
+ const flatbuffers::Offset<AccelBasedState> accel_state_offset =
+ BuildAccelState(fbb, state.accel_state);
+
+ const flatbuffers::Offset<AccelBasedState> oldest_accel_state_offset =
+ branches_.empty() ? flatbuffers::Offset<AccelBasedState>()
+ : BuildAccelState(fbb, branches_[0].accel_state);
+
+ const flatbuffers::Offset<ModelBasedState> oldest_model_state_offset =
+ branches_.empty() ? flatbuffers::Offset<ModelBasedState>()
+ : BuildModelState(fbb, branches_[0].model_state);
+
+ ModelBasedStatus::Builder builder(*fbb);
+ builder.add_accel_state(accel_state_offset);
+ builder.add_oldest_accel_state(oldest_accel_state_offset);
+ builder.add_oldest_model_state(oldest_model_state_offset);
+ builder.add_model_state(model_state_offset);
+ builder.add_using_model(using_model_);
+ builder.add_residual(last_residual_);
+ builder.add_filtered_residual(filtered_residual_);
+ builder.add_velocity_residual(velocity_residual_);
+ builder.add_accel_residual(accel_residual_);
+ builder.add_theta_rate_residual(theta_rate_residual_);
+ builder.add_down_estimator(down_estimator_offset);
+ builder.add_x(xytheta()(0));
+ builder.add_y(xytheta()(1));
+ builder.add_theta(xytheta()(2));
+ builder.add_implied_accel_x(abs_accel_(0));
+ builder.add_implied_accel_y(abs_accel_(1));
+ builder.add_implied_accel_z(abs_accel_(2));
+ builder.add_clock_resets(clock_resets_);
+ return builder.Finish();
+}
+
+namespace {
+// Period at which the encoder readings from the IMU board wrap.
+static double DrivetrainWrapPeriod() {
+ return y2022::constants::Values::DrivetrainEncoderToMeters(1 << 16);
+}
+}
+
+EventLoopLocalizer::EventLoopLocalizer(
+ aos::EventLoop *event_loop,
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
+ : event_loop_(event_loop),
+ model_based_(dt_config),
+ status_sender_(event_loop_->MakeSender<LocalizerStatus>("/localizer")),
+ output_sender_(event_loop_->MakeSender<LocalizerOutput>("/localizer")),
+ output_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ left_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()),
+ right_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()) {
+ event_loop_->MakeWatcher(
+ "/drivetrain",
+ [this](
+ const frc971::control_loops::drivetrain::LocalizerControl &control) {
+ const double theta = control.keep_current_theta()
+ ? model_based_.xytheta()(2)
+ : control.theta();
+ model_based_.HandleReset(event_loop_->monotonic_now(),
+ {control.x(), control.y(), theta});
+ });
+ event_loop_->MakeWatcher(
+ "/localizer", [this](const frc971::IMUValuesBatch &values) {
+ CHECK(values.has_readings());
+ 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())};
+ if (zeroer_.Zeroed()) {
+ const aos::monotonic_clock::time_point pico_timestamp{
+ std::chrono::microseconds(value->pico_timestamp_us())};
+ // TODO(james): If we get large enough drift off of the pico,
+ // actually do something about it.
+ if (!pico_offset_.has_value()) {
+ pico_offset_ =
+ event_loop_->context().monotonic_event_time - pico_timestamp;
+ last_pico_timestamp_ = pico_timestamp;
+ }
+ if (pico_timestamp < last_pico_timestamp_) {
+ pico_offset_.value() += std::chrono::microseconds(1ULL << 32);
+ }
+ const aos::monotonic_clock::time_point sample_timestamp =
+ pico_offset_.value() + pico_timestamp;
+ pico_offset_error_ =
+ event_loop_->context().monotonic_event_time - sample_timestamp;
+ const bool disabled =
+ (output_fetcher_.get() == nullptr) ||
+ (output_fetcher_.context().monotonic_event_time +
+ std::chrono::milliseconds(10) <
+ event_loop_->context().monotonic_event_time);
+ model_based_.HandleImu(
+ sample_timestamp,
+ zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(), encoders,
+ disabled ? Eigen::Vector2d::Zero()
+ : Eigen::Vector2d{output_fetcher_->left_voltage(),
+ output_fetcher_->right_voltage()});
+ last_pico_timestamp_ = pico_timestamp;
+ }
+ {
+ auto builder = status_sender_.MakeBuilder();
+ const flatbuffers::Offset<ModelBasedStatus> model_based_status =
+ model_based_.PopulateStatus(builder.fbb());
+ const flatbuffers::Offset<control_loops::drivetrain::ImuZeroerState>
+ zeroer_status = zeroer_.PopulateStatus(builder.fbb());
+ LocalizerStatus::Builder status_builder =
+ builder.MakeBuilder<LocalizerStatus>();
+ status_builder.add_model_based(model_based_status);
+ 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 (pico_offset_.has_value()) {
+ status_builder.add_pico_offset_ns(pico_offset_.value().count());
+ status_builder.add_pico_offset_error_ns(
+ pico_offset_error_.count());
+ }
+ builder.CheckOk(builder.Send(status_builder.Finish()));
+ }
+ if (last_output_send_ + std::chrono::milliseconds(5) <
+ event_loop_->context().monotonic_event_time) {
+ auto builder = output_sender_.MakeBuilder();
+ LocalizerOutput::Builder output_builder =
+ builder.MakeBuilder<LocalizerOutput>();
+ // TODO(james): Should we bother to try to estimate time offsets for
+ // the pico?
+ output_builder.add_monotonic_timestamp_ns(
+ value->monotonic_timestamp_ns());
+ output_builder.add_x(model_based_.xytheta()(0));
+ output_builder.add_y(model_based_.xytheta()(1));
+ output_builder.add_theta(model_based_.xytheta()(2));
+ const Eigen::Quaterniond &orientation = model_based_.orientation();
+ Quaternion quaternion;
+ quaternion.mutate_x(orientation.x());
+ quaternion.mutate_y(orientation.y());
+ quaternion.mutate_z(orientation.z());
+ quaternion.mutate_w(orientation.w());
+ output_builder.add_orientation(&quaternion);
+ builder.CheckOk(builder.Send(output_builder.Finish()));
+ last_output_send_ = event_loop_->monotonic_now();
+ }
+ }
+ });
+}
+
+} // namespace frc971::controls
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
new file mode 100644
index 0000000..d7b6a23
--- /dev/null
+++ b/y2022/localizer/localizer.h
@@ -0,0 +1,248 @@
+#ifndef Y2022_LOCALIZER_LOCALIZER_H_
+#define Y2022_LOCALIZER_LOCALIZER_H_
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+
+#include "aos/events/event_loop.h"
+#include "aos/containers/ring_buffer.h"
+#include "aos/time/time.h"
+#include "y2020/vision/sift/sift_generated.h"
+#include "y2022/localizer/localizer_status_generated.h"
+#include "y2022/localizer/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/zeroing/imu_zeroer.h"
+#include "frc971/zeroing/wrap.h"
+
+namespace frc971::controls {
+
+namespace testing {
+class LocalizerTest;
+}
+
+// Localizer implementation that makes use of a 6-axis IMU, encoder readings,
+// drivetrain voltages, and camera returns to localize the robot. Meant to
+// be run on a raspberry pi.
+//
+// This operates on the principle that the drivetrain can be in one of two
+// modes:
+// 1) A "normal" mode where it is obeying the regular drivetrain model, with
+// minimal lateral motion and no major external disturbances. This is
+// referred to as the "model" mode in the code/variable names.
+// 2) An non-holonomic mode where the robot is just flying around on a 2-D
+// plane with no meaningful constraints (referred to as an "accel" model
+// in the code, because we rely primarily on the accelerometer readings).
+//
+// In order to determine which mode to be in, we attempt to track whether the
+// two models are diverging substantially. In order to do this, we maintain a
+// 1-second long queue of "branches". A branch is generated every X iterations
+// and contains a model state and an accel state. When the branch starts, the
+// two will have identical states. For the remaining 1 second, the model state
+// will evolve purely according to the drivetrian model, and the accel state
+// will evolve purely using IMU readings.
+//
+// When the branch reaches 1 second in age, we calculate a residual associated
+// with how much the accel and model based states diverged. If they have
+// diverged substantially, that implies that the model is a poor match for
+// whatever has been happening to the robot in the past second, so if we were
+// previously relying on the model, we will override the current "actual"
+// state with the branched accel state, and then continue to update the accel
+// state based on IMU readings.
+// If we are currently in the accel state, we will continue generating branches
+// until the branches stop diverging--this will indicate that the model
+// matches the accelerometer readings again, and so we will swap back to
+// the model-based state.
+//
+// TODO:
+// * Implement paying attention to camera readings.
+// * Tune for ADIS16505/real robot.
+class ModelBasedLocalizer {
+ public:
+ static constexpr size_t kX = 0;
+ static constexpr size_t kY = 1;
+ static constexpr size_t kTheta = 2;
+
+ static constexpr size_t kVelocityX = 3;
+ static constexpr size_t kVelocityY = 4;
+ static constexpr size_t kNAccelStates = 5;
+
+ static constexpr size_t kLeftEncoder = 3;
+ static constexpr size_t kLeftVelocity = 4;
+ static constexpr size_t kLeftVoltageError = 5;
+ static constexpr size_t kRightEncoder = 6;
+ static constexpr size_t kRightVelocity = 7;
+ static constexpr size_t kRightVoltageError = 8;
+ static constexpr size_t kNModelStates = 9;
+
+ static constexpr size_t kNModelOutputs = 3;
+
+ static constexpr size_t kNAccelOuputs = 1;
+
+ static constexpr size_t kAccelX = 0;
+ static constexpr size_t kAccelY = 1;
+ static constexpr size_t kThetaRate = 2;
+ static constexpr size_t kNAccelInputs = 3;
+
+ static constexpr size_t kLeftVoltage = 0;
+ static constexpr size_t kRightVoltage = 1;
+ static constexpr size_t kNModelInputs = 2;
+
+ // Branching period, in cycles.
+ // Needs 10 to even stay alive, and still at ~96% CPU.
+ // ~20 gives ~55-60% CPU.
+ static constexpr int kBranchPeriod = 20;
+
+ typedef Eigen::Matrix<double, kNModelStates, 1> ModelState;
+ typedef Eigen::Matrix<double, kNAccelStates, 1> AccelState;
+ typedef Eigen::Matrix<double, kNModelInputs, 1> ModelInput;
+ typedef Eigen::Matrix<double, kNAccelInputs, 1> AccelInput;
+
+ ModelBasedLocalizer(
+ 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);
+ void HandleImageMatch(aos::monotonic_clock::time_point,
+ const vision::sift::ImageMatchResult *) {
+ LOG(FATAL) << "Unimplemented.";
+ }
+ void HandleReset(aos::monotonic_clock::time_point,
+ const Eigen::Vector3d &xytheta);
+
+ flatbuffers::Offset<ModelBasedStatus> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ Eigen::Vector3d xytheta() const {
+ if (using_model_) {
+ return current_state_.model_state.block<3, 1>(0, 0);
+ } else {
+ return current_state_.accel_state.block<3, 1>(0, 0);
+ }
+ }
+
+ Eigen::Quaterniond orientation() const { return last_orientation_; }
+
+ AccelState accel_state() const { return current_state_.accel_state; };
+
+ void set_longitudinal_offset(double offset) { long_offset_ = offset; }
+
+ private:
+ struct CombinedState {
+ AccelState accel_state;
+ ModelState model_state;
+ aos::monotonic_clock::time_point branch_time;
+ double accumulated_divergence;
+ };
+
+ static flatbuffers::Offset<AccelBasedState> BuildAccelState(
+ flatbuffers::FlatBufferBuilder *fbb, const AccelState &state);
+
+ static flatbuffers::Offset<ModelBasedState> BuildModelState(
+ flatbuffers::FlatBufferBuilder *fbb, const ModelState &state);
+
+ Eigen::Matrix<double, kNModelStates, kNModelStates> AModel(
+ const ModelState &state) const;
+ Eigen::Matrix<double, kNAccelStates, kNAccelStates> AAccel() const;
+ ModelState DiffModel(const ModelState &state, const ModelInput &U) const;
+ AccelState DiffAccel(const AccelState &state, const AccelInput &U) const;
+
+ ModelState UpdateModel(const ModelState &model, const ModelInput &input,
+ aos::monotonic_clock::duration dt) const;
+ AccelState UpdateAccel(const AccelState &accel, const AccelInput &input,
+ aos::monotonic_clock::duration dt) const;
+
+ AccelState AccelStateForModelState(const ModelState &state) const;
+ ModelState ModelStateForAccelState(const AccelState &state,
+ const Eigen::Vector2d &encoders,
+ const double yaw_rate) const;
+ double ModelDivergence(const CombinedState &state,
+ const AccelInput &accel_inputs,
+ const Eigen::Vector2d &filtered_accel,
+ const ModelInput &model_inputs);
+ void UpdateState(
+ CombinedState *state,
+ const Eigen::Matrix<double, kNModelStates, kNModelOutputs> &K,
+ const Eigen::Matrix<double, kNModelOutputs, 1> &Z,
+ const Eigen::Matrix<double, kNModelOutputs, kNModelStates> &H,
+ const AccelInput &accel_input, const ModelInput &model_input,
+ aos::monotonic_clock::duration dt);
+
+ const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ const StateFeedbackHybridPlantCoefficients<2, 2, 2, double>
+ velocity_drivetrain_coefficients_;
+ Eigen::Matrix<double, kNModelStates, kNModelStates> A_continuous_model_;
+ Eigen::Matrix<double, kNAccelStates, kNAccelStates> A_continuous_accel_;
+ Eigen::Matrix<double, kNModelStates, kNModelInputs> B_continuous_model_;
+ Eigen::Matrix<double, kNAccelStates, kNAccelInputs> B_continuous_accel_;
+
+ Eigen::Matrix<double, kNModelStates, kNModelStates> Q_continuous_model_;
+
+ Eigen::Matrix<double, kNModelStates, kNModelStates> P_model_;
+ // When we are following the model, we will, on each iteration:
+ // 1) Perform a model-based update of a single state.
+ // 2) Add a hypothetical non-model-based entry based on the current state.
+ // 3) Evict too-old non-model-based entries.
+ control_loops::drivetrain::DrivetrainUkf down_estimator_;
+
+ // Buffer of old branches these are all created by initializing a new
+ // model-based state based on the current state, and then initializing a new
+ // accel-based state on top of that new model-based state (to eliminate the
+ // impact of any lateral motion).
+ // We then integrate up all of these states and observe how much the model and
+ // accel based states of each branch compare to one another.
+ aos::RingBuffer<CombinedState, 2000 / kBranchPeriod> branches_;
+ int branch_counter_ = 0;
+
+ CombinedState current_state_;
+ aos::monotonic_clock::time_point t_ = aos::monotonic_clock::min_time;
+ bool using_model_;
+
+ // X position of the IMU, in meters. 0 = center of robot, positive = ahead of
+ // center, negative = behind center.
+ double long_offset_ = -0.15;
+
+ double last_residual_ = 0.0;
+ double filtered_residual_ = 0.0;
+ Eigen::Vector2d filtered_residual_accel_ = Eigen::Vector2d::Zero();
+ Eigen::Vector3d abs_accel_ = Eigen::Vector3d::Zero();
+ double velocity_residual_ = 0.0;
+ double accel_residual_ = 0.0;
+ double theta_rate_residual_ = 0.0;
+ int hysteresis_count_ = 0;
+ Eigen::Quaterniond last_orientation_ = Eigen::Quaterniond::Identity();
+
+ int clock_resets_ = 0;
+
+ friend class testing::LocalizerTest;
+};
+
+class EventLoopLocalizer {
+ public:
+ EventLoopLocalizer(
+ aos::EventLoop *event_loop,
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
+
+ ModelBasedLocalizer *localizer() { return &model_based_; }
+
+ private:
+ aos::EventLoop *event_loop_;
+ ModelBasedLocalizer model_based_;
+ aos::Sender<LocalizerStatus> status_sender_;
+ aos::Sender<LocalizerOutput> output_sender_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
+ zeroing::ImuZeroer zeroer_;
+ aos::monotonic_clock::time_point last_output_send_ =
+ aos::monotonic_clock::min_time;
+ std::optional<aos::monotonic_clock::time_point> last_pico_timestamp_;
+ aos::monotonic_clock::duration pico_offset_error_;
+ // t = pico_offset_ + pico_timestamp.
+ // Note that this can drift over sufficiently long time periods!
+ std::optional<std::chrono::nanoseconds> pico_offset_;
+
+ zeroing::UnwrapSensor left_encoder_;
+ zeroing::UnwrapSensor right_encoder_;
+};
+} // namespace frc971::controls
+#endif // Y2022_LOCALIZER_LOCALIZER_H_
diff --git a/y2022/localizer/localizer_main.cc b/y2022/localizer/localizer_main.cc
new file mode 100644
index 0000000..fab1d51
--- /dev/null
+++ b/y2022/localizer/localizer_main.cc
@@ -0,0 +1,21 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2022/localizer/localizer.h"
+#include "y2022/control_loops/drivetrain/drivetrain_base.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+ frc971::controls::EventLoopLocalizer localizer(
+ &event_loop, ::y2022::control_loops::drivetrain::GetDrivetrainConfig());
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2022/localizer/localizer_output.fbs b/y2022/localizer/localizer_output.fbs
new file mode 100644
index 0000000..03abf19
--- /dev/null
+++ b/y2022/localizer/localizer_output.fbs
@@ -0,0 +1,26 @@
+namespace frc971.controls;
+
+// This provides a minimal output from the localizer that can be forwarded to
+// the roborio and used for corrections to its (simpler) localizer.
+
+struct Quaternion {
+ w:double (id: 0);
+ x:double (id: 1);
+ y:double (id: 2);
+ z:double (id: 3);
+}
+
+table LocalizerOutput {
+ // Timestamp (on the source node) that this sample corresponds with. This
+ // may be older than the sent time to account for delays in sensor readings.
+ monotonic_timestamp_ns:int64 (id: 0);
+ // Current x/y position estimate, in meters.
+ x:double (id: 1);
+ y:double (id: 2);
+ // Current heading, in radians.
+ theta:double (id: 3);
+ // Current estimate of the robot's 3-D rotation.
+ orientation:Quaternion (id: 4);
+}
+
+root_type LocalizerOutput;
diff --git a/y2022/localizer/localizer_plotter.ts b/y2022/localizer/localizer_plotter.ts
new file mode 100644
index 0000000..239d2cb
--- /dev/null
+++ b/y2022/localizer/localizer_plotter.ts
@@ -0,0 +1,277 @@
+// Provides a plot for debugging drivetrain-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import {ImuMessageHandler} from 'org_frc971/frc971/wpilib/imu_plot_utils';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+
+export function plotLocalizer(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+
+ const position = aosPlotter.addMessageSource("/drivetrain",
+ "frc971.control_loops.drivetrain.Position");
+ const status = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+ const output = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Output');
+ const localizer = aosPlotter.addMessageSource(
+ '/localizer', 'frc971.controls.LocalizerStatus');
+ const imu = aosPlotter.addRawMessageSource(
+ '/localizer', 'frc971.IMUValuesBatch',
+ new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
+
+ // Drivetrain Status estimated relative position
+ const positionPlot = aosPlotter.addPlot(element);
+ positionPlot.plot.getAxisLabels().setTitle("Estimated Relative Position " +
+ "of the Drivetrain");
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
+ positionPlot.plot.getAxisLabels().setYLabel("Relative Position (m)");
+ const leftPosition =
+ positionPlot.addMessageLine(status, ["estimated_left_position"]);
+ leftPosition.setColor(RED);
+ const rightPosition =
+ positionPlot.addMessageLine(status, ["estimated_right_position"]);
+ rightPosition.setColor(GREEN);
+ positionPlot
+ .addMessageLine(localizer, ['model_based', 'model_state', 'left_encoder'])
+ .setColor(BROWN)
+ .setPointSize(0.0);
+ positionPlot
+ .addMessageLine(localizer, ['model_based', 'model_state', 'right_encoder'])
+ .setColor(CYAN)
+ .setPointSize(0.0);
+ positionPlot.addMessageLine(position, ['left_encoder'])
+ .setColor(BROWN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(imu, ['left_encoder'])
+ .setColor(BROWN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(localizer, ['left_encoder'])
+ .setColor(RED)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(position, ['right_encoder'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(imu, ['right_encoder'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(localizer, ['right_encoder'])
+ .setColor(GREEN)
+ .setDrawLine(false);
+
+
+ // Drivetrain Velocities
+ const velocityPlot = aosPlotter.addPlot(element);
+ velocityPlot.plot.getAxisLabels().setTitle('Velocity Plots');
+ velocityPlot.plot.getAxisLabels().setXLabel(TIME);
+ velocityPlot.plot.getAxisLabels().setYLabel('Wheel Velocity (m/s)');
+
+ const leftVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_left_velocity']);
+ leftVelocity.setColor(RED);
+ const rightVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_right_velocity']);
+ rightVelocity.setColor(GREEN);
+
+ const leftSpeed = velocityPlot.addMessageLine(position, ["left_speed"]);
+ leftSpeed.setColor(BLUE);
+ const rightSpeed = velocityPlot.addMessageLine(position, ["right_speed"]);
+ rightSpeed.setColor(BROWN);
+
+ const ekfLeftVelocity = velocityPlot.addMessageLine(
+ localizer, ['model_based', 'model_state', 'left_velocity']);
+ ekfLeftVelocity.setColor(RED);
+ ekfLeftVelocity.setPointSize(0.0);
+ const ekfRightVelocity = velocityPlot.addMessageLine(
+ localizer, ['model_based', 'model_state', 'right_velocity']);
+ ekfRightVelocity.setColor(GREEN);
+ ekfRightVelocity.setPointSize(0.0);
+
+ velocityPlot
+ .addMessageLine(
+ localizer, ['model_based', 'oldest_model_state', 'left_velocity'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ velocityPlot
+ .addMessageLine(
+ localizer, ['model_based', 'oldest_model_state', 'right_velocity'])
+ .setColor(GREEN)
+ .setDrawLine(false);
+
+ const splineVelocityOffset =
+ velocityPlot
+ .addMessageLine(status, ['localizer', 'longitudinal_velocity_offset'])
+ .setColor(BROWN)
+ .setPointSize(0.0);
+
+ const splineLateralVelocity =
+ velocityPlot.addMessageLine(status, ['localizer', 'lateral_velocity'])
+ .setColor(PINK)
+ .setPointSize(0.0);
+
+ // Drivetrain Voltage
+ const voltagePlot = aosPlotter.addPlot(element);
+ voltagePlot.plot.getAxisLabels().setTitle('Voltage Plots');
+ voltagePlot.plot.getAxisLabels().setXLabel(TIME);
+ voltagePlot.plot.getAxisLabels().setYLabel('Voltage (V)')
+
+ voltagePlot.addMessageLine(localizer, ['model_based', 'model_state', 'left_voltage_error'])
+ .setColor(RED)
+ .setDrawLine(false);
+ voltagePlot.addMessageLine(localizer, ['model_based', 'model_state', 'right_voltage_error'])
+ .setColor(GREEN)
+ .setDrawLine(false);
+ voltagePlot.addMessageLine(output, ['left_voltage'])
+ .setColor(RED)
+ .setPointSize(0);
+ voltagePlot.addMessageLine(output, ['right_voltage'])
+ .setColor(GREEN)
+ .setPointSize(0);
+
+ // Heading
+ const yawPlot = aosPlotter.addPlot(element);
+ yawPlot.plot.getAxisLabels().setTitle('Robot Yaw');
+ yawPlot.plot.getAxisLabels().setXLabel(TIME);
+ yawPlot.plot.getAxisLabels().setYLabel('Yaw (rad)');
+
+ yawPlot.addMessageLine(status, ['localizer', 'theta']).setColor(GREEN);
+
+ yawPlot.addMessageLine(status, ['down_estimator', 'yaw']).setColor(BLUE);
+
+ yawPlot.addMessageLine(localizer, ['model_based', 'theta']).setColor(RED);
+
+ // Pitch/Roll
+ const orientationPlot = aosPlotter.addPlot(element);
+ orientationPlot.plot.getAxisLabels().setTitle('Orientation');
+ orientationPlot.plot.getAxisLabels().setXLabel(TIME);
+ orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
+
+ orientationPlot.addMessageLine(localizer, ['model_based', 'down_estimator', 'lateral_pitch'])
+ .setColor(RED)
+ .setLabel('roll');
+ orientationPlot
+ .addMessageLine(localizer, ['model_based', 'down_estimator', 'longitudinal_pitch'])
+ .setColor(GREEN)
+ .setLabel('pitch');
+
+ const stillPlot = aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 3]);
+ stillPlot.plot.getAxisLabels().setTitle('Still Plot');
+ stillPlot.plot.getAxisLabels().setXLabel(TIME);
+ stillPlot.plot.getAxisLabels().setYLabel('bool, g\'s');
+ stillPlot.plot.setDefaultYRange([-0.1, 1.1]);
+
+ stillPlot.addMessageLine(localizer, ['model_based', 'down_estimator', 'gravity_magnitude'])
+ .setColor(WHITE)
+ .setDrawLine(false);
+ stillPlot.addMessageLine(localizer, ['model_based', 'using_model'])
+ .setColor(PINK)
+ .setDrawLine(false);
+
+ // Accelerometer/Gravity
+ const accelPlot = aosPlotter.addPlot(element);
+ accelPlot.plot.getAxisLabels().setTitle('Absoluate Velocities')
+ accelPlot.plot.getAxisLabels().setYLabel('Velocity (m/s)');
+ accelPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+
+ accelPlot.addMessageLine(localizer, ['model_based', 'accel_state', 'velocity_x'])
+ .setColor(RED)
+ .setDrawLine(false);
+ accelPlot.addMessageLine(localizer, ['model_based', 'accel_state', 'velocity_y'])
+ .setColor(GREEN)
+ .setDrawLine(false);
+
+ accelPlot.addMessageLine(localizer, ['model_based', 'oldest_accel_state', 'velocity_x'])
+ .setColor(RED)
+ .setPointSize(0);
+ accelPlot.addMessageLine(localizer, ['model_based', 'oldest_accel_state', 'velocity_y'])
+ .setColor(GREEN)
+ .setPointSize(0);
+
+ // Absolute X Position
+ const xPositionPlot = aosPlotter.addPlot(element);
+ xPositionPlot.plot.getAxisLabels().setTitle('X Position');
+ xPositionPlot.plot.getAxisLabels().setXLabel(TIME);
+ xPositionPlot.plot.getAxisLabels().setYLabel('X Position (m)');
+
+ xPositionPlot.addMessageLine(status, ['x']).setColor(RED);
+ xPositionPlot.addMessageLine(status, ['down_estimator', 'position_x'])
+ .setColor(BLUE);
+ xPositionPlot.addMessageLine(localizer, ['model_based', 'x']).setColor(CYAN);
+
+ // Absolute Y Position
+ const yPositionPlot = aosPlotter.addPlot(element);
+ yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
+ yPositionPlot.plot.getAxisLabels().setXLabel(TIME);
+ yPositionPlot.plot.getAxisLabels().setYLabel('Y Position (m)');
+
+ const localizerY = yPositionPlot.addMessageLine(status, ['y']);
+ localizerY.setColor(RED);
+ yPositionPlot.addMessageLine(status, ['down_estimator', 'position_y'])
+ .setColor(BLUE);
+ yPositionPlot.addMessageLine(localizer, ['model_based', 'y']).setColor(CYAN);
+
+ // Gyro
+ const gyroPlot = aosPlotter.addPlot(element);
+ gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
+ gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
+ gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const gyroX = gyroPlot.addMessageLine(imu, ['gyro_x']);
+ gyroX.setColor(RED);
+ const gyroY = gyroPlot.addMessageLine(imu, ['gyro_y']);
+ gyroY.setColor(GREEN);
+ const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
+ gyroZ.setColor(BLUE);
+
+ const impliedAccelPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ impliedAccelPlot.plot.getAxisLabels().setTitle('Implied Accelerations');
+ impliedAccelPlot.plot.getAxisLabels().setXLabel(TIME);
+
+ impliedAccelPlot.addMessageLine(localizer, ['model_based', 'implied_accel_z'])
+ .setColor(BLUE);
+ impliedAccelPlot.addMessageLine(localizer, ['model_based', 'implied_accel_y'])
+ .setColor(GREEN);
+ impliedAccelPlot.addMessageLine(localizer, ['model_based', 'implied_accel_x'])
+ .setColor(RED);
+
+ const costPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ costPlot.plot.getAxisLabels().setTitle('Costs');
+ costPlot.plot.getAxisLabels().setXLabel(TIME);
+
+ costPlot.addMessageLine(localizer, ['model_based', 'residual'])
+ .setColor(RED)
+ .setPointSize(0);
+
+ costPlot.addMessageLine(localizer, ['model_based', 'filtered_residual'])
+ .setColor(BLUE)
+ .setPointSize(0);
+
+ costPlot.addMessageLine(localizer, ['model_based', 'velocity_residual'])
+ .setColor(GREEN)
+ .setPointSize(0);
+
+ costPlot.addMessageLine(localizer, ['model_based', 'theta_rate_residual'])
+ .setColor(BROWN)
+ .setPointSize(0);
+
+ costPlot.addMessageLine(localizer, ['model_based', 'accel_residual'])
+ .setColor(CYAN)
+ .setPointSize(0);
+
+ const timingPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ timingPlot.plot.getAxisLabels().setTitle('Timing');
+ timingPlot.plot.getAxisLabels().setXLabel(TIME);
+
+ timingPlot.addMessageLine(localizer, ['model_based', 'clock_resets'])
+ .setColor(GREEN)
+ .setDrawLine(false);
+}
diff --git a/y2022/localizer/localizer_replay.cc b/y2022/localizer/localizer_replay.cc
new file mode 100644
index 0000000..d328948
--- /dev/null
+++ b/y2022/localizer/localizer_replay.cc
@@ -0,0 +1,95 @@
+#include "aos/configuration.h"
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/network/team_number.h"
+#include "y2022/localizer/localizer.h"
+#include "y2022/localizer/localizer_schema.h"
+#include "gflags/gflags.h"
+#include "y2022/control_loops/drivetrain/drivetrain_base.h"
+
+DEFINE_string(config, "y2022/aos_config.json",
+ "Name of the config file to replay using.");
+DEFINE_int32(team, 7971, "Team number to use for logfile replay.");
+DEFINE_string(output_folder, "/tmp/replayed",
+ "Name of the folder to write replayed logs to.");
+
+class LoggerState {
+ public:
+ LoggerState(aos::logger::LogReader *reader, const aos::Node *node)
+ : event_loop_(
+ reader->event_loop_factory()->MakeEventLoop("logger", node)),
+ namer_(std::make_unique<aos::logger::MultiNodeLogNamer>(
+ absl::StrCat(FLAGS_output_folder, "/", node->name()->string_view(),
+ "/"),
+ event_loop_.get())),
+ logger_(std::make_unique<aos::logger::Logger>(event_loop_.get())) {
+ event_loop_->SkipTimingReport();
+ event_loop_->SkipAosLog();
+ event_loop_->OnRun([this]() { logger_->StartLogging(std::move(namer_)); });
+ }
+
+ private:
+ std::unique_ptr<aos::EventLoop> event_loop_;
+ std::unique_ptr<aos::logger::LogNamer> namer_;
+ std::unique_ptr<aos::logger::Logger> logger_;
+};
+
+// TODO(james): Currently, this replay produces logfiles that can't be read due
+// to time estimation issues. Pending the active refactorings of the
+// timestamp-related code, fix this.
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::network::OverrideTeamNumber(FLAGS_team);
+
+ const aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ // find logfiles
+ std::vector<std::string> unsorted_logfiles =
+ aos::logger::FindLogs(argc, argv);
+
+ // sort logfiles
+ const std::vector<aos::logger::LogFile> logfiles =
+ aos::logger::SortParts(unsorted_logfiles);
+
+ // open logfiles
+ aos::logger::LogReader reader(logfiles, &config.message());
+
+ auto factory =
+ std::make_unique<aos::SimulatedEventLoopFactory>(reader.configuration());
+
+ reader.Register(factory.get());
+
+ std::vector<std::unique_ptr<LoggerState>> loggers;
+ // List of nodes to create loggers for (note: currently just roborio; this
+ // code was refactored to allow easily adding new loggers to accommodate
+ // debugging and potential future changes).
+ const std::vector<std::string> nodes_to_log = {"imu"};
+ for (const std::string &node : nodes_to_log) {
+ loggers.emplace_back(std::make_unique<LoggerState>(
+ &reader, aos::configuration::GetNode(reader.configuration(), node)));
+ }
+
+ const aos::Node *node = nullptr;
+ if (aos::configuration::MultiNode(reader.configuration())) {
+ node = aos::configuration::GetNode(reader.configuration(), "imu");
+ }
+
+ std::unique_ptr<aos::EventLoop> localizer_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("localizer", node);
+ localizer_event_loop->SkipTimingReport();
+
+ frc971::controls::EventLoopLocalizer localizer(
+ localizer_event_loop.get(),
+ y2022::control_loops::drivetrain::GetDrivetrainConfig());
+
+ reader.event_loop_factory()->Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2022/localizer/localizer_status.fbs b/y2022/localizer/localizer_status.fbs
new file mode 100644
index 0000000..80cee0b
--- /dev/null
+++ b/y2022/localizer/localizer_status.fbs
@@ -0,0 +1,99 @@
+include "frc971/control_loops/drivetrain/drivetrain_status.fbs";
+
+namespace frc971.controls;
+
+// Stores the state associated with the acceleration-based modelling.
+table AccelBasedState {
+ // x/y position, in meters.
+ x:double (id: 0);
+ y:double (id: 1);
+ // heading, in radians.
+ theta:double (id: 2);
+ // Velocity in X/Y directions, in m/s.
+ velocity_x:double (id: 3);
+ velocity_y:double (id: 4);
+}
+
+// Stores the state associated with the drivetrain model-based state.
+// This model assumes zero lateral motion of the drivetrain.
+table ModelBasedState {
+ // x/y position, in meters.
+ x:double (id: 0);
+ y:double (id: 1);
+ // heading, in radians.
+ theta:double (id: 2);
+ // Expected encoder reading for the left side of the drivetrain, in meters.
+ left_encoder:double (id: 3);
+ // Modelled velocity of the left side of the drivetrain, in meters / second.
+ left_velocity:double (id: 4);
+ // Estimated voltage error, in volts.
+ left_voltage_error:double (id: 5);
+ // Same as the left_* fields, but for the right side of the drivetrain.
+ right_encoder:double (id: 6);
+ right_velocity:double (id: 7);
+ right_voltage_error:double (id: 8);
+}
+
+table ModelBasedStatus {
+ // Current acceleration and model-based states. Depending on using_model,
+ // one of these will be the ground-truth and the other will be calculated
+ // based on it. E.g. if using_model is true, then model_state will be
+ // populated as you'd expect, while accel_state will be populated to be
+ // consistent with model_state (e.g., no lateral motion).
+ accel_state:AccelBasedState (id: 0);
+ model_state:ModelBasedState (id: 1);
+ // using_model indicates whether we are currently in in model-based or
+ // accelerometer-based estimation.
+ using_model:bool (id: 2);
+ // Current residual associated with the amount of inconsistency between
+ // the two models. Will be zero if the drivetrain model is perfectly
+ // consistent with the IMU readings.
+ residual:double (id: 3);
+ // Status from the down estimator.
+ down_estimator:frc971.control_loops.drivetrain.DownEstimatorState (id: 4);
+ // Current ground-truth for x/y/theta. Should match those present in *_state.
+ x:double (id: 5);
+ y:double (id: 6);
+ theta:double (id: 7);
+ // Current accelerations implied by the current accelerometer + down estimator
+ // + yaw readings.
+ implied_accel_x:double (id: 8);
+ implied_accel_y:double (id: 9);
+ implied_accel_z:double (id: 10);
+ // oldest_* are the oldest surviving branches of the model that have just been
+ // running purely on one model.
+ oldest_accel_state:AccelBasedState (id: 11);
+ oldest_model_state:ModelBasedState (id: 12);
+ // Filtered version of the residual field--this is what is actually used by
+ // the code for determining when to swap between modes.
+ filtered_residual:double (id: 13);
+ // Components of the residual. Useful for debugging.
+ velocity_residual:double (id: 14);
+ accel_residual:double (id: 15);
+ theta_rate_residual:double (id: 16);
+ // Number of times we have missed an IMU reading. Should never increase except
+ // *maybe* during startup.
+ clock_resets:int (id: 17);
+}
+
+table LocalizerStatus {
+ model_based:ModelBasedStatus (id: 0);
+ // Whether the IMU is zeroed or not.
+ zeroed:bool (id: 1);
+ // Whether the IMU zeroing is faulted or not.
+ faulted_zero:bool (id: 2);
+ zeroing:control_loops.drivetrain.ImuZeroerState (id: 3);
+ // Offset between the pico clock and the pi clock, such that
+ // pico_timestamp + pico_offset_ns = pi_timestamp
+ pico_offset_ns:int64 (id: 4);
+ // Error in the offset, if we assume that the pi/pico clocks are identical and
+ // that there is a perfectly consistent latency between the two. Will be zero
+ // for the very first cycle, and then referenced off of the initial offset
+ // thereafter. If greater than zero, implies that the pico is "behind",
+ // whether due to unusually large latency or due to clock drift.
+ pico_offset_error_ns:int64 (id: 5);
+ left_encoder:double (id: 6);
+ right_encoder:double (id: 7);
+}
+
+root_type LocalizerStatus;
diff --git a/y2022/localizer/localizer_test.cc b/y2022/localizer/localizer_test.cc
new file mode 100644
index 0000000..1997b80
--- /dev/null
+++ b/y2022/localizer/localizer_test.cc
@@ -0,0 +1,528 @@
+#include "y2022/localizer/localizer.h"
+
+#include "aos/events/simulated_event_loop.h"
+#include "gtest/gtest.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+
+namespace frc971::controls::testing {
+typedef ModelBasedLocalizer::ModelState ModelState;
+typedef ModelBasedLocalizer::AccelState AccelState;
+typedef ModelBasedLocalizer::ModelInput ModelInput;
+typedef ModelBasedLocalizer::AccelInput AccelInput;
+namespace {
+constexpr size_t kX = ModelBasedLocalizer::kX;
+constexpr size_t kY = ModelBasedLocalizer::kY;
+constexpr size_t kTheta = ModelBasedLocalizer::kTheta;
+constexpr size_t kVelocityX = ModelBasedLocalizer::kVelocityX;
+constexpr size_t kVelocityY = ModelBasedLocalizer::kVelocityY;
+constexpr size_t kAccelX = ModelBasedLocalizer::kAccelX;
+constexpr size_t kAccelY = ModelBasedLocalizer::kAccelY;
+constexpr size_t kThetaRate = ModelBasedLocalizer::kThetaRate;
+constexpr size_t kLeftEncoder = ModelBasedLocalizer::kLeftEncoder;
+constexpr size_t kLeftVelocity = ModelBasedLocalizer::kLeftVelocity;
+constexpr size_t kLeftVoltageError = ModelBasedLocalizer::kLeftVoltageError;
+constexpr size_t kRightEncoder = ModelBasedLocalizer::kRightEncoder;
+constexpr size_t kRightVelocity = ModelBasedLocalizer::kRightVelocity;
+constexpr size_t kRightVoltageError = ModelBasedLocalizer::kRightVoltageError;
+constexpr size_t kLeftVoltage = ModelBasedLocalizer::kLeftVoltage;
+constexpr size_t kRightVoltage = ModelBasedLocalizer::kRightVoltage;
+}
+
+class LocalizerTest : public ::testing::Test {
+ protected:
+ LocalizerTest()
+ : dt_config_(
+ control_loops::drivetrain::testing::GetTestDrivetrainConfig()),
+ localizer_(dt_config_) {
+ localizer_.set_longitudinal_offset(0.0);
+ }
+ ModelState CallDiffModel(const ModelState &state, const ModelInput &U) {
+ return localizer_.DiffModel(state, U);
+ }
+
+ AccelState CallDiffAccel(const AccelState &state, const AccelInput &U) {
+ return localizer_.DiffAccel(state, U);
+ }
+
+ const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ ModelBasedLocalizer localizer_;
+
+};
+
+TEST_F(LocalizerTest, AccelIntegrationTest) {
+ AccelState state;
+ state.setZero();
+ AccelInput input;
+ input.setZero();
+
+ EXPECT_EQ(0.0, CallDiffAccel(state, input).norm());
+ // Non-zero x/y/theta states should still result in a zero derivative.
+ state(kX) = 1.0;
+ state(kY) = 1.0;
+ state(kTheta) = 1.0;
+ EXPECT_EQ(0.0, CallDiffAccel(state, input).norm());
+
+ state.setZero();
+ state(kVelocityX) = 1.0;
+ state(kVelocityY) = 2.0;
+ EXPECT_EQ((AccelState() << 1.0, 2.0, 0.0, 0.0, 0.0).finished(),
+ CallDiffAccel(state, input));
+ // Derivatives should be independent of theta.
+ state(kTheta) = M_PI / 2.0;
+ EXPECT_EQ((AccelState() << 1.0, 2.0, 0.0, 0.0, 0.0).finished(),
+ CallDiffAccel(state, input));
+
+ state.setZero();
+ input(kAccelX) = 1.0;
+ input(kAccelY) = 2.0;
+ input(kThetaRate) = 3.0;
+ EXPECT_EQ((AccelState() << 0.0, 0.0, 3.0, 1.0, 2.0).finished(),
+ CallDiffAccel(state, input));
+ state(kTheta) = M_PI / 2.0;
+ EXPECT_EQ((AccelState() << 0.0, 0.0, 3.0, 1.0, 2.0).finished(),
+ CallDiffAccel(state, input));
+}
+
+TEST_F(LocalizerTest, ModelIntegrationTest) {
+ ModelState state;
+ state.setZero();
+ ModelInput input;
+ input.setZero();
+ ModelState diff;
+
+ EXPECT_EQ(0.0, CallDiffModel(state, input).norm());
+ // Non-zero x/y/theta/encoder states should still result in a zero derivative.
+ state(kX) = 1.0;
+ state(kY) = 1.0;
+ state(kTheta) = 1.0;
+ state(kLeftEncoder) = 1.0;
+ state(kRightEncoder) = 1.0;
+ EXPECT_EQ(0.0, CallDiffModel(state, input).norm());
+
+ state.setZero();
+ state(kLeftVelocity) = 1.0;
+ state(kRightVelocity) = 1.0;
+ diff = CallDiffModel(state, input);
+ const ModelState mask_velocities =
+ (ModelState() << 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0).finished();
+ EXPECT_EQ(
+ (ModelState() << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(),
+ diff.cwiseProduct(mask_velocities));
+ EXPECT_EQ(diff(kLeftVelocity), diff(kRightVelocity));
+ EXPECT_GT(0.0, diff(kLeftVelocity));
+ state(kTheta) = M_PI / 2.0;
+ diff = CallDiffModel(state, input);
+ EXPECT_NEAR(0.0,
+ ((ModelState() << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0)
+ .finished() -
+ diff.cwiseProduct(mask_velocities))
+ .norm(),
+ 1e-12);
+ EXPECT_EQ(diff(kLeftVelocity), diff(kRightVelocity));
+ EXPECT_GT(0.0, diff(kLeftVelocity));
+
+ state.setZero();
+ state(kLeftVelocity) = -1.0;
+ state(kRightVelocity) = 1.0;
+ diff = CallDiffModel(state, input);
+ EXPECT_EQ((ModelState() << 0.0, 0.0, 1.0 / dt_config_.robot_radius, -1.0, 0.0,
+ 0.0, 1.0, 0.0, 0.0)
+ .finished(),
+ diff.cwiseProduct(mask_velocities));
+ EXPECT_EQ(-diff(kLeftVelocity), diff(kRightVelocity));
+ EXPECT_LT(0.0, diff(kLeftVelocity));
+
+ state.setZero();
+ input(kLeftVoltage) = 5.0;
+ input(kRightVoltage) = 6.0;
+ diff = CallDiffModel(state, input);
+ EXPECT_EQ(0, diff(kX));
+ EXPECT_EQ(0, diff(kY));
+ EXPECT_EQ(0, diff(kTheta));
+ EXPECT_EQ(0, diff(kLeftEncoder));
+ EXPECT_EQ(0, diff(kRightEncoder));
+ EXPECT_EQ(0, diff(kLeftVoltageError));
+ EXPECT_EQ(0, diff(kRightVoltageError));
+ EXPECT_LT(0, diff(kLeftVelocity));
+ EXPECT_LT(0, diff(kRightVelocity));
+ EXPECT_LT(diff(kLeftVelocity), diff(kRightVelocity));
+
+ state.setZero();
+ state(kLeftVoltageError) = -1.0;
+ state(kRightVoltageError) = -2.0;
+ input(kLeftVoltage) = 1.0;
+ input(kRightVoltage) = 2.0;
+ EXPECT_EQ(ModelState::Zero(), CallDiffModel(state, input));
+}
+
+// Test that the HandleReset does indeed reset the state of the localizer.
+TEST_F(LocalizerTest, LocalizerReset) {
+ aos::monotonic_clock::time_point t = aos::monotonic_clock::epoch();
+ localizer_.HandleReset(t, {1.0, 2.0, 3.0});
+ EXPECT_EQ((Eigen::Vector3d{1.0, 2.0, 3.0}), localizer_.xytheta());
+ localizer_.HandleReset(t, {4.0, 5.0, 6.0});
+ EXPECT_EQ((Eigen::Vector3d{4.0, 5.0, 6.0}), localizer_.xytheta());
+}
+
+// Test that if we are moving only by accelerometer readings (and just assuming
+// zero voltage/encoders) that we initially don't believe it but then latch into
+// following the accelerometer.
+// Note: this test is somewhat sensitive to the exact tuning values used for the
+// filter.
+TEST_F(LocalizerTest, AccelOnly) {
+ const aos::monotonic_clock::time_point start = aos::monotonic_clock::epoch();
+ const std::chrono::microseconds kDt{500};
+ aos::monotonic_clock::time_point t = start - std::chrono::milliseconds(1000);
+ Eigen::Vector3d gyro{0.0, 0.0, 0.0};
+ const Eigen::Vector2d encoders{0.0, 0.0};
+ const Eigen::Vector2d voltages{0.0, 0.0};
+ Eigen::Vector3d accel{5.0, 2.0, 9.80665};
+ Eigen::Vector3d accel_gs = accel / 9.80665;
+ while (t < start) {
+ // Spin to fill up the buffer.
+ localizer_.HandleImu(t, gyro, Eigen::Vector3d::UnitZ(), encoders, voltages);
+ t += kDt;
+ }
+ while (t < start + std::chrono::milliseconds(100)) {
+ localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
+ EXPECT_EQ(Eigen::Vector3d::Zero(), localizer_.xytheta());
+ t += kDt;
+ }
+ while (t < start + std::chrono::milliseconds(500)) {
+ // Too lazy to hard-code when the transition happens.
+ localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
+ t += kDt;
+ }
+ while (t < start + std::chrono::milliseconds(1000)) {
+ SCOPED_TRACE(t);
+ localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
+ const Eigen::Vector3d xytheta = localizer_.xytheta();
+ t += kDt;
+ EXPECT_NEAR(
+ 0.5 * accel(0) * std::pow(aos::time::DurationInSeconds(t - start), 2),
+ xytheta(0), 1e-4);
+ EXPECT_NEAR(
+ 0.5 * accel(1) * std::pow(aos::time::DurationInSeconds(t - start), 2),
+ xytheta(1), 1e-4);
+ EXPECT_EQ(0.0, xytheta(2));
+ }
+
+ ASSERT_NEAR(accel(0), localizer_.accel_state()(kVelocityX), 1e-10);
+ ASSERT_NEAR(accel(1), localizer_.accel_state()(kVelocityY), 1e-10);
+
+ // Start going in a cirlce, and confirm that we
+ // handle things correctly. We rotate the accelerometer readings by 90 degrees
+ // and then leave them constant, which should make it look like we are going
+ // around in a circle.
+ accel = Eigen::Vector3d{-accel(1), accel(0), 9.80665};
+ accel_gs = accel / 9.80665;
+ // v^2 / r = a
+ // w * r = v
+ // v^2 / v * w = a
+ // w = a / v
+ const double omega = accel.topRows<2>().norm() /
+ std::hypot(localizer_.accel_state()(kVelocityX),
+ localizer_.accel_state()(kVelocityY));
+ gyro << 0.0, 0.0, omega;
+ // Due to the magic of math, omega works out to be 1.0 after having run at the
+ // acceleration for one second.
+ ASSERT_NEAR(1.0, omega, 1e-10);
+ // Yes, we could save some operations here, but let's be at least somewhat
+ // clear about what we're doing...
+ const double radius = accel.topRows<2>().norm() / (omega * omega);
+ const Eigen::Vector2d center = localizer_.xytheta().topRows<2>() +
+ accel.topRows<2>().normalized() * radius;
+ const double initial_theta = std::atan2(-accel(1), -accel(0));
+
+ std::chrono::microseconds one_revolution_time(
+ static_cast<int>(2 * M_PI / omega * 1e6));
+
+ aos::monotonic_clock::time_point circle_start = t;
+
+ while (t < circle_start + one_revolution_time) {
+ SCOPED_TRACE(t);
+ localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
+ t += kDt;
+ const double t_circle = aos::time::DurationInSeconds(t - circle_start);
+ ASSERT_NEAR(t_circle * omega, localizer_.xytheta()(2), 1e-5);
+ const double theta_circle = t_circle * omega + initial_theta;
+ const Eigen::Vector2d offset =
+ radius *
+ Eigen::Vector2d{std::cos(theta_circle), std::sin(theta_circle)};
+ const Eigen::Vector2d expected = center + offset;
+ const Eigen::Vector2d estimated = localizer_.xytheta().topRows<2>();
+ const Eigen::Vector2d implied_offset = estimated - center;
+ const double implied_theta =
+ std::atan2(implied_offset.y(), implied_offset.x());
+ VLOG(1) << "center: " << center.transpose() << " radius " << radius
+ << "\nlocalizer " << localizer_.xytheta().transpose()
+ << " t_circle " << t_circle << " omega " << omega << " theta "
+ << theta_circle << "\noffset " << offset.transpose()
+ << "\nexpected " << expected.transpose() << "\nimplied offset "
+ << implied_offset << " implied_theta " << implied_theta << "\nvel "
+ << localizer_.accel_state()(kVelocityX) << ", "
+ << localizer_.accel_state()(kVelocityY);
+ ASSERT_NEAR(0.0, (expected - localizer_.xytheta().topRows<2>()).norm(),
+ 1e-2);
+ }
+
+ // Set accelerometer back to zero and confirm that we recover (the
+ // implementation decays the accelerometer speeds to zero when still, so
+ // should recover).
+ while (t <
+ circle_start + one_revolution_time + std::chrono::milliseconds(3000)) {
+ localizer_.HandleImu(t, Eigen::Vector3d::Zero(), Eigen::Vector3d::UnitZ(),
+ encoders, voltages);
+ t += kDt;
+ }
+ const Eigen::Vector3d final_pos = localizer_.xytheta();
+ localizer_.HandleImu(t, Eigen::Vector3d::Zero(), Eigen::Vector3d::UnitZ(),
+ encoders, voltages);
+ ASSERT_NEAR(0.0, (final_pos - localizer_.xytheta()).norm(), 1e-10);
+}
+
+using control_loops::drivetrain::Output;
+
+class EventLoopLocalizerTest : public ::testing::Test {
+ protected:
+ EventLoopLocalizerTest()
+ : configuration_(aos::configuration::ReadConfig("y2022/aos_config.json")),
+ event_loop_factory_(&configuration_.message()),
+ roborio_node_(
+ aos::configuration::GetNode(&configuration_.message(), "roborio")),
+ imu_node_(
+ aos::configuration::GetNode(&configuration_.message(), "imu")),
+ dt_config_(
+ control_loops::drivetrain::testing::GetTestDrivetrainConfig()),
+ localizer_event_loop_(
+ event_loop_factory_.MakeEventLoop("localizer", imu_node_)),
+ localizer_(localizer_event_loop_.get(), dt_config_),
+ drivetrain_plant_event_loop_(event_loop_factory_.MakeEventLoop(
+ "drivetrain_plant", roborio_node_)),
+ drivetrain_plant_imu_event_loop_(
+ event_loop_factory_.MakeEventLoop("drivetrain_plant", imu_node_)),
+ drivetrain_plant_(drivetrain_plant_event_loop_.get(),
+ drivetrain_plant_imu_event_loop_.get(), dt_config_,
+ std::chrono::microseconds(500)),
+ roborio_test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", roborio_node_)),
+ imu_test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", imu_node_)),
+ logger_test_event_loop_(
+ event_loop_factory_.GetNodeEventLoopFactory("logger")
+ ->MakeEventLoop("test")),
+ output_sender_(
+ roborio_test_event_loop_->MakeSender<Output>("/drivetrain")),
+ output_fetcher_(roborio_test_event_loop_->MakeFetcher<LocalizerOutput>(
+ "/localizer")),
+ status_fetcher_(
+ imu_test_event_loop_->MakeFetcher<LocalizerStatus>("/localizer")) {
+ localizer_.localizer()->set_longitudinal_offset(0.0);
+ aos::TimerHandler *timer = roborio_test_event_loop_->AddTimer([this]() {
+ auto builder = output_sender_.MakeBuilder();
+ auto output_builder = builder.MakeBuilder<Output>();
+ output_builder.add_left_voltage(output_voltages_(0));
+ output_builder.add_right_voltage(output_voltages_(1));
+ builder.CheckOk(builder.Send(output_builder.Finish()));
+ });
+ roborio_test_event_loop_->OnRun([timer, this]() {
+ timer->Setup(roborio_test_event_loop_->monotonic_now(),
+ std::chrono::milliseconds(5));
+ });
+ // Get things zeroed.
+ event_loop_factory_.RunFor(std::chrono::seconds(10));
+ CHECK(status_fetcher_.Fetch());
+ CHECK(status_fetcher_->zeroed());
+ }
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+ const aos::Node *const roborio_node_;
+ const aos::Node *const imu_node_;
+ const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ std::unique_ptr<aos::EventLoop> localizer_event_loop_;
+ EventLoopLocalizer localizer_;
+
+ std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
+ std::unique_ptr<aos::EventLoop> drivetrain_plant_imu_event_loop_;
+ control_loops::drivetrain::testing::DrivetrainSimulation drivetrain_plant_;
+
+ std::unique_ptr<aos::EventLoop> roborio_test_event_loop_;
+ std::unique_ptr<aos::EventLoop> imu_test_event_loop_;
+ std::unique_ptr<aos::EventLoop> logger_test_event_loop_;
+
+ aos::Sender<Output> output_sender_;
+ aos::Fetcher<LocalizerOutput> output_fetcher_;
+ aos::Fetcher<LocalizerStatus> status_fetcher_;
+
+ Eigen::Vector2d output_voltages_ = Eigen::Vector2d::Zero();
+};
+
+TEST_F(EventLoopLocalizerTest, Nominal) {
+ output_voltages_ << 1.0, 1.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ drivetrain_plant_.set_accel_sin_magnitude(0.01);
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different times.
+ ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
+ 1e-6);
+ ASSERT_LT(0.1, output_fetcher_->x());
+ ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
+ ASSERT_TRUE(status_fetcher_->has_model_based());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_LT(0.1, status_fetcher_->model_based()->accel_state()->velocity_x());
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(),
+ 1e-10);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
+ 1e-1);
+}
+
+TEST_F(EventLoopLocalizerTest, Reverse) {
+ output_voltages_ << -4.0, -4.0;
+ drivetrain_plant_.set_accel_sin_magnitude(0.01);
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different times.
+ ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
+ 1e-6);
+ ASSERT_GT(-0.1, output_fetcher_->x());
+ ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
+ ASSERT_TRUE(status_fetcher_->has_model_based());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_GT(-0.1, status_fetcher_->model_based()->accel_state()->velocity_x());
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(),
+ 1e-10);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
+ 1e-1);
+}
+
+TEST_F(EventLoopLocalizerTest, SpinInPlace) {
+ output_voltages_ << 4.0, -4.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different times.
+ ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
+ 1e-1);
+ ASSERT_NEAR(0.0, output_fetcher_->x(), 1e-10);
+ ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ ASSERT_LT(0.1, std::abs(output_fetcher_->theta()));
+ ASSERT_TRUE(status_fetcher_->has_model_based());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_x(),
+ 1e-10);
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(),
+ 1e-10);
+ ASSERT_NEAR(-status_fetcher_->model_based()->model_state()->left_velocity(),
+ status_fetcher_->model_based()->model_state()->right_velocity(),
+ 1e-3);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->residual(), 1e-3);
+}
+
+TEST_F(EventLoopLocalizerTest, Curve) {
+ output_voltages_ << 2.0, 4.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different times.
+ ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-2);
+ ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-2);
+ ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
+ 1e-1);
+ ASSERT_LT(0.1, output_fetcher_->x());
+ ASSERT_LT(0.1, output_fetcher_->y());
+ ASSERT_LT(0.1, std::abs(output_fetcher_->theta()));
+ ASSERT_TRUE(status_fetcher_->has_model_based());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_LT(0.0, status_fetcher_->model_based()->accel_state()->velocity_x());
+ ASSERT_LT(0.0, status_fetcher_->model_based()->accel_state()->velocity_y());
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->residual(), 1e-1)
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+}
+
+// Tests that small amounts of voltage error are handled by the model-based
+// half of the localizer.
+TEST_F(EventLoopLocalizerTest, VoltageError) {
+ output_voltages_ << 0.0, 0.0;
+ drivetrain_plant_.set_left_voltage_offset(2.0);
+ drivetrain_plant_.set_right_voltage_offset(2.0);
+ drivetrain_plant_.set_accel_sin_magnitude(0.01);
+
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // Should still be using the model, but have a non-trivial residual.
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_LT(0.02, status_fetcher_->model_based()->residual())
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+
+ // Afer running for a while, voltage error terms should converge and result in
+ // low residuals.
+ event_loop_factory_.RunFor(std::chrono::seconds(10));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_NEAR(
+ 2.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
+ 0.1)
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+ ASSERT_NEAR(
+ 2.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
+ 0.1)
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+ ASSERT_GT(0.02, status_fetcher_->model_based()->residual())
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+}
+
+// Tests that large amounts of voltage error force us into the
+// acceleration-based localizer.
+TEST_F(EventLoopLocalizerTest, HighVoltageError) {
+ output_voltages_ << 0.0, 0.0;
+ drivetrain_plant_.set_left_voltage_offset(200.0);
+ drivetrain_plant_.set_right_voltage_offset(200.0);
+ drivetrain_plant_.set_accel_sin_magnitude(0.01);
+
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // Should still be using the model, but have a non-trivial residual.
+ ASSERT_FALSE(status_fetcher_->model_based()->using_model());
+ ASSERT_LT(0.1, status_fetcher_->model_based()->residual())
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+ ASSERT_NEAR(drivetrain_plant_.state()(0),
+ status_fetcher_->model_based()->x(), 1.0);
+ ASSERT_NEAR(drivetrain_plant_.state()(1),
+ status_fetcher_->model_based()->y(), 1e-6);
+}
+
+} // namespace frc91::controls::testing