Create y2022 localizer
TODO:
* Update Roborio's localizer to listen to LocalizerOutput messages.
* Actually listen to camera messages.
* Correct the interface between this and the pico-reading process for
encoder values.
Change-Id: If1c7b0cca60581780fbe39fe2f78fa6b4a6933e3
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/control_loops/localizer/BUILD b/y2022/control_loops/localizer/BUILD
index d5e642f..f1198a1 100644
--- a/y2022/control_loops/localizer/BUILD
+++ b/y2022/control_loops/localizer/BUILD
@@ -31,3 +31,64 @@
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_position_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",
+ "//y2020/control_loops/drivetrain:drivetrain_base",
+ "//y2020/vision/sift:sift_fbs",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "localizer_test",
+ srcs = ["localizer_test.cc"],
+ data = [
+ "//y2022: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 = [
+ "//y2020: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",
+ ],
+)
diff --git a/y2022/control_loops/localizer/localizer.cc b/y2022/control_loops/localizer/localizer.cc
new file mode 100644
index 0000000..a145ad4
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer.cc
@@ -0,0 +1,576 @@
+#include "y2022/control_loops/localizer/localizer.h"
+
+#include "frc971/control_loops/c2d.h"
+#include "frc971/wpilib/imu_batch_generated.h"
+// TODO(james): Things to do:
+// -Approach still seems fundamentally sound.
+// -Really need to work on cost/residual function.
+// -Particularly for handling stopping.
+// -Extra hysteresis
+
+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;
+}
+
+#if 0
+Eigen::Matrix<double, 3, 3> AxisToMatrix(const Eigen::Vector3d &vec) {
+ const double rotation_norm = vec.norm();
+ return rotation_norm < 1e-5
+ ? Eigen::Matrix<double, 3, 3>::Identity()
+ : Eigen::AngleAxis<double>(rotation_norm, vec / rotation_norm)
+ .toRotationMatrix();
+}
+#endif
+} // 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) {
+ 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 velocity_x = std::cos(state(kTheta)) * robot_speed;
+ const double velocity_y = std::sin(state(kTheta)) * robot_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).
+ 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;
+ const Eigen::Vector2d robot_frame_accel(
+ model_lng_accel, diff_model(kTheta) * model_lng_velocity);
+ 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>();
+ const 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::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?
+ 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());
+
+ const Eigen::Vector3d absolute_accel =
+ orientation * dt_config_.imu_transform * kG * accel;
+ abs_accel_ = absolute_accel;
+ const Eigen::Vector3d absolute_gyro =
+ orientation * dt_config_.imu_transform * gyro;
+ (void)absolute_gyro;
+
+ VLOG(2) << "abs accel " << absolute_accel.transpose();
+ VLOG(2) << "abs gyro " << absolute_gyro.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_) {
+ state.accel_state = UpdateAccel(state.accel_state, accel_input, dt);
+ if (down_estimator_.consecutive_still() > 100.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);
+ }
+
+ 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_);
+ constexpr double kUseAccelThreshold = 0.4;
+ constexpr double kUseModelThreshold = 0.2;
+ 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_.model_state = branches_[branches_.size() - 1].model_state;
+ 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 {
+ current_state_.accel_state = branches_[branches_.size() - 1].accel_state;
+ // TODO(james): Why was I leaving the encoders/wheel velocities in place?
+ current_state_.model_state = branches_[branches_.size() - 1].model_state;
+ 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).
+ CombinedState new_branch = current_state_;
+ //if (!keep_accel_state) {
+ if (using_model_) {
+ new_branch.accel_state = AccelStateForModelState(new_branch.model_state);
+ }
+ new_branch.accumulated_divergence = 0.0;
+
+ branches_.Push(new_branch);
+
+ 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();
+}
+
+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")),
+ position_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Position>(
+ "/localizer")),
+ output_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")) {
+ 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());
+ position_fetcher_.Fetch();
+ output_fetcher_.Fetch();
+ for (const IMUValues *value : *values.readings()) {
+ zeroer_.InsertAndProcessMeasurement(*value);
+ if (zeroer_.Zeroed()) {
+ CHECK(position_fetcher_.get() != nullptr);
+ CHECK(output_fetcher_.get() != nullptr);
+ const bool disabled =
+ output_fetcher_.context().monotonic_event_time +
+ std::chrono::milliseconds(10) <
+ event_loop_->context().monotonic_event_time;
+ model_based_.HandleImu(
+ aos::monotonic_clock::time_point(
+ std::chrono::nanoseconds(value->monotonic_timestamp_ns())),
+ zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(),
+ {position_fetcher_->left_encoder(),
+ position_fetcher_->right_encoder()},
+ disabled ? Eigen::Vector2d::Zero()
+ : Eigen::Vector2d{output_fetcher_->left_voltage(),
+ output_fetcher_->right_voltage()});
+ }
+ {
+ auto builder = status_sender_.MakeBuilder();
+ const flatbuffers::Offset<ModelBasedStatus> model_based_status =
+ model_based_.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());
+ 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>();
+ output_builder.add_x(model_based_.xytheta()(0));
+ output_builder.add_y(model_based_.xytheta()(1));
+ output_builder.add_theta(model_based_.xytheta()(2));
+ builder.CheckOk(builder.Send(output_builder.Finish()));
+ last_output_send_ = event_loop_->monotonic_now();
+ }
+ }
+ });
+}
+
+} // namespace frc971::controls
diff --git a/y2022/control_loops/localizer/localizer.h b/y2022/control_loops/localizer/localizer.h
new file mode 100644
index 0000000..f2d76ef
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer.h
@@ -0,0 +1,218 @@
+#ifndef Y2022_CONTROL_LOOPS_LOCALIZER_LOCALIZER_H_
+#define Y2022_CONTROL_LOOPS_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/control_loops/localizer/localizer_status_generated.h"
+#include "y2022/control_loops/localizer/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/zeroing/imu_zeroer.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.
+// * Tune down CPU usage to run on a pi.
+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;
+
+ 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);
+ }
+ }
+
+ AccelState accel_state() const { return current_state_.accel_state; };
+
+ 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);
+
+ 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> branches_;
+
+ CombinedState current_state_;
+ aos::monotonic_clock::time_point t_ = aos::monotonic_clock::min_time;
+ bool using_model_;
+
+ 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;
+
+ int clock_resets_ = 0;
+
+ friend class testing::LocalizerTest;
+};
+
+class EventLoopLocalizer {
+ public:
+ EventLoopLocalizer(
+ aos::EventLoop *event_loop,
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
+
+ private:
+ aos::EventLoop *event_loop_;
+ ModelBasedLocalizer model_based_;
+ aos::Sender<LocalizerStatus> status_sender_;
+ aos::Sender<LocalizerOutput> output_sender_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Position> position_fetcher_;
+ 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;
+};
+} // namespace frc971::controls
+#endif // Y2022_CONTROL_LOOPS_LOCALIZER_LOCALIZER_H_
diff --git a/y2022/control_loops/localizer/localizer_replay.cc b/y2022/control_loops/localizer/localizer_replay.cc
new file mode 100644
index 0000000..67fb35a
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer_replay.cc
@@ -0,0 +1,119 @@
+
+#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/control_loops/localizer/localizer.h"
+#include "y2022/control_loops/localizer/localizer_schema.h"
+#include "gflags/gflags.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+DEFINE_string(config, "y2020/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);
+ // Patch in any new channels.
+ // TODO(james): With some of the extra changes I've made recently, this is no
+ // longer adequate for replaying old logfiles. Just stop trying to support old
+ // logs.
+ aos::FlatbufferDetachedBuffer<aos::Configuration> updated_config =
+ aos::configuration::MergeWithConfig(
+ reader.configuration(),
+ aos::configuration::AddSchema(
+ R"channel({
+ "channels": [
+ {
+ "name": "/localizer",
+ "type": "frc971.controls.LocalizerStatus",
+ "source_node": "roborio",
+ "frequency": 2000,
+ "max_size": 2000,
+ "num_senders": 2
+ }
+ ]
+})channel",
+ {aos::FlatbufferVector<reflection::Schema>(
+ aos::FlatbufferSpan<reflection::Schema>(
+ frc971::controls::LocalizerStatusSchema()))}));
+
+ auto factory = std::make_unique<aos::SimulatedEventLoopFactory>(
+ &updated_config.message());
+
+ 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 = {"roborio"};
+ 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(), "roborio");
+ }
+
+ 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(),
+ y2020::control_loops::drivetrain::GetDrivetrainConfig());
+
+ reader.event_loop_factory()->Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2022/control_loops/localizer/localizer_test.cc b/y2022/control_loops/localizer/localizer_test.cc
new file mode 100644
index 0000000..5857bda
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer_test.cc
@@ -0,0 +1,525 @@
+#include "y2022/control_loops/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_) {}
+ 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{1.0, 0.2, 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(1.0, localizer_.accel_state()(kVelocityX), 1e-10);
+ ASSERT_NEAR(0.2, 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/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")) {
+ 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.1, 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.01, 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