From 0d0b8e286974df0fbef7e047bc880515950120b0 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 10:22:23 -0700 Subject: [PATCH 01/39] x --- src/software/simulation/er_force_simulator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 36374a54a8..d6935f0716 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -65,7 +65,7 @@ double sampleCorrelatedNoise(std::mt19937& rng, double& bias, double dt_seconds, } } // namespace -const std::string ErForceSimulator::CSV_OUTPUT_PATH = "/tmp/offense_play_test_master.csv"; +const std::string ErForceSimulator::CSV_OUTPUT_PATH = "/tmp/master_test_master.csv"; ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, const robot_constants::RobotConstants& robot_constants, From 323b750638b1508f0cb6bd16eb0db772c2ae00fd Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Tue, 15 Sep 2026 03:41:29 +0000 Subject: [PATCH 02/39] [pre-commit.ci lite] apply automatic fixes --- src/shared/robot_constants.cpp | 3 +- src/software/embedded/robot_localizer.cpp | 51 ++++++++----------- src/software/embedded/robot_localizer.h | 2 +- .../embedded/robot_localizer_test.cpp | 8 +-- src/software/embedded/thunderloop.cpp | 42 +++++++-------- 5 files changed, 51 insertions(+), 55 deletions(-) diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp index cbca9ea90d..77949c94f6 100644 --- a/src/shared/robot_constants.cpp +++ b/src/shared/robot_constants.cpp @@ -55,8 +55,7 @@ RobotConstants createRobotConstants() .kalman_process_noise_variance_rad_per_s_4 = 1.0f, .kalman_vision_noise_variance_rad_2 = 0.0001f, .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f, - .kalman_motor_sensor_noise_variance_m_per_s_2 = 0.05f - }; + .kalman_motor_sensor_noise_variance_m_per_s_2 = 0.05f}; } #elif CHECK_VERSION(2021) constexpr RobotConstants createRobotConstants() diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index fb154b740a..4f9f2b5194 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -57,8 +57,8 @@ void RobotLocalizer::update(const VisionData& data) [&](const FilterStep& step) { return (current_time_seconds_ - step.time_seconds) >= data.age_seconds; }); - // If rollback point is at the start, vision is newer than all history steps - // So we empty history and apply vision + // If rollback point is at the start, vision is newer than all history steps + // So we empty history and apply vision if (rollback_point == history.begin()) { updateFilterWithVision(data.position, data.orientation); @@ -66,8 +66,8 @@ void RobotLocalizer::update(const VisionData& data) return; } - // If rollback point is at the end, vision is older than all history steps - // So rollback ever step + // If rollback point is at the end, vision is older than all history steps + // So rollback ever step if (rollback_point == history.end()) { rollback_point = std::prev(history.end()); @@ -127,7 +127,7 @@ void RobotLocalizer::updateFilterWithVision(const Point& position, measurement(static_cast(MeasurementIndex::VISION_Y_POSITION)) = position.y(); - // Integrating omega for position makes angule goes out of bounds so we wrap it around + // Integrating omega for position makes angule goes out of bounds so we wrap it around measurement(static_cast(MeasurementIndex::VISION_ORIENTATION)) = orientation_estimate + (orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians(); @@ -222,7 +222,7 @@ RobotState RobotLocalizer::getRobotState() const getAngularVelocity()); } -// TODO: Investigate proces models/variances/etc +// TODO: Investigate process models/variances/etc void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) { // Velocity is estimated in the robot's local frame (see StateIndex), but position @@ -236,12 +236,9 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) filter_.process_model_function = [delta_time_seconds](Eigen::Vector state) { - const double theta = - state(static_cast(StateIndex::ORIENTATION)); - const double local_vx = - state(static_cast(StateIndex::X_VELOCITY)); - const double local_vy = - state(static_cast(StateIndex::Y_VELOCITY)); + const double theta = state(static_cast(StateIndex::ORIENTATION)); + const double local_vx = state(static_cast(StateIndex::X_VELOCITY)); + const double local_vy = state(static_cast(StateIndex::Y_VELOCITY)); Eigen::Vector next_state = Eigen::Vector::Zero(); @@ -266,20 +263,19 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) filter_.process_model_jacobian_function = [delta_time_seconds](Eigen::Vector state) { - const auto x_position_index = static_cast(StateIndex::X_POSITION); - const auto y_position_index = static_cast(StateIndex::Y_POSITION); - const auto orientation_index = - static_cast(StateIndex::ORIENTATION); - const auto x_velocity_index = static_cast(StateIndex::X_VELOCITY); - const auto y_velocity_index = static_cast(StateIndex::Y_VELOCITY); + const auto x_position_index = static_cast(StateIndex::X_POSITION); + const auto y_position_index = static_cast(StateIndex::Y_POSITION); + const auto orientation_index = static_cast(StateIndex::ORIENTATION); + const auto x_velocity_index = static_cast(StateIndex::X_VELOCITY); + const auto y_velocity_index = static_cast(StateIndex::Y_VELOCITY); const auto angular_velocity_index = static_cast(StateIndex::ANGULAR_VELOCITY); - const double theta = state(orientation_index); - const double local_vx = state(x_velocity_index); - const double local_vy = state(y_velocity_index); - const double cos_theta = std::cos(theta); - const double sin_theta = std::sin(theta); + const double theta = state(orientation_index); + const double local_vx = state(x_velocity_index); + const double local_vy = state(y_velocity_index); + const double cos_theta = std::cos(theta); + const double sin_theta = std::sin(theta); Eigen::Matrix jacobian = Eigen::Matrix::Identity(); @@ -355,18 +351,15 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) control_model.setZero(); control_model(static_cast(StateIndex::X_VELOCITY), - static_cast(ControlIndex::X_VELOCITY_TARGET)) = - cos_theta; + static_cast(ControlIndex::X_VELOCITY_TARGET)) = cos_theta; control_model(static_cast(StateIndex::X_VELOCITY), - static_cast(ControlIndex::Y_VELOCITY_TARGET)) = - sin_theta; + static_cast(ControlIndex::Y_VELOCITY_TARGET)) = sin_theta; control_model(static_cast(StateIndex::Y_VELOCITY), static_cast(ControlIndex::X_VELOCITY_TARGET)) = -sin_theta; control_model(static_cast(StateIndex::Y_VELOCITY), - static_cast(ControlIndex::Y_VELOCITY_TARGET)) = - cos_theta; + static_cast(ControlIndex::Y_VELOCITY_TARGET)) = cos_theta; } void RobotLocalizer::generateMeasurementModel(FilterStepType source) diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 14d023ca80..6d76b6fada 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -213,7 +213,7 @@ class RobotLocalizer // during replay (see generateMeasurementModel). std::optional> measurement; - // Post operation state + // Post operation state Eigen::Vector state_estimate; Eigen::Matrix state_covariance; diff --git a/src/software/embedded/robot_localizer_test.cpp b/src/software/embedded/robot_localizer_test.cpp index 52efe91fcb..e7b5df2339 100644 --- a/src/software/embedded/robot_localizer_test.cpp +++ b/src/software/embedded/robot_localizer_test.cpp @@ -44,7 +44,8 @@ RobotLocalizer runConstantVelocity(bool feed_vision, double vision_age = RTT_S / const Vector local_velocity = globalToLocalVelocity(true_velocity, true_orientation); - localizer.update(RobotLocalizer::MotorData{local_velocity, AngularVelocity::zero()}); + localizer.update( + RobotLocalizer::MotorData{local_velocity, AngularVelocity::zero()}); localizer.predict(Vector(0.0, 0.0), Duration::fromSeconds(DT)); @@ -69,8 +70,9 @@ TEST(RobotLocalizer, tracks_constant_forward_velocity) const RobotLocalizer localizer = runConstantVelocity(/*feed_vision=*/true); std::cerr << "[motor+vision] pos=(" << localizer.getPosition().x() << ", " - << localizer.getPosition().y() << ") vel=(" << localizer.getGlobalVelocity().x() - << ", " << localizer.getGlobalVelocity().y() + << localizer.getPosition().y() << ") vel=(" + << localizer.getGlobalVelocity().x() << ", " + << localizer.getGlobalVelocity().y() << ") orient=" << localizer.getOrientation().toDegrees() << "deg\n"; // NOTE: we assert on velocity and orientation, not absolute position. RobotLocalizer diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index c3348d124a..2175c398cc 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -269,26 +269,28 @@ void Thunderloop::updateRobotLocalizer(const TbotsProto::Primitive& primitive) void Thunderloop::updateRobotLocalizer(const TbotsProto::RobotStatus& robot_status) { - // Seperate update is okay because measurement model is linear - if (robot_status.has_imu_status()){ - AngularVelocity res = createAngularVelocity(robot_status.imu_status().angular_velocity()); - if (res <0.1){ - res = 0; - } - robot_localizer_->update(RobotLocalizer::ImuData{ - createAngularVelocity(0)}); - } + // Separate update is okay because measurement model is linear + if (robot_status.has_imu_status()) + { + AngularVelocity res = + createAngularVelocity(robot_status.imu_status().angular_velocity()); + if (res < 0.1) + { + res = 0; + } + robot_localizer_->update(RobotLocalizer::ImuData{createAngularVelocity(0)}); + } if (robot_status.has_motor_status()) - Vector velocity = robot_status.motor_status().local_velocity(); - if ( velocity.x() <0.05 && velocity.y() <0.05 ) { - velocity = Vector(0,0); - } - AngularVelocity angular_velocity = robot_status.motor_status().angular_velocity(); - if ( angular_velocity.toRadians() <0.1 ) { - angular_velocity = Angle::zero(); - } - robot_localizer_->update(RobotLocalizer::MotorData{ - velocity, angular_velocity -}); + Vector velocity = robot_status.motor_status().local_velocity(); + if (velocity.x() < 0.05 && velocity.y() < 0.05) + { + velocity = Vector(0, 0); + } + AngularVelocity angular_velocity = robot_status.motor_status().angular_velocity(); + if (angular_velocity.toRadians() < 0.1) + { + angular_velocity = Angle::zero(); } + robot_localizer_->update(RobotLocalizer::MotorData{velocity, angular_velocity}); +} } From 8ec3bfc43eac7ff7a981233ee3841a854f7d18ab Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 17:53:12 -0700 Subject: [PATCH 03/39] dynamicaly compute matrices after vision rollback instead of using old estiamte --- src/software/embedded/robot_localizer.cpp | 58 +++++++++++++---------- src/software/embedded/robot_localizer.h | 10 ++++ src/software/embedded/thunderloop.cpp | 10 +++- 3 files changed, 52 insertions(+), 26 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index cda68b5607..5e5bd34b73 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -20,21 +20,10 @@ RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) .asDiagonal(); } -void RobotLocalizer::step(const Vector& linear_acceleration, const Duration& delta_time) +void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) { - const double delta_time_seconds = delta_time.toSeconds(); - current_time_seconds_ += delta_time_seconds; - - FilterStep step{ - .prediction = std::make_optional(), - .update = std::nullopt, - .state_estimate = filter_.state_estimate, - .state_covariance = filter_.state_covariance, - .time_seconds = current_time_seconds_, - }; - // clang-format off - step.prediction->process_model << + filter_.process_model << 1, 0, 0, delta_time_seconds, 0, 0, 0, 1, 0, 0, delta_time_seconds, 0, 0, 0, 1, 0, 0, delta_time_seconds, @@ -47,7 +36,7 @@ void RobotLocalizer::step(const Vector& linear_acceleration, const Duration& del const double delta_time_cubed = delta_time_squared * delta_time_seconds; const double delta_time_fourth = delta_time_cubed * delta_time_seconds; - auto& process_covariance = step.prediction->process_covariance; + auto& process_covariance = filter_.process_covariance; process_covariance.setZero(); process_covariance(static_cast(StateIndex::X_POSITION), @@ -98,7 +87,7 @@ void RobotLocalizer::step(const Vector& linear_acceleration, const Duration& del static_cast(StateIndex::ANGULAR_VELOCITY)) = delta_time_squared * process_angular_acceleration_noise_variance_; - auto& control_model = step.prediction->control_model; + auto& control_model = filter_.control_model; control_model.setZero(); control_model(static_cast(StateIndex::X_POSITION), @@ -116,12 +105,27 @@ void RobotLocalizer::step(const Vector& linear_acceleration, const Duration& del control_model(static_cast(StateIndex::Y_VELOCITY), static_cast(ControlIndex::Y_ACCELERATION)) = delta_time_seconds; +} - step.prediction->control_input << linear_acceleration.x(), linear_acceleration.y(); +void RobotLocalizer::step(const Vector& linear_acceleration, const Duration& delta_time) +{ + const double delta_time_seconds = delta_time.toSeconds(); + current_time_seconds_ += delta_time_seconds; + + generatedPredictionMatrices(delta_time_seconds); + + FilterStep step{ + .prediction = std::make_optional(), + .update = std::nullopt, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time_seconds = current_time_seconds_, + }; - filter_.process_model = step.prediction->process_model; - filter_.process_covariance = step.prediction->process_covariance; - filter_.control_model = step.prediction->control_model; + step.prediction->process_model = filter_.process_model; + step.prediction->process_covariance = filter_.process_covariance; + step.prediction->control_model = filter_.control_model; + step.prediction->control_input << linear_acceleration.x(), linear_acceleration.y(); history.push_front(step); filter_.predict(step.prediction->control_input); @@ -169,21 +173,25 @@ void RobotLocalizer::update(const VisionData& data) history.erase(std::next(rollback_point), history.end()); // 3. Apply the delayed vision measurement at the rolled-back time. - updateFilterWithVision(data.position, data.orientation); + updateFilterWithVision(data.position, data.orientation); // 4. Replay the remaining history (from oldest to newest), including the rollback // point's own operation. + double prev_time = rollback_point.time_seconds; for (auto it = history.rbegin(); it != history.rend(); ++it) { + double elapsed_time = it.time_seconds - prev_time; if (it->prediction.has_value()) { - const auto& prediction = it->prediction.value(); - filter_.process_model = prediction.process_model; - filter_.process_covariance = prediction.process_covariance; - filter_.control_model = prediction.control_model; + const auto& prediction = it->prediction.value(); + if (it==history.rbegin()){ + generatedPredictionMatrices(it.time_seconds - (current_time_seconds_ - data.age_seconds)); + } + else{ + generatedPredictionMatrices(elapsed_time); + } filter_.predict(prediction.control_input); } - if (it->update.has_value()) { const auto& update = it->update.value(); diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 07a718adf1..851112fcce 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -144,6 +144,16 @@ class RobotLocalizer */ void updateFilterWithVision(const Point& position, const Angle& orientation); + /** + * Computes the process model, process covariance, and control model for the + * given elapsed time, and writes them into the filter. Does not run the + * predict step itself. + * + * @param delta_time_seconds The elapsed time to generate the prediction + * matrices for + */ + void generatedPredictionMatrices(double delta_time_seconds); + static constexpr size_t STATE_SIZE = reflective_enum::size(); static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size(); static constexpr size_t CONTROL_SIZE = reflective_enum::size(); diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index fd88172a42..675b132e79 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -7,6 +7,7 @@ #include #include +#include "proto/message_translation/tbots_geometry.h" #include "proto/message_translation/tbots_protobuf.h" #include "proto/primitive/primitive_msg_factory.h" #include "proto/robot_crash_msg.pb.h" @@ -213,6 +214,7 @@ void Thunderloop::runLoop() updateRobotLocalizer(primitive.value()); primitive_executor_->updatePrimitive(primitive.value(), robot_status_); } + imu_service_->poll(robot_status_); robot_localizer_->step(Vector(), delta_time); updateRobotLocalizer(robot_status_); @@ -222,7 +224,6 @@ void Thunderloop::runLoop() const TbotsProto::DirectControlPrimitive direct_control_primitive = primitive_executor_->stepPrimitive(robot_status_, delta_time); - imu_service_->poll(robot_status_); #ifndef DISABLE_MOTOR_SERVICE motor_service_->poll(direct_control_primitive, robot_status_, delta_time); @@ -267,6 +268,13 @@ void Thunderloop::updateRobotLocalizer(const TbotsProto::Primitive& primitive) void Thunderloop::updateRobotLocalizer(const TbotsProto::RobotStatus& robot_status) { + // Seperate update is okay because measurement model is linear + if (robot_status.has_imu_status()){ + robot_localizer_.update(RobotLocalizer::ImuData{ + createAngularVelocity(robot_status.imu_status().angular_velocity()) + }) + + } if (robot_status.has_motor_status()) { robot_localizer_->update(RobotLocalizer::MotorData{ From a3d97c27013765cdb9043bf5f2d5581ffd323570 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 17:55:13 -0700 Subject: [PATCH 04/39] store post update/prediction state, instead of the state before for consistency --- src/software/embedded/robot_localizer.cpp | 41 +++++++++++------------ src/software/embedded/robot_localizer.h | 2 ++ 2 files changed, 22 insertions(+), 21 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 5e5bd34b73..cbbb891420 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -114,21 +114,22 @@ void RobotLocalizer::step(const Vector& linear_acceleration, const Duration& del generatedPredictionMatrices(delta_time_seconds); - FilterStep step{ - .prediction = std::make_optional(), + FilterStep::Predict prediction{ + .process_model = filter_.process_model, + .process_covariance = filter_.process_covariance, + .control_model = filter_.control_model, + }; + prediction.control_input << linear_acceleration.x(), linear_acceleration.y(); + + filter_.predict(prediction.control_input); + + history.push_front(FilterStep{ + .prediction = prediction, .update = std::nullopt, .state_estimate = filter_.state_estimate, .state_covariance = filter_.state_covariance, .time_seconds = current_time_seconds_, - }; - - step.prediction->process_model = filter_.process_model; - step.prediction->process_covariance = filter_.process_covariance; - step.prediction->control_model = filter_.control_model; - step.prediction->control_input << linear_acceleration.x(), linear_acceleration.y(); - - history.push_front(step); - filter_.predict(step.prediction->control_input); + }); } void RobotLocalizer::update(const VisionData& data) @@ -264,16 +265,15 @@ void RobotLocalizer::update(const MotorData& data) update.measurement(static_cast( MeasurementIndex::MOTOR_ANGULAR_VELOCITY)) = data.angular_velocity.toRadians(); - const FilterStep step{ + filter_.update(update.measurement); + + history.push_front(FilterStep{ .prediction = std::nullopt, .update = update, .state_estimate = filter_.state_estimate, .state_covariance = filter_.state_covariance, .time_seconds = current_time_seconds_, - }; - - history.push_front(step); - filter_.update(step.update->measurement); + }); } void RobotLocalizer::update(const ImuData& data) @@ -291,16 +291,15 @@ void RobotLocalizer::update(const ImuData& data) update.measurement(static_cast( MeasurementIndex::IMU_ANGULAR_VELOCITY)) = data.angular_velocity.toRadians(); - const FilterStep step{ + filter_.update(update.measurement); + + history.push_front(FilterStep{ .prediction = std::nullopt, .update = update, .state_estimate = filter_.state_estimate, .state_covariance = filter_.state_covariance, .time_seconds = current_time_seconds_, - }; - - history.push_front(step); - filter_.update(step.update->measurement); + }); } Point RobotLocalizer::getPosition() const diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 851112fcce..bb4170e7ca 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -180,6 +180,8 @@ class RobotLocalizer std::optional prediction; std::optional update; + // Filter state captured immediately after this step's own operation ran + // (i.e. post-operation, not pre-operation). Eigen::Vector state_estimate; Eigen::Matrix state_covariance; From 4b99c946c9b933848938556c7e770794f15910dd Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 17:57:02 -0700 Subject: [PATCH 05/39] fix rollback cnsistency --- src/software/embedded/robot_localizer.cpp | 39 ++++++++++------------- 1 file changed, 17 insertions(+), 22 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index cbbb891420..1aac0b499b 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -161,38 +161,34 @@ void RobotLocalizer::update(const VisionData& data) rollback_point = std::prev(history.end()); } - // 1. Roll the filter back to the state from just before the rollback point's - // operation (its stored state is captured pre-operation). + // 1. Roll the filter back to the state right after the rollback point's own + // operation ran (state is captured post-operation), since the rollback point + // happened at or before the vision sample's timestamp. filter_.state_estimate = rollback_point->state_estimate; filter_.state_covariance = rollback_point->state_covariance; - // 2. Drop everything strictly older than the rollback point, but KEEP the rollback - // point itself so its operation is replayed after the vision measurement is - // inserted. (Erasing the rollback point too would silently drop one - // predict/update step on every vision frame, making the position estimate lag - // and inflating the velocity estimate through the position/velocity covariance.) - history.erase(std::next(rollback_point), history.end()); + // 2. Drop the rollback point and everything older than it. Its operation is + // already reflected in the state we just restored, so it doesn't need to be + // replayed again. + history.erase(rollback_point, history.end()); // 3. Apply the delayed vision measurement at the rolled-back time. - updateFilterWithVision(data.position, data.orientation); + updateFilterWithVision(data.position, data.orientation); - // 4. Replay the remaining history (from oldest to newest), including the rollback - // point's own operation. - double prev_time = rollback_point.time_seconds; + // 4. Replay the remaining (newer) history, from oldest to newest, recomputing each + // predict step's elapsed time from the vision sample's true timestamp so the + // inserted correction doesn't skew the replayed intervals. + double prev_time = current_time_seconds_ - data.age_seconds; for (auto it = history.rbegin(); it != history.rend(); ++it) { - double elapsed_time = it.time_seconds - prev_time; if (it->prediction.has_value()) { - const auto& prediction = it->prediction.value(); - if (it==history.rbegin()){ - generatedPredictionMatrices(it.time_seconds - (current_time_seconds_ - data.age_seconds)); - } - else{ - generatedPredictionMatrices(elapsed_time); - } + const auto& prediction = it->prediction.value(); + generatedPredictionMatrices(it->time_seconds - prev_time); filter_.predict(prediction.control_input); + prev_time = it->time_seconds; } + if (it->update.has_value()) { const auto& update = it->update.value(); @@ -200,8 +196,7 @@ void RobotLocalizer::update(const VisionData& data) filter_.update(update.measurement); } - // IMPORTANT: Update the history with the recomputed state so future rollbacks are - // correct + // Update the history with the recomputed state so future rollbacks are correct it->state_estimate = filter_.state_estimate; it->state_covariance = filter_.state_covariance; } From 4fe2930cc087d11d2b29bd1a8ed5ed62ef7215a1 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 18:02:23 -0700 Subject: [PATCH 06/39] generateMeasurementModel for better clarity in code --- src/software/embedded/robot_localizer.cpp | 63 ++++++++++++++--------- src/software/embedded/robot_localizer.h | 9 ++++ 2 files changed, 48 insertions(+), 24 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 1aac0b499b..baed4728bd 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -107,6 +107,42 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) delta_time_seconds; } +void RobotLocalizer::generateMeasurementModel(MeasurementSource source) +{ + filter_.measurement_model.setZero(); + + switch (source) + { + case MeasurementSource::VISION_DATA: + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_X_POSITION), + static_cast(StateIndex::X_POSITION)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_Y_POSITION), + static_cast(StateIndex::Y_POSITION)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_ORIENTATION), + static_cast(StateIndex::ORIENTATION)) = 1; + break; + case MeasurementSource::MOTOR_DATA: + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_X_VELOCITY), + static_cast(StateIndex::X_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_Y_VELOCITY), + static_cast(StateIndex::Y_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + break; + case MeasurementSource::IMU_DATA: + filter_.measurement_model( + static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + break; + } +} + void RobotLocalizer::step(const Vector& linear_acceleration, const Duration& delta_time) { const double delta_time_seconds = delta_time.toSeconds(); @@ -221,32 +257,14 @@ void RobotLocalizer::updateFilterWithVision(const Point& position, orientation_estimate + (orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians(); - filter_.measurement_model.setZero(); - filter_.measurement_model( - static_cast(MeasurementIndex::VISION_X_POSITION), - static_cast(StateIndex::X_POSITION)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::VISION_Y_POSITION), - static_cast(StateIndex::Y_POSITION)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::VISION_ORIENTATION), - static_cast(StateIndex::ORIENTATION)) = 1; + generateMeasurementModel(MeasurementSource::VISION_DATA); filter_.update(measurement); } void RobotLocalizer::update(const MotorData& data) { - filter_.measurement_model.setZero(); - filter_.measurement_model( - static_cast(MeasurementIndex::MOTOR_X_VELOCITY), - static_cast(StateIndex::X_VELOCITY)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::MOTOR_Y_VELOCITY), - static_cast(StateIndex::Y_VELOCITY)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY), - static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + generateMeasurementModel(MeasurementSource::MOTOR_DATA); FilterStep::Update update{ .measurement_model = filter_.measurement_model, @@ -273,10 +291,7 @@ void RobotLocalizer::update(const MotorData& data) void RobotLocalizer::update(const ImuData& data) { - filter_.measurement_model.setZero(); - filter_.measurement_model( - static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY), - static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + generateMeasurementModel(MeasurementSource::IMU_DATA); FilterStep::Update update{ .measurement_model = filter_.measurement_model, diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index bb4170e7ca..3ebfd625ff 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -24,6 +24,8 @@ MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTA MAKE_ENUM(ControlIndex, X_ACCELERATION, Y_ACCELERATION); +MAKE_ENUM(MeasurementSource, MOTOR_DATA, IMU_DATA, VISION_DATA); + /** * Estimates robot orientation, angular velocity, and angular acceleration * using a Kalman filter. @@ -154,6 +156,13 @@ class RobotLocalizer */ void generatedPredictionMatrices(double delta_time_seconds); + /** + * Writes the measurement model for the given data source into the filter. + * + * @param source Which sensor's measurement model to generate + */ + void generateMeasurementModel(MeasurementSource source); + static constexpr size_t STATE_SIZE = reflective_enum::size(); static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size(); static constexpr size_t CONTROL_SIZE = reflective_enum::size(); From 46c5083da802611a672e241d77123310d5c0ac53 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 18:03:38 -0700 Subject: [PATCH 07/39] store enum instead of full matrix --- src/software/embedded/robot_localizer.cpp | 12 ++++++------ src/software/embedded/robot_localizer.h | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index baed4728bd..0b98538497 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -227,8 +227,8 @@ void RobotLocalizer::update(const VisionData& data) if (it->update.has_value()) { - const auto& update = it->update.value(); - filter_.measurement_model = update.measurement_model; + const auto& update = it->update.value(); + generateMeasurementModel(update.source); filter_.update(update.measurement); } @@ -267,8 +267,8 @@ void RobotLocalizer::update(const MotorData& data) generateMeasurementModel(MeasurementSource::MOTOR_DATA); FilterStep::Update update{ - .measurement_model = filter_.measurement_model, - .measurement = Eigen::Vector::Zero(), + .source = MeasurementSource::MOTOR_DATA, + .measurement = Eigen::Vector::Zero(), }; update.measurement(static_cast(MeasurementIndex::MOTOR_X_VELOCITY)) = @@ -294,8 +294,8 @@ void RobotLocalizer::update(const ImuData& data) generateMeasurementModel(MeasurementSource::IMU_DATA); FilterStep::Update update{ - .measurement_model = filter_.measurement_model, - .measurement = Eigen::Vector::Zero(), + .source = MeasurementSource::IMU_DATA, + .measurement = Eigen::Vector::Zero(), }; update.measurement(static_cast( diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 3ebfd625ff..1ce01e79fe 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -182,7 +182,7 @@ class RobotLocalizer struct Update { - Eigen::Matrix measurement_model; + MeasurementSource source; Eigen::Vector measurement; }; From c4e6ba6c1188811a494ed371e7a7b65b55c40bae Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 18:05:17 -0700 Subject: [PATCH 08/39] reorder for clairy --- src/software/embedded/robot_localizer.cpp | 246 +++++++++++----------- 1 file changed, 123 insertions(+), 123 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 0b98538497..ffd960669f 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -20,129 +20,6 @@ RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) .asDiagonal(); } -void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) -{ - // clang-format off - filter_.process_model << - 1, 0, 0, delta_time_seconds, 0, 0, - 0, 1, 0, 0, delta_time_seconds, 0, - 0, 0, 1, 0, 0, delta_time_seconds, - 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1; - // clang-format on - - const double delta_time_squared = delta_time_seconds * delta_time_seconds; - const double delta_time_cubed = delta_time_squared * delta_time_seconds; - const double delta_time_fourth = delta_time_cubed * delta_time_seconds; - - auto& process_covariance = filter_.process_covariance; - process_covariance.setZero(); - - process_covariance(static_cast(StateIndex::X_POSITION), - static_cast(StateIndex::X_POSITION)) = - delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::X_POSITION), - static_cast(StateIndex::X_VELOCITY)) = - delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::X_VELOCITY), - static_cast(StateIndex::X_POSITION)) = - delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::X_VELOCITY), - static_cast(StateIndex::X_VELOCITY)) = - delta_time_squared * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::Y_POSITION), - static_cast(StateIndex::Y_POSITION)) = - delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::Y_POSITION), - static_cast(StateIndex::Y_VELOCITY)) = - delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::Y_VELOCITY), - static_cast(StateIndex::Y_POSITION)) = - delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::Y_VELOCITY), - static_cast(StateIndex::Y_VELOCITY)) = - delta_time_squared * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::ORIENTATION), - static_cast(StateIndex::ORIENTATION)) = - delta_time_fourth / 4 * process_angular_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::ORIENTATION), - static_cast(StateIndex::ANGULAR_VELOCITY)) = - delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), - static_cast(StateIndex::ORIENTATION)) = - delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), - static_cast(StateIndex::ANGULAR_VELOCITY)) = - delta_time_squared * process_angular_acceleration_noise_variance_; - - auto& control_model = filter_.control_model; - control_model.setZero(); - - control_model(static_cast(StateIndex::X_POSITION), - static_cast(ControlIndex::X_ACCELERATION)) = - delta_time_squared / 2; - - control_model(static_cast(StateIndex::Y_POSITION), - static_cast(ControlIndex::Y_ACCELERATION)) = - delta_time_squared / 2; - - control_model(static_cast(StateIndex::X_VELOCITY), - static_cast(ControlIndex::X_ACCELERATION)) = - delta_time_seconds; - - control_model(static_cast(StateIndex::Y_VELOCITY), - static_cast(ControlIndex::Y_ACCELERATION)) = - delta_time_seconds; -} - -void RobotLocalizer::generateMeasurementModel(MeasurementSource source) -{ - filter_.measurement_model.setZero(); - - switch (source) - { - case MeasurementSource::VISION_DATA: - filter_.measurement_model( - static_cast(MeasurementIndex::VISION_X_POSITION), - static_cast(StateIndex::X_POSITION)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::VISION_Y_POSITION), - static_cast(StateIndex::Y_POSITION)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::VISION_ORIENTATION), - static_cast(StateIndex::ORIENTATION)) = 1; - break; - case MeasurementSource::MOTOR_DATA: - filter_.measurement_model( - static_cast(MeasurementIndex::MOTOR_X_VELOCITY), - static_cast(StateIndex::X_VELOCITY)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::MOTOR_Y_VELOCITY), - static_cast(StateIndex::Y_VELOCITY)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY), - static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; - break; - case MeasurementSource::IMU_DATA: - filter_.measurement_model( - static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY), - static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; - break; - } -} - void RobotLocalizer::step(const Vector& linear_acceleration, const Duration& delta_time) { const double delta_time_seconds = delta_time.toSeconds(); @@ -344,3 +221,126 @@ RobotState RobotLocalizer::getRobotState() const return RobotState(getPosition(), getVelocity(), getOrientation(), getAngularVelocity()); } + +void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) +{ + // clang-format off + filter_.process_model << + 1, 0, 0, delta_time_seconds, 0, 0, + 0, 1, 0, 0, delta_time_seconds, 0, + 0, 0, 1, 0, 0, delta_time_seconds, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; + // clang-format on + + const double delta_time_squared = delta_time_seconds * delta_time_seconds; + const double delta_time_cubed = delta_time_squared * delta_time_seconds; + const double delta_time_fourth = delta_time_cubed * delta_time_seconds; + + auto& process_covariance = filter_.process_covariance; + process_covariance.setZero(); + + process_covariance(static_cast(StateIndex::X_POSITION), + static_cast(StateIndex::X_POSITION)) = + delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_POSITION), + static_cast(StateIndex::X_VELOCITY)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_VELOCITY), + static_cast(StateIndex::X_POSITION)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_VELOCITY), + static_cast(StateIndex::X_VELOCITY)) = + delta_time_squared * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_POSITION), + static_cast(StateIndex::Y_POSITION)) = + delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_POSITION), + static_cast(StateIndex::Y_VELOCITY)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_VELOCITY), + static_cast(StateIndex::Y_POSITION)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_VELOCITY), + static_cast(StateIndex::Y_VELOCITY)) = + delta_time_squared * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ORIENTATION), + static_cast(StateIndex::ORIENTATION)) = + delta_time_fourth / 4 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ORIENTATION), + static_cast(StateIndex::ANGULAR_VELOCITY)) = + delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), + static_cast(StateIndex::ORIENTATION)) = + delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = + delta_time_squared * process_angular_acceleration_noise_variance_; + + auto& control_model = filter_.control_model; + control_model.setZero(); + + control_model(static_cast(StateIndex::X_POSITION), + static_cast(ControlIndex::X_ACCELERATION)) = + delta_time_squared / 2; + + control_model(static_cast(StateIndex::Y_POSITION), + static_cast(ControlIndex::Y_ACCELERATION)) = + delta_time_squared / 2; + + control_model(static_cast(StateIndex::X_VELOCITY), + static_cast(ControlIndex::X_ACCELERATION)) = + delta_time_seconds; + + control_model(static_cast(StateIndex::Y_VELOCITY), + static_cast(ControlIndex::Y_ACCELERATION)) = + delta_time_seconds; +} + +void RobotLocalizer::generateMeasurementModel(MeasurementSource source) +{ + filter_.measurement_model.setZero(); + + switch (source) + { + case MeasurementSource::VISION_DATA: + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_X_POSITION), + static_cast(StateIndex::X_POSITION)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_Y_POSITION), + static_cast(StateIndex::Y_POSITION)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_ORIENTATION), + static_cast(StateIndex::ORIENTATION)) = 1; + break; + case MeasurementSource::MOTOR_DATA: + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_X_VELOCITY), + static_cast(StateIndex::X_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_Y_VELOCITY), + static_cast(StateIndex::Y_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + break; + case MeasurementSource::IMU_DATA: + filter_.measurement_model( + static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + break; + } +} From 38df81bccba36a6a6d55d9d98bbe60ffef0889ad Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 18:23:11 -0700 Subject: [PATCH 09/39] remove filter step structs --- src/software/embedded/robot_localizer.cpp | 134 +++++++++------------- src/software/embedded/robot_localizer.h | 25 +--- 2 files changed, 58 insertions(+), 101 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index ffd960669f..21c21d1eb8 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -27,21 +27,18 @@ void RobotLocalizer::step(const Vector& linear_acceleration, const Duration& del generatedPredictionMatrices(delta_time_seconds); - FilterStep::Predict prediction{ - .process_model = filter_.process_model, - .process_covariance = filter_.process_covariance, - .control_model = filter_.control_model, - }; - prediction.control_input << linear_acceleration.x(), linear_acceleration.y(); + Eigen::Vector control_input; + control_input << linear_acceleration.x(), linear_acceleration.y(); - filter_.predict(prediction.control_input); + filter_.predict(control_input); history.push_front(FilterStep{ - .prediction = prediction, - .update = std::nullopt, - .state_estimate = filter_.state_estimate, - .state_covariance = filter_.state_covariance, - .time_seconds = current_time_seconds_, + .control_input = control_input, + .measurement_source = std::nullopt, + .measurement = std::nullopt, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time_seconds = current_time_seconds_, }); } @@ -58,19 +55,19 @@ void RobotLocalizer::update(const VisionData& data) [&](const FilterStep& step) { return (current_time_seconds_ - step.time_seconds) >= data.age_seconds; }); + // If rollback point is at the start, vision is newer than all history steps + // So we empty history and apply vision if (rollback_point == history.begin()) { - // All history predates the sample, or is exactly at the same time. - // No need to rollback, just apply to the current state. updateFilterWithVision(data.position, data.orientation); history.clear(); // Safe to clear, since all history is older than current state return; } + // If rollback point is at the end, vision is older than all history steps + // So rollback ever step if (rollback_point == history.end()) { - // The vision sample is older than our entire history. - // Roll back as far as we can (to the oldest step). rollback_point = std::prev(history.end()); } @@ -94,19 +91,17 @@ void RobotLocalizer::update(const VisionData& data) double prev_time = current_time_seconds_ - data.age_seconds; for (auto it = history.rbegin(); it != history.rend(); ++it) { - if (it->prediction.has_value()) + if (it->control_input.has_value()) { - const auto& prediction = it->prediction.value(); generatedPredictionMatrices(it->time_seconds - prev_time); - filter_.predict(prediction.control_input); + filter_.predict(it->control_input.value()); prev_time = it->time_seconds; } - if (it->update.has_value()) + if (it->measurement.has_value()) { - const auto& update = it->update.value(); - generateMeasurementModel(update.source); - filter_.update(update.measurement); + generateMeasurementModel(it->measurement_source.value()); + filter_.update(it->measurement.value()); } // Update the history with the recomputed state so future rollbacks are correct @@ -118,6 +113,8 @@ void RobotLocalizer::update(const VisionData& data) void RobotLocalizer::updateFilterWithVision(const Point& position, const Angle& orientation) { + generateMeasurementModel(MeasurementSource::VISION_DATA); + const double orientation_estimate = filter_.state_estimate(static_cast(StateIndex::ORIENTATION)); @@ -129,12 +126,11 @@ void RobotLocalizer::updateFilterWithVision(const Point& position, measurement(static_cast(MeasurementIndex::VISION_Y_POSITION)) = position.y(); - // Coterminal angle that is closest to current estimate + // Integrating omega for position makes angule goes out of bounds so we wrap it around measurement(static_cast(MeasurementIndex::VISION_ORIENTATION)) = orientation_estimate + (orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians(); - generateMeasurementModel(MeasurementSource::VISION_DATA); filter_.update(measurement); } @@ -143,26 +139,25 @@ void RobotLocalizer::update(const MotorData& data) { generateMeasurementModel(MeasurementSource::MOTOR_DATA); - FilterStep::Update update{ - .source = MeasurementSource::MOTOR_DATA, - .measurement = Eigen::Vector::Zero(), - }; + Eigen::Vector measurement = + Eigen::Vector::Zero(); - update.measurement(static_cast(MeasurementIndex::MOTOR_X_VELOCITY)) = + measurement(static_cast(MeasurementIndex::MOTOR_X_VELOCITY)) = data.velocity.x(); - update.measurement(static_cast(MeasurementIndex::MOTOR_Y_VELOCITY)) = + measurement(static_cast(MeasurementIndex::MOTOR_Y_VELOCITY)) = data.velocity.y(); - update.measurement(static_cast( - MeasurementIndex::MOTOR_ANGULAR_VELOCITY)) = data.angular_velocity.toRadians(); + measurement(static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY)) = + data.angular_velocity.toRadians(); - filter_.update(update.measurement); + filter_.update(measurement); history.push_front(FilterStep{ - .prediction = std::nullopt, - .update = update, - .state_estimate = filter_.state_estimate, - .state_covariance = filter_.state_covariance, - .time_seconds = current_time_seconds_, + .control_input = std::nullopt, + .measurement_source = MeasurementSource::MOTOR_DATA, + .measurement = measurement, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time_seconds = current_time_seconds_, }); } @@ -238,57 +233,32 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) const double delta_time_cubed = delta_time_squared * delta_time_seconds; const double delta_time_fourth = delta_time_cubed * delta_time_seconds; - auto& process_covariance = filter_.process_covariance; - process_covariance.setZero(); - - process_covariance(static_cast(StateIndex::X_POSITION), - static_cast(StateIndex::X_POSITION)) = + const double linear_position_variance = delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::X_POSITION), - static_cast(StateIndex::X_VELOCITY)) = - delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::X_VELOCITY), - static_cast(StateIndex::X_POSITION)) = + const double linear_position_velocity_covariance = delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::X_VELOCITY), - static_cast(StateIndex::X_VELOCITY)) = + const double linear_velocity_variance = delta_time_squared * process_linear_acceleration_noise_variance_; - process_covariance(static_cast(StateIndex::Y_POSITION), - static_cast(StateIndex::Y_POSITION)) = - delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::Y_POSITION), - static_cast(StateIndex::Y_VELOCITY)) = - delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::Y_VELOCITY), - static_cast(StateIndex::Y_POSITION)) = - delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::Y_VELOCITY), - static_cast(StateIndex::Y_VELOCITY)) = - delta_time_squared * process_linear_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::ORIENTATION), - static_cast(StateIndex::ORIENTATION)) = + const double angular_position_variance = delta_time_fourth / 4 * process_angular_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::ORIENTATION), - static_cast(StateIndex::ANGULAR_VELOCITY)) = - delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), - static_cast(StateIndex::ORIENTATION)) = + const double angular_position_velocity_covariance = delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; - - process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), - static_cast(StateIndex::ANGULAR_VELOCITY)) = + const double angular_velocity_variance = delta_time_squared * process_angular_acceleration_noise_variance_; + // State order: X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY, + // ANGULAR_VELOCITY + // clang-format off + filter_.process_covariance << + linear_position_variance, 0, 0, linear_position_velocity_covariance, 0, 0, + 0, linear_position_variance, 0, 0, linear_position_velocity_covariance, 0, + 0, 0, angular_position_variance, 0, 0, angular_position_velocity_covariance, + linear_position_velocity_covariance, 0, 0, linear_velocity_variance, 0, 0, + 0, linear_position_velocity_covariance, 0, 0, linear_velocity_variance, 0, + 0, 0, angular_position_velocity_covariance, 0, 0, angular_velocity_variance; + // clang-format on + auto& control_model = filter_.control_model; control_model.setZero(); diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 1ce01e79fe..41e53acf2c 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -172,25 +172,12 @@ class RobotLocalizer */ struct FilterStep { - struct Predict - { - Eigen::Matrix process_model; - Eigen::Matrix process_covariance; - Eigen::Matrix control_model; - Eigen::Vector control_input; - }; - - struct Update - { - MeasurementSource source; - Eigen::Vector measurement; - }; - - std::optional prediction; - std::optional update; - - // Filter state captured immediately after this step's own operation ran - // (i.e. post-operation, not pre-operation). + std::optional> control_input; + + std::optional measurement_source; + std::optional> measurement; + + // Post operation state Eigen::Vector state_estimate; Eigen::Matrix state_covariance; From c8d1025dda1af1ab80fa177c5a0995a5ea8327ce Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 18:23:23 -0700 Subject: [PATCH 10/39] keep removing filter step structs --- src/software/embedded/robot_localizer.cpp | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 21c21d1eb8..5c1bba55ac 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -165,22 +165,21 @@ void RobotLocalizer::update(const ImuData& data) { generateMeasurementModel(MeasurementSource::IMU_DATA); - FilterStep::Update update{ - .source = MeasurementSource::IMU_DATA, - .measurement = Eigen::Vector::Zero(), - }; + Eigen::Vector measurement = + Eigen::Vector::Zero(); - update.measurement(static_cast( - MeasurementIndex::IMU_ANGULAR_VELOCITY)) = data.angular_velocity.toRadians(); + measurement(static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY)) = + data.angular_velocity.toRadians(); - filter_.update(update.measurement); + filter_.update(measurement); history.push_front(FilterStep{ - .prediction = std::nullopt, - .update = update, - .state_estimate = filter_.state_estimate, - .state_covariance = filter_.state_covariance, - .time_seconds = current_time_seconds_, + .control_input = std::nullopt, + .measurement_source = MeasurementSource::IMU_DATA, + .measurement = measurement, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time_seconds = current_time_seconds_, }); } From fb2ca0d2040f5e7dbe594189f0d728e20035122c Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 18:27:22 -0700 Subject: [PATCH 11/39] Use mandatory FilterStepType insted of optionals checking --- src/software/embedded/robot_localizer.cpp | 60 ++++++++++++----------- src/software/embedded/robot_localizer.h | 15 ++++-- 2 files changed, 42 insertions(+), 33 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 5c1bba55ac..9fe308cd6a 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -33,12 +33,12 @@ void RobotLocalizer::step(const Vector& linear_acceleration, const Duration& del filter_.predict(control_input); history.push_front(FilterStep{ - .control_input = control_input, - .measurement_source = std::nullopt, - .measurement = std::nullopt, - .state_estimate = filter_.state_estimate, - .state_covariance = filter_.state_covariance, - .time_seconds = current_time_seconds_, + .type = FilterStepType::PREDICT, + .control_input = control_input, + .measurement = std::nullopt, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time_seconds = current_time_seconds_, }); } @@ -91,16 +91,15 @@ void RobotLocalizer::update(const VisionData& data) double prev_time = current_time_seconds_ - data.age_seconds; for (auto it = history.rbegin(); it != history.rend(); ++it) { - if (it->control_input.has_value()) + if (it->type == FilterStepType::PREDICT) { generatedPredictionMatrices(it->time_seconds - prev_time); filter_.predict(it->control_input.value()); prev_time = it->time_seconds; } - - if (it->measurement.has_value()) + else { - generateMeasurementModel(it->measurement_source.value()); + generateMeasurementModel(it->type); filter_.update(it->measurement.value()); } @@ -113,7 +112,7 @@ void RobotLocalizer::update(const VisionData& data) void RobotLocalizer::updateFilterWithVision(const Point& position, const Angle& orientation) { - generateMeasurementModel(MeasurementSource::VISION_DATA); + generateMeasurementModel(FilterStepType::VISION_DATA); const double orientation_estimate = filter_.state_estimate(static_cast(StateIndex::ORIENTATION)); @@ -137,7 +136,7 @@ void RobotLocalizer::updateFilterWithVision(const Point& position, void RobotLocalizer::update(const MotorData& data) { - generateMeasurementModel(MeasurementSource::MOTOR_DATA); + generateMeasurementModel(FilterStepType::MOTOR_DATA); Eigen::Vector measurement = Eigen::Vector::Zero(); @@ -152,18 +151,18 @@ void RobotLocalizer::update(const MotorData& data) filter_.update(measurement); history.push_front(FilterStep{ - .control_input = std::nullopt, - .measurement_source = MeasurementSource::MOTOR_DATA, - .measurement = measurement, - .state_estimate = filter_.state_estimate, - .state_covariance = filter_.state_covariance, - .time_seconds = current_time_seconds_, + .type = FilterStepType::MOTOR_DATA, + .control_input = std::nullopt, + .measurement = measurement, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time_seconds = current_time_seconds_, }); } void RobotLocalizer::update(const ImuData& data) { - generateMeasurementModel(MeasurementSource::IMU_DATA); + generateMeasurementModel(FilterStepType::IMU_DATA); Eigen::Vector measurement = Eigen::Vector::Zero(); @@ -174,12 +173,12 @@ void RobotLocalizer::update(const ImuData& data) filter_.update(measurement); history.push_front(FilterStep{ - .control_input = std::nullopt, - .measurement_source = MeasurementSource::IMU_DATA, - .measurement = measurement, - .state_estimate = filter_.state_estimate, - .state_covariance = filter_.state_covariance, - .time_seconds = current_time_seconds_, + .type = FilterStepType::IMU_DATA, + .control_input = std::nullopt, + .measurement = measurement, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time_seconds = current_time_seconds_, }); } @@ -278,13 +277,13 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) delta_time_seconds; } -void RobotLocalizer::generateMeasurementModel(MeasurementSource source) +void RobotLocalizer::generateMeasurementModel(FilterStepType source) { filter_.measurement_model.setZero(); switch (source) { - case MeasurementSource::VISION_DATA: + case FilterStepType::VISION_DATA: filter_.measurement_model( static_cast(MeasurementIndex::VISION_X_POSITION), static_cast(StateIndex::X_POSITION)) = 1; @@ -295,7 +294,7 @@ void RobotLocalizer::generateMeasurementModel(MeasurementSource source) static_cast(MeasurementIndex::VISION_ORIENTATION), static_cast(StateIndex::ORIENTATION)) = 1; break; - case MeasurementSource::MOTOR_DATA: + case FilterStepType::MOTOR_DATA: filter_.measurement_model( static_cast(MeasurementIndex::MOTOR_X_VELOCITY), static_cast(StateIndex::X_VELOCITY)) = 1; @@ -306,10 +305,13 @@ void RobotLocalizer::generateMeasurementModel(MeasurementSource source) static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY), static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; break; - case MeasurementSource::IMU_DATA: + case FilterStepType::IMU_DATA: filter_.measurement_model( static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY), static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; break; + case FilterStepType::PREDICT: + // Never called with PREDICT; predict steps use generatedPredictionMatrices. + break; } } diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 41e53acf2c..17e3037f6c 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -24,7 +24,7 @@ MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTA MAKE_ENUM(ControlIndex, X_ACCELERATION, Y_ACCELERATION); -MAKE_ENUM(MeasurementSource, MOTOR_DATA, IMU_DATA, VISION_DATA); +MAKE_ENUM(FilterStepType, PREDICT, MOTOR_DATA, IMU_DATA, VISION_DATA); /** * Estimates robot orientation, angular velocity, and angular acceleration @@ -159,9 +159,10 @@ class RobotLocalizer /** * Writes the measurement model for the given data source into the filter. * - * @param source Which sensor's measurement model to generate + * @param source Which sensor's measurement model to generate. Must not be + * FilterStepType::PREDICT. */ - void generateMeasurementModel(MeasurementSource source); + void generateMeasurementModel(FilterStepType source); static constexpr size_t STATE_SIZE = reflective_enum::size(); static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size(); @@ -172,9 +173,15 @@ class RobotLocalizer */ struct FilterStep { + FilterStepType type; + + // Set iff type == PREDICT. process_model/process_covariance/control_model are + // recomputed from the elapsed time during replay instead of being stored (see + // generatedPredictionMatrices). std::optional> control_input; - std::optional measurement_source; + // Set iff type != PREDICT. The measurement model is regenerated from type + // during replay (see generateMeasurementModel). std::optional> measurement; // Post operation state From 2ecadf1dafe83c72004ea1577653f5b394b69ff1 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 18:28:23 -0700 Subject: [PATCH 12/39] rename step to predict for discrete filters consistency --- src/software/embedded/robot_localizer.cpp | 2 +- src/software/embedded/robot_localizer.h | 2 +- src/software/embedded/robot_localizer_test.cpp | 2 +- src/software/embedded/thunderloop.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 9fe308cd6a..2be0825ef0 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -20,7 +20,7 @@ RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) .asDiagonal(); } -void RobotLocalizer::step(const Vector& linear_acceleration, const Duration& delta_time) +void RobotLocalizer::predict(const Vector& linear_acceleration, const Duration& delta_time) { const double delta_time_seconds = delta_time.toSeconds(); current_time_seconds_ += delta_time_seconds; diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 17e3037f6c..49125c14d8 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -78,7 +78,7 @@ class RobotLocalizer * @param linear_acceleration The current linear acceleration of the robot * @param delta_time The elapsed time since the previous step */ - void step(const Vector& linear_acceleration, const Duration& delta_time); + void predict(const Vector& linear_acceleration, const Duration& delta_time); /** * Update the robot's position and orientation from data reported by vision. diff --git a/src/software/embedded/robot_localizer_test.cpp b/src/software/embedded/robot_localizer_test.cpp index 712f23cddb..ca930de79a 100644 --- a/src/software/embedded/robot_localizer_test.cpp +++ b/src/software/embedded/robot_localizer_test.cpp @@ -48,7 +48,7 @@ RobotLocalizer runConstantVelocity(bool feed_vision, double vision_age = RTT_S / localToGlobalVelocity(local_velocity, localizer.getOrientation()), AngularVelocity::zero()}); - localizer.step(Vector(0.0, 0.0), Duration::fromSeconds(DT)); + localizer.predict(Vector(0.0, 0.0), Duration::fromSeconds(DT)); // Periodic vision fix (~60 Hz). Feed the position from RTT_S/2 ago, consistent // with the reported age. diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 675b132e79..e05e5ef14d 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -216,7 +216,7 @@ void Thunderloop::runLoop() } imu_service_->poll(robot_status_); - robot_localizer_->step(Vector(), delta_time); + robot_localizer_->predict(Vector(), delta_time); updateRobotLocalizer(robot_status_); primitive_executor_->updateRobotState(robot_localizer_->getRobotState()); From 8c3d36a4751e269f1ea3bebaaa3ba2d9493db8a5 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 19:14:55 -0700 Subject: [PATCH 13/39] add getters for pev target velocities --- src/software/embedded/primitive_executor.cpp | 5 +++++ src/software/embedded/primitive_executor.h | 7 +++++++ 2 files changed, 12 insertions(+) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 3035bf4969..9f48429601 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -62,6 +62,11 @@ void PrimitiveExecutor::updateRobotState(const RobotState& robot_state) robot_state_ = robot_state; } +Vector PrimitiveExecutor::getPrevCommandedVelocity() const +{ + return prev_target_global_velocity_; +} + Vector PrimitiveExecutor::stepTargetLinearVelocity(const Duration& delta_time) { Vector target_v_global = diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index b3b07a4548..928d4cf8f0 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -65,6 +65,13 @@ class PrimitiveExecutor TbotsProto::DirectControlPrimitive stepPrimitive( TbotsProto::RobotStatus& robot_status, const Duration& delta_time); + /** + * Gets the global-frame velocity commanded on the last call to stepPrimitive. + * + * @return The global-frame velocity commanded on the last step + */ + Vector getPrevCommandedVelocity() const; + private: /** * Tracks the planned trajectory to compute the robot's next target local linear From d138f5e6f477afcb38ebb4f8d6a112e000c2d719 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 19:15:49 -0700 Subject: [PATCH 14/39] wire up prediction --- src/software/embedded/thunderloop.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index e05e5ef14d..40a4da3e96 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -216,7 +216,8 @@ void Thunderloop::runLoop() } imu_service_->poll(robot_status_); - robot_localizer_->predict(Vector(), delta_time); + robot_localizer_->predict(primitive_executor_->getPrevCommandedVelocity(), + delta_time); updateRobotLocalizer(robot_status_); primitive_executor_->updateRobotState(robot_localizer_->getRobotState()); From f2e9dd6248cbec0f7cf04ba963ce7fc848bd099c Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 19:23:40 -0700 Subject: [PATCH 15/39] rely on control input for velcoity and predicted position --- src/software/embedded/robot_localizer.cpp | 46 +++++++++++++---------- src/software/embedded/robot_localizer.h | 13 ++++--- 2 files changed, 35 insertions(+), 24 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 2be0825ef0..b12db39256 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -5,7 +5,7 @@ #include "software/physics/velocity_conversion_util.h" RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) - : process_linear_acceleration_noise_variance_(config.process_noise_variance), + : process_linear_velocity_noise_variance_(config.process_noise_variance), process_angular_acceleration_noise_variance_(config.process_noise_variance) { filter_.state_covariance = @@ -20,7 +20,7 @@ RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) .asDiagonal(); } -void RobotLocalizer::predict(const Vector& linear_acceleration, const Duration& delta_time) +void RobotLocalizer::predict(const Vector& target_velocity, const Duration& delta_time) { const double delta_time_seconds = delta_time.toSeconds(); current_time_seconds_ += delta_time_seconds; @@ -28,7 +28,7 @@ void RobotLocalizer::predict(const Vector& linear_acceleration, const Duration& generatedPredictionMatrices(delta_time_seconds); Eigen::Vector control_input; - control_input << linear_acceleration.x(), linear_acceleration.y(); + control_input << target_velocity.x(), target_velocity.y(); filter_.predict(control_input); @@ -215,15 +215,17 @@ RobotState RobotLocalizer::getRobotState() const getAngularVelocity()); } +// TODO: Investigate proces models/variances/etc void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) { - // clang-format off + // In the current model, we use target velocity as our new velocity of the preiction state, and position is derived from it. + // Therefore, process model keeps the positions and we don't predict it using estimated velocities filter_.process_model << - 1, 0, 0, delta_time_seconds, 0, 0, - 0, 1, 0, 0, delta_time_seconds, 0, + 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, delta_time_seconds, - 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; // clang-format on @@ -231,13 +233,18 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) const double delta_time_cubed = delta_time_squared * delta_time_seconds; const double delta_time_fourth = delta_time_cubed * delta_time_seconds; + // Linear terms model velocity itself as the noisy quantity (how much actual + // velocity deviates from the commanded target velocity), integrated once into + // position, rather than a noisy acceleration integrated twice. const double linear_position_variance = - delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; + delta_time_cubed * process_linear_velocity_noise_variance_; const double linear_position_velocity_covariance = - delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + delta_time_squared * process_linear_velocity_noise_variance_; const double linear_velocity_variance = - delta_time_squared * process_linear_acceleration_noise_variance_; + delta_time_seconds * process_linear_velocity_noise_variance_; + // Angular terms are unchanged: angular velocity has no control input, so it's + // still modeled as a noisy acceleration integrated twice. const double angular_position_variance = delta_time_fourth / 4 * process_angular_acceleration_noise_variance_; const double angular_position_velocity_covariance = @@ -257,24 +264,25 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) 0, 0, angular_position_velocity_covariance, 0, 0, angular_velocity_variance; // clang-format on + // Control input is the commanded (target) linear velocity: it replaces the old + // velocity state outright (see process_model above) and drives position over this + // step's elapsed time. auto& control_model = filter_.control_model; control_model.setZero(); control_model(static_cast(StateIndex::X_POSITION), - static_cast(ControlIndex::X_ACCELERATION)) = - delta_time_squared / 2; + static_cast(ControlIndex::X_VELOCITY_TARGET)) = + delta_time_seconds; control_model(static_cast(StateIndex::Y_POSITION), - static_cast(ControlIndex::Y_ACCELERATION)) = - delta_time_squared / 2; + static_cast(ControlIndex::Y_VELOCITY_TARGET)) = + delta_time_seconds; control_model(static_cast(StateIndex::X_VELOCITY), - static_cast(ControlIndex::X_ACCELERATION)) = - delta_time_seconds; + static_cast(ControlIndex::X_VELOCITY_TARGET)) = 1; control_model(static_cast(StateIndex::Y_VELOCITY), - static_cast(ControlIndex::Y_ACCELERATION)) = - delta_time_seconds; + static_cast(ControlIndex::Y_VELOCITY_TARGET)) = 1; } void RobotLocalizer::generateMeasurementModel(FilterStepType source) diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 49125c14d8..695632d560 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -22,7 +22,7 @@ MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTA MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, IMU_ANGULAR_VELOCITY); -MAKE_ENUM(ControlIndex, X_ACCELERATION, Y_ACCELERATION); +MAKE_ENUM(ControlIndex, X_VELOCITY_TARGET, Y_VELOCITY_TARGET); MAKE_ENUM(FilterStepType, PREDICT, MOTOR_DATA, IMU_DATA, VISION_DATA); @@ -75,10 +75,11 @@ class RobotLocalizer /** * Runs one prediction step over the given elapsed time. * - * @param linear_acceleration The current linear acceleration of the robot + * @param target_velocity The global-frame linear velocity the robot is currently + * being commanded to achieve * @param delta_time The elapsed time since the previous step */ - void predict(const Vector& linear_acceleration, const Duration& delta_time); + void predict(const Vector& target_velocity, const Duration& delta_time); /** * Update the robot's position and orientation from data reported by vision. @@ -193,8 +194,10 @@ class RobotLocalizer KalmanFilter filter_; - // Process noise variance used in prediction - double process_linear_acceleration_noise_variance_; + // Process noise variance used in prediction. The linear term models how much + // actual velocity deviates from the commanded target velocity (a rate, per unit + // time); the angular term models unmeasured angular acceleration disturbance. + double process_linear_velocity_noise_variance_; double process_angular_acceleration_noise_variance_; // History is ordered newest-first (front is the most recent step) From 17c7e9f51e2fc3716d4782839120d0b42c108021 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Sun, 13 Sep 2026 13:12:46 -0700 Subject: [PATCH 16/39] implmeent process model --- src/software/embedded/robot_localizer.cpp | 17 +++++++++++++++++ src/software/embedded/robot_localizer.h | 3 ++- .../filter/extended_kalman_filter.hpp | 4 ++-- 3 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index b12db39256..24e5952b75 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -18,6 +18,8 @@ RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) config.motor_sensor_noise_variance, config.motor_sensor_noise_variance, ImuService::IMU_VARIANCE) .asDiagonal(); + + filter_.process_model_function = robot_localizer_process_model; } void RobotLocalizer::predict(const Vector& target_velocity, const Duration& delta_time) @@ -215,6 +217,21 @@ RobotState RobotLocalizer::getRobotState() const getAngularVelocity()); } +std::function(Eigen::Vector)> robot_localizer_process_model = [](Eigen::Vector state, double dt){ + Eigen::Vector prior; + Vector velocity = Vector(state(static_cast(StateIndex::X_VELOCITY)) , state(static_cast(StateIndex::Y_VELOCITY))); + double rot = state(static_cast(StateIndex::ORIENTATION)); + prior << state(static_cast(StateIndex::X_POSITION)) + (velocity.x() * rot.cos() - velocity.y() * rot.sin())*dt, + state(static_cast(StateIndex::Y_POSITION)) + (velocity.x() * rot.sin() + velocity.y() * rot.cos())*dt, + state(static_cast(StateIndex::ORIENTATION))+ state(static_cast(StateIndex::ANGULAR_VELOCITY)) * dt; + 0, + 0, + 1; + return prior; + +} + return Vector(, ); + // TODO: Investigate proces models/variances/etc void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) { diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 695632d560..5ae0d14228 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -11,6 +11,7 @@ #include "software/geom/point.h" #include "software/geom/vector.h" #include "software/sensor_fusion/filter/kalman_filter.hpp" +#include "software/sensor_fusion/filter/extended_kalman_filter.hpp" #include "software/time/duration.h" #include "software/util/make_enum/make_enum.hpp" #include "software/world/robot_state.h" @@ -192,7 +193,7 @@ class RobotLocalizer double time_seconds; }; - KalmanFilter filter_; + ExtendedKalmanFilter filter_; // Process noise variance used in prediction. The linear term models how much // actual velocity deviates from the commanded target velocity (a rate, per unit diff --git a/src/software/sensor_fusion/filter/extended_kalman_filter.hpp b/src/software/sensor_fusion/filter/extended_kalman_filter.hpp index 9f31af8415..4680bbbf51 100644 --- a/src/software/sensor_fusion/filter/extended_kalman_filter.hpp +++ b/src/software/sensor_fusion/filter/extended_kalman_filter.hpp @@ -48,13 +48,13 @@ class ExtendedKalmanFilter * The process model f(x): propagates a state forward by one time step. */ using ProcessModelFunction = - std::function(Eigen::Vector)>; + std::function(Eigen::Vector, double dt)>; /** * The Jacobian of the process model (F = df/dx), evaluated at a given state. */ using ProcessModelJacobianFunction = - std::function(Eigen::Vector)>; + std::function(Eigen::Vector, double dt)>; /** * Creates an extended Kalman filter with all internal matrices and vectors set From 1f03db1a19309034a2788489c5ec4c751aa548a0 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 13:16:36 -0700 Subject: [PATCH 17/39] jacobian and integration --- src/software/embedded/robot_localizer.cpp | 149 +++++++++++++----- src/software/embedded/robot_localizer.h | 46 ++++-- .../embedded/robot_localizer_test.cpp | 28 ++-- src/software/embedded/thunderloop.cpp | 10 +- 4 files changed, 161 insertions(+), 72 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 24e5952b75..fb154b740a 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -1,5 +1,7 @@ #include "robot_localizer.h" +#include + #include "proto/message_translation/tbots_geometry.h" #include "shared/constants.h" #include "software/physics/velocity_conversion_util.h" @@ -18,8 +20,6 @@ RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) config.motor_sensor_noise_variance, config.motor_sensor_noise_variance, ImuService::IMU_VARIANCE) .asDiagonal(); - - filter_.process_model_function = robot_localizer_process_model; } void RobotLocalizer::predict(const Vector& target_velocity, const Duration& delta_time) @@ -191,7 +191,12 @@ Point RobotLocalizer::getPosition() const filter_.state_estimate(static_cast(StateIndex::Y_POSITION))); } -Vector RobotLocalizer::getVelocity() const +Vector RobotLocalizer::getGlobalVelocity() const +{ + return localToGlobalVelocity(getLocalVelocity(), getOrientation()); +} + +Vector RobotLocalizer::getLocalVelocity() const { return Vector( filter_.state_estimate(static_cast(StateIndex::X_VELOCITY)), @@ -213,38 +218,92 @@ AngularVelocity RobotLocalizer::getAngularVelocity() const RobotState RobotLocalizer::getRobotState() const { - return RobotState(getPosition(), getVelocity(), getOrientation(), + return RobotState(getPosition(), getGlobalVelocity(), getOrientation(), getAngularVelocity()); } -std::function(Eigen::Vector)> robot_localizer_process_model = [](Eigen::Vector state, double dt){ - Eigen::Vector prior; - Vector velocity = Vector(state(static_cast(StateIndex::X_VELOCITY)) , state(static_cast(StateIndex::Y_VELOCITY))); - double rot = state(static_cast(StateIndex::ORIENTATION)); - prior << state(static_cast(StateIndex::X_POSITION)) + (velocity.x() * rot.cos() - velocity.y() * rot.sin())*dt, - state(static_cast(StateIndex::Y_POSITION)) + (velocity.x() * rot.sin() + velocity.y() * rot.cos())*dt, - state(static_cast(StateIndex::ORIENTATION))+ state(static_cast(StateIndex::ANGULAR_VELOCITY)) * dt; - 0, - 0, - 1; - return prior; - -} - return Vector(, ); - // TODO: Investigate proces models/variances/etc void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) { - // In the current model, we use target velocity as our new velocity of the preiction state, and position is derived from it. - // Therefore, process model keeps the positions and we don't predict it using estimated velocities - filter_.process_model << - 1, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, - 0, 0, 1, 0, 0, delta_time_seconds, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1; - // clang-format on + // Velocity is estimated in the robot's local frame (see StateIndex), but position + // is in world space, so propagating position requires rotating local velocity by + // the current orientation estimate -- a nonlinear operation, hence the process + // model function/Jacobian pair instead of a constant matrix. + // + // Velocity itself isn't propagated from its own estimate: it's replaced outright + // by the (rotated) control input every step (see control_model below), so f(x) + // leaves it at zero and its row of the Jacobian is zero too. + filter_.process_model_function = + [delta_time_seconds](Eigen::Vector state) + { + const double theta = + state(static_cast(StateIndex::ORIENTATION)); + const double local_vx = + state(static_cast(StateIndex::X_VELOCITY)); + const double local_vy = + state(static_cast(StateIndex::Y_VELOCITY)); + + Eigen::Vector next_state = + Eigen::Vector::Zero(); + + next_state(static_cast(StateIndex::X_POSITION)) = + state(static_cast(StateIndex::X_POSITION)) + + delta_time_seconds * + (local_vx * std::cos(theta) - local_vy * std::sin(theta)); + next_state(static_cast(StateIndex::Y_POSITION)) = + state(static_cast(StateIndex::Y_POSITION)) + + delta_time_seconds * + (local_vx * std::sin(theta) + local_vy * std::cos(theta)); + next_state(static_cast(StateIndex::ORIENTATION)) = + theta + delta_time_seconds * + state(static_cast(StateIndex::ANGULAR_VELOCITY)); + next_state(static_cast(StateIndex::ANGULAR_VELOCITY)) = + state(static_cast(StateIndex::ANGULAR_VELOCITY)); + + return next_state; + }; + + filter_.process_model_jacobian_function = + [delta_time_seconds](Eigen::Vector state) + { + const auto x_position_index = static_cast(StateIndex::X_POSITION); + const auto y_position_index = static_cast(StateIndex::Y_POSITION); + const auto orientation_index = + static_cast(StateIndex::ORIENTATION); + const auto x_velocity_index = static_cast(StateIndex::X_VELOCITY); + const auto y_velocity_index = static_cast(StateIndex::Y_VELOCITY); + const auto angular_velocity_index = + static_cast(StateIndex::ANGULAR_VELOCITY); + + const double theta = state(orientation_index); + const double local_vx = state(x_velocity_index); + const double local_vy = state(y_velocity_index); + const double cos_theta = std::cos(theta); + const double sin_theta = std::sin(theta); + + Eigen::Matrix jacobian = + Eigen::Matrix::Identity(); + + jacobian(x_position_index, orientation_index) = + delta_time_seconds * (-local_vx * sin_theta - local_vy * cos_theta); + jacobian(x_position_index, x_velocity_index) = delta_time_seconds * cos_theta; + jacobian(x_position_index, y_velocity_index) = -delta_time_seconds * sin_theta; + + jacobian(y_position_index, orientation_index) = + delta_time_seconds * (local_vx * cos_theta - local_vy * sin_theta); + jacobian(y_position_index, x_velocity_index) = delta_time_seconds * sin_theta; + jacobian(y_position_index, y_velocity_index) = delta_time_seconds * cos_theta; + + jacobian(orientation_index, angular_velocity_index) = delta_time_seconds; + + // f leaves velocity at zero regardless of the input state (see + // process_model_function above), so its row of the Jacobian is zero, not the + // identity default. + jacobian(x_velocity_index, x_velocity_index) = 0; + jacobian(y_velocity_index, y_velocity_index) = 0; + + return jacobian; + }; const double delta_time_squared = delta_time_seconds * delta_time_seconds; const double delta_time_cubed = delta_time_squared * delta_time_seconds; @@ -281,25 +340,33 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) 0, 0, angular_position_velocity_covariance, 0, 0, angular_velocity_variance; // clang-format on - // Control input is the commanded (target) linear velocity: it replaces the old - // velocity state outright (see process_model above) and drives position over this - // step's elapsed time. + // Control input is the commanded (target) linear velocity in world space: it + // replaces the local velocity state outright, rotated into the robot's local + // frame by the current orientation estimate (see process_model_function above, + // which then rotates that local velocity back into world space to propagate + // position). Position is no longer driven directly from control input here -- + // that happens through the process model function instead. + const double theta = + filter_.state_estimate(static_cast(StateIndex::ORIENTATION)); + const double cos_theta = std::cos(theta); + const double sin_theta = std::sin(theta); + auto& control_model = filter_.control_model; control_model.setZero(); - control_model(static_cast(StateIndex::X_POSITION), + control_model(static_cast(StateIndex::X_VELOCITY), static_cast(ControlIndex::X_VELOCITY_TARGET)) = - delta_time_seconds; - - control_model(static_cast(StateIndex::Y_POSITION), - static_cast(ControlIndex::Y_VELOCITY_TARGET)) = - delta_time_seconds; - + cos_theta; control_model(static_cast(StateIndex::X_VELOCITY), - static_cast(ControlIndex::X_VELOCITY_TARGET)) = 1; + static_cast(ControlIndex::Y_VELOCITY_TARGET)) = + sin_theta; control_model(static_cast(StateIndex::Y_VELOCITY), - static_cast(ControlIndex::Y_VELOCITY_TARGET)) = 1; + static_cast(ControlIndex::X_VELOCITY_TARGET)) = + -sin_theta; + control_model(static_cast(StateIndex::Y_VELOCITY), + static_cast(ControlIndex::Y_VELOCITY_TARGET)) = + cos_theta; } void RobotLocalizer::generateMeasurementModel(FilterStepType source) diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 5ae0d14228..cf8d1346ca 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -10,15 +10,19 @@ #include "software/geom/angle.h" #include "software/geom/point.h" #include "software/geom/vector.h" -#include "software/sensor_fusion/filter/kalman_filter.hpp" #include "software/sensor_fusion/filter/extended_kalman_filter.hpp" #include "software/time/duration.h" #include "software/util/make_enum/make_enum.hpp" #include "software/world/robot_state.h" +// X_POSITION/Y_POSITION are in world space; X_VELOCITY/Y_VELOCITY are in the robot's +// local frame (see velocity_conversion_util.h), matching what the motor sensors report +// directly and avoiding a lossy conversion through the orientation estimate. MAKE_ENUM(StateIndex, X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY, ANGULAR_VELOCITY); +// MOTOR_X_VELOCITY/MOTOR_Y_VELOCITY are in the robot's local frame, matching +// StateIndex::X_VELOCITY/Y_VELOCITY. MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTATION, MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, IMU_ANGULAR_VELOCITY); @@ -28,8 +32,12 @@ MAKE_ENUM(ControlIndex, X_VELOCITY_TARGET, Y_VELOCITY_TARGET); MAKE_ENUM(FilterStepType, PREDICT, MOTOR_DATA, IMU_DATA, VISION_DATA); /** - * Estimates robot orientation, angular velocity, and angular acceleration - * using a Kalman filter. + * Estimates robot position, orientation, velocity, and angular velocity using an + * extended Kalman filter. + * + * The process model is nonlinear because velocity is estimated in the robot's local + * frame (see StateIndex) while position is in world space, so propagating position + * requires rotating local velocity by the current orientation estimate. * * The filter keeps a history of recent predict/update operations. When delayed * vision data arrives, the localizer rewinds to the matching historical state, @@ -48,6 +56,8 @@ class RobotLocalizer struct MotorData { + // Local-frame velocity, as reported directly by the motor sensors (see + // velocity_conversion_util.h) Vector velocity; AngularVelocity angular_velocity; }; @@ -114,9 +124,20 @@ class RobotLocalizer /** * Gets the estimated velocity of the robot in world space. * + * The filter estimates velocity in the robot's local frame (see StateIndex), so + * this converts it to world space using the current orientation estimate. + * * @return the estimated velocity of the robot in world space */ - Vector getVelocity() const; + Vector getGlobalVelocity() const; + + /** + * Gets the estimated velocity of the robot in its own local frame (see StateIndex + * and velocity_conversion_util.h), i.e. the filter's raw velocity state. + * + * @return the estimated velocity of the robot in its local frame + */ + Vector getLocalVelocity() const; /** * Gets the estimated orientation of the robot in world space. @@ -149,9 +170,15 @@ class RobotLocalizer void updateFilterWithVision(const Point& position, const Angle& orientation); /** - * Computes the process model, process covariance, and control model for the - * given elapsed time, and writes them into the filter. Does not run the - * predict step itself. + * Computes the process model function, its Jacobian, the process covariance, and + * the control model for the given elapsed time, and writes them into the filter. + * Does not run the predict step itself. + * + * The control model also depends on the filter's current orientation estimate + * (used to rotate the global-frame control input into the local frame that + * velocity is estimated in), so this must be called with the filter's state + * estimate set to what it was immediately before the predict step being + * (re)computed. * * @param delta_time_seconds The elapsed time to generate the prediction * matrices for @@ -177,8 +204,9 @@ class RobotLocalizer { FilterStepType type; - // Set iff type == PREDICT. process_model/process_covariance/control_model are - // recomputed from the elapsed time during replay instead of being stored (see + // Set iff type == PREDICT. The process model function/Jacobian, process + // covariance, and control model are recomputed from the elapsed time and the + // state estimate during replay instead of being stored (see // generatedPredictionMatrices). std::optional> control_input; diff --git a/src/software/embedded/robot_localizer_test.cpp b/src/software/embedded/robot_localizer_test.cpp index ca930de79a..52efe91fcb 100644 --- a/src/software/embedded/robot_localizer_test.cpp +++ b/src/software/embedded/robot_localizer_test.cpp @@ -44,9 +44,7 @@ RobotLocalizer runConstantVelocity(bool feed_vision, double vision_age = RTT_S / const Vector local_velocity = globalToLocalVelocity(true_velocity, true_orientation); - localizer.update(RobotLocalizer::MotorData{ - localToGlobalVelocity(local_velocity, localizer.getOrientation()), - AngularVelocity::zero()}); + localizer.update(RobotLocalizer::MotorData{local_velocity, AngularVelocity::zero()}); localizer.predict(Vector(0.0, 0.0), Duration::fromSeconds(DT)); @@ -71,8 +69,8 @@ TEST(RobotLocalizer, tracks_constant_forward_velocity) const RobotLocalizer localizer = runConstantVelocity(/*feed_vision=*/true); std::cerr << "[motor+vision] pos=(" << localizer.getPosition().x() << ", " - << localizer.getPosition().y() << ") vel=(" << localizer.getVelocity().x() - << ", " << localizer.getVelocity().y() + << localizer.getPosition().y() << ") vel=(" << localizer.getGlobalVelocity().x() + << ", " << localizer.getGlobalVelocity().y() << ") orient=" << localizer.getOrientation().toDegrees() << "deg\n"; // NOTE: we assert on velocity and orientation, not absolute position. RobotLocalizer @@ -84,9 +82,9 @@ TEST(RobotLocalizer, tracks_constant_forward_velocity) // measurements and are robust to this. The key property under test is that the // periodic vision update no longer corrupts the velocity estimate. EXPECT_NEAR(localizer.getOrientation().toDegrees(), 0.0, 10.0); - EXPECT_NEAR(localizer.getVelocity().x(), 1.0, 0.2) + EXPECT_NEAR(localizer.getGlobalVelocity().x(), 1.0, 0.2) << "Forward velocity estimate does not track"; - EXPECT_NEAR(localizer.getVelocity().y(), 0.0, 0.2); + EXPECT_NEAR(localizer.getGlobalVelocity().y(), 0.0, 0.2); } // Diagnostic: with no periodic vision fix, the velocity estimate comes purely from the @@ -96,11 +94,11 @@ TEST(RobotLocalizer, velocity_tracks_from_motors_without_vision) { const RobotLocalizer localizer = runConstantVelocity(/*feed_vision=*/false); - std::cerr << "[motor only] vel=(" << localizer.getVelocity().x() << ", " - << localizer.getVelocity().y() << ")\n"; + std::cerr << "[motor only] vel=(" << localizer.getGlobalVelocity().x() << ", " + << localizer.getGlobalVelocity().y() << ")\n"; - EXPECT_NEAR(localizer.getVelocity().x(), 1.0, 0.2); - EXPECT_NEAR(localizer.getVelocity().y(), 0.0, 0.2); + EXPECT_NEAR(localizer.getGlobalVelocity().x(), 1.0, 0.2); + EXPECT_NEAR(localizer.getGlobalVelocity().y(), 0.0, 0.2); } // Diagnostic: feed vision with a near-zero age, which takes the non-rollback path @@ -112,9 +110,9 @@ TEST(RobotLocalizer, velocity_with_zero_age_vision) const RobotLocalizer localizer = runConstantVelocity(/*feed_vision=*/true, /*vision_age=*/1e-6); - std::cerr << "[zero-age vision] vel=(" << localizer.getVelocity().x() << ", " - << localizer.getVelocity().y() << ")\n"; + std::cerr << "[zero-age vision] vel=(" << localizer.getGlobalVelocity().x() << ", " + << localizer.getGlobalVelocity().y() << ")\n"; - EXPECT_NEAR(localizer.getVelocity().x(), 1.0, 0.2); - EXPECT_NEAR(localizer.getVelocity().y(), 0.0, 0.2); + EXPECT_NEAR(localizer.getGlobalVelocity().x(), 1.0, 0.2); + EXPECT_NEAR(localizer.getGlobalVelocity().y(), 0.0, 0.2); } diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 40a4da3e96..b718688b97 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -271,17 +271,13 @@ void Thunderloop::updateRobotLocalizer(const TbotsProto::RobotStatus& robot_stat { // Seperate update is okay because measurement model is linear if (robot_status.has_imu_status()){ - robot_localizer_.update(RobotLocalizer::ImuData{ - createAngularVelocity(robot_status.imu_status().angular_velocity()) - }) - + robot_localizer_->update(RobotLocalizer::ImuData{ + createAngularVelocity(robot_status.imu_status().angular_velocity())}); } if (robot_status.has_motor_status()) { robot_localizer_->update(RobotLocalizer::MotorData{ - localToGlobalVelocity( - createVector(robot_status.motor_status().local_velocity()), - robot_localizer_->getOrientation()), + createVector(robot_status.motor_status().local_velocity()), createAngularVelocity(robot_status.motor_status().angular_velocity())}); } } From c0224c723a96f6d42cc1c8560849726186d34541 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 13:17:40 -0700 Subject: [PATCH 18/39] x --- src/software/embedded/thunderloop.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index b718688b97..d05896b190 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -19,7 +19,6 @@ #include "software/embedded/services/motor.h" #include "software/logger/network_logger.h" #include "software/networking/tbots_network_exception.h" -#include "software/physics/velocity_conversion_util.h" #include "software/time/duration.h" #include "software/tracy/tracy_constants.h" From 0dc69d1a09a2de1e4931ae42c4908395ece1f7d3 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 13:28:51 -0700 Subject: [PATCH 19/39] build --- src/software/embedded/BUILD | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 64ea96171e..05107bf4f4 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -126,7 +126,7 @@ cc_library( "//software/geom:angular_velocity", "//software/geom:point", "//software/geom:vector", - "//software/sensor_fusion/filter:kalman_filter", + "//software/sensor_fusion/filter:extended_kalman_filter", "//software/world:robot_state", "@eigen", ], From e10f9ac9037e4f78ad40f556e1e031965fb7d6aa Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Mon, 14 Sep 2026 22:04:35 -0700 Subject: [PATCH 20/39] rebase --- src/shared/constants.h | 2 +- src/software/embedded/BUILD | 3 + src/software/embedded/primitive_executor.cpp | 16 +- src/software/embedded/primitive_executor.h | 9 +- src/software/embedded/robot_localizer.cpp | 2 + src/software/embedded/robot_localizer.h | 13 ++ src/software/embedded/thunderloop.cpp | 4 + src/software/simulation/BUILD | 3 + .../simulation/er_force_simulator.cpp | 166 +++++++++++++++++- src/software/simulation/er_force_simulator.h | 75 ++++++++ 10 files changed, 284 insertions(+), 9 deletions(-) diff --git a/src/shared/constants.h b/src/shared/constants.h index c31b062b2d..5c4c68df6f 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -30,7 +30,7 @@ static const std::string LOOPBACK_INTERFACE = "lo"; #endif // PlotJuggler's default host and port -static const std::string PLOTJUGGLER_GUI_DEFAULT_HOST = "ff02::c3d0:42d2:aaaa"; +static const std::string PLOTJUGGLER_GUI_DEFAULT_HOST = "127.0.0.1"; static const short unsigned int PLOTJUGGLER_GUI_DEFAULT_PORT = 9870; // ProtoLogger constants for replay files diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 05107bf4f4..80ec2c897e 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -39,6 +39,7 @@ cc_library( "//proto/primitive:primitive_msg_factory", "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular", "//software/ai/navigator/trajectory:trajectory_path", + "//software/embedded:robot_localizer", "//software/embedded/motion_control:orientation_controller", "//software/embedded/motion_control:position_controller", "//software/math:math_functions", @@ -119,6 +120,7 @@ cc_library( hdrs = ["robot_localizer.h"], deps = [ "//proto:tbots_cc_proto", + "//proto/message_translation:tbots_protobuf", "//proto/primitive:primitive_msg_factory", "//software:constants", "//software/embedded/services:imu", @@ -127,6 +129,7 @@ cc_library( "//software/geom:point", "//software/geom:vector", "//software/sensor_fusion/filter:extended_kalman_filter", + "//software/sensor_fusion/filter:kalman_filter", "//software/world:robot_state", "@eigen", ], diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 9f48429601..2a6291b246 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -8,13 +8,18 @@ #include "proto/primitive/primitive_msg_factory.h" #include "proto/tbots_software_msgs.pb.h" #include "proto/visualization.pb.h" +#include "software/embedded/robot_localizer.h" #include "software/geom/algorithms/distance.h" #include "software/logger/logger.h" #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor( - const robot_constants::RobotConstants& robot_constants, const RobotId robot_id) - : robot_state_(), robot_constants_(robot_constants), robot_id_(robot_id) + const robot_constants::RobotConstants& robot_constants, const RobotId robot_id, + const TeamColour team_colour) + : robot_state_(), + robot_constants_(robot_constants), + robot_id_(robot_id), + team_colour_(team_colour) { } @@ -60,6 +65,13 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m void PrimitiveExecutor::updateRobotState(const RobotState& robot_state) { robot_state_ = robot_state; + + // Team colour is embedded in the key since the simulator runs both teams (which + // number robots independently) in one process/log stream; without it, e.g. yellow + // robot 0 and blue robot 0 would collide onto the same PlotJuggler key. + const std::string team_tag = + (team_colour_ == TeamColour::YELLOW) ? "_yellow" : "_blue"; + RobotLocalizer::logToPlotJuggler(robot_id_, robot_state_, team_tag); } Vector PrimitiveExecutor::getPrevCommandedVelocity() const diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 928d4cf8f0..af3c70de7e 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -9,6 +9,7 @@ #include "software/geom/vector.h" #include "software/time/duration.h" #include "software/world/robot_state.h" +#include "software/world/team_types.h" /** * "Executes" primitives, turning them into the direct control commands that @@ -25,9 +26,14 @@ class PrimitiveExecutor * * @param robot_constants The constants for the robot using this primitive executor * @param robot_id The ID of the robot using this primitive executor + * @param team_colour The colour of the team this robot belongs to. Only matters for + * disambiguating PlotJuggler log keys when multiple teams share one process (e.g. + * the simulator, where a yellow and blue robot can have the same ID); real hardware + * only ever runs one robot so this can be left at its default. */ explicit PrimitiveExecutor(const robot_constants::RobotConstants& robot_constants, - RobotId robot_id); + RobotId robot_id, + TeamColour team_colour = TeamColour::YELLOW); /** * Starts executing a new primitive. @@ -117,6 +123,7 @@ class PrimitiveExecutor robot_constants::RobotConstants robot_constants_; RobotId robot_id_; + TeamColour team_colour_; std::optional trajectory_path_; std::optional angular_trajectory_; diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index fb154b740a..699cc2f6b7 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -1,8 +1,10 @@ #include "robot_localizer.h" +#include "software/logger/logger.h" #include #include "proto/message_translation/tbots_geometry.h" +#include "proto/message_translation/tbots_protobuf.h" #include "shared/constants.h" #include "software/physics/velocity_conversion_util.h" diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index cf8d1346ca..1fe11af9ab 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -3,6 +3,7 @@ #include #include #include +#include #include "proto/primitive.pb.h" #include "proto/robot_status_msg.pb.h" @@ -160,6 +161,18 @@ class RobotLocalizer */ RobotState getRobotState() const; + /** + * Logs a robot's position and velocity to PlotJuggler, with the robot ID embedded + * in each key (e.g. "vel_x_robot_4"). + * + * @param robot_id The ID of the robot the state belongs to + * @param robot_state The robot state to log + * @param tag Optional suffix appended after the robot ID (e.g. "_estimated"), to + * distinguish multiple state sources logged for the same robot + */ + static void logToPlotJuggler(RobotId robot_id, const RobotState& robot_state, + const std::string& tag = ""); + private: /** * Update the Kalman filter with the robot's position and orientation from vision. diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index d05896b190..f8ca0bb5ad 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -17,6 +17,7 @@ #include "software/embedded/primitive_executor.h" #include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" +#include "software/logger/custom_logging_levels.h" #include "software/logger/network_logger.h" #include "software/networking/tbots_network_exception.h" #include "software/time/duration.h" @@ -221,6 +222,9 @@ void Thunderloop::runLoop() primitive_executor_->updateRobotState(robot_localizer_->getRobotState()); + Point position = robot_localizer_->getPosition(); + Vector velocity = robot_localizer_->getVelocity(); + const TbotsProto::DirectControlPrimitive direct_control_primitive = primitive_executor_->stepPrimitive(robot_status_, delta_time); diff --git a/src/software/simulation/BUILD b/src/software/simulation/BUILD index 8d2f54dea2..c0adfc8b8a 100644 --- a/src/software/simulation/BUILD +++ b/src/software/simulation/BUILD @@ -17,7 +17,10 @@ cc_library( "//proto/message_translation:ssl_geometry", "//proto/message_translation:ssl_simulation_robot_control", "//proto/message_translation:ssl_wrapper", + "//shared:constants", "//software/embedded:primitive_executor", + "//software/embedded:robot_localizer", + "//software/embedded/services:imu", "//software/physics:euclidean_to_wheel", "//software/physics:velocity_conversion_util", "//software/world", diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index f1f2ba513c..39e13319f5 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -10,12 +10,61 @@ #include "proto/message_translation/ssl_geometry.h" #include "proto/message_translation/ssl_simulation_robot_control.h" #include "proto/message_translation/ssl_wrapper.h" +#include "proto/message_translation/tbots_geometry.h" #include "proto/message_translation/tbots_protobuf.h" #include "proto/robot_status_msg.pb.h" +#include "shared/constants.h" +#include "software/embedded/services/imu.h" #include "software/logger/logger.h" #include "software/physics/velocity_conversion_util.h" #include "software/world/robot_state.h" +namespace +{ +double sampleGaussianNoise(std::mt19937& rng, double variance) +{ + std::normal_distribution distribution(0.0, std::sqrt(variance)); + return distribution(rng); +} + +// Most of a synthesized sensor channel's assumed variance is modeled as a slowly +// drifting bias (an Ornstein-Uhlenbeck process) rather than fresh white noise, since +// real error sources like wheel slip or calibration drift persist over time instead of +// resetting every sample; the rest is left as fast white noise for sample-to-sample +// jitter. +constexpr double BIAS_VARIANCE_FRACTION = 0.9; +constexpr double BIAS_TIME_CONSTANT_SECONDS = 0.5; + +// IMU/motor noise is scaled up from the filter's own assumed variance so the +// synthesized sensors show a visible, meaningful divergence from ground truth instead +// of being dominated by (real, correct) vision corrections. +constexpr double IMU_MOTOR_NOISE_SCALE_FACTOR = 3.0; + +// Advances a single drifting bias value by one Euler-Maruyama step of an +// Ornstein-Uhlenbeck process, whose stationary variance equals `stationary_variance` +// and whose fluctuations decorrelate over roughly `BIAS_TIME_CONSTANT_SECONDS`. +void stepDriftingBias(std::mt19937& rng, double& bias, double dt_seconds, + double stationary_variance) +{ + const double mean_reversion_rate = 1.0 / BIAS_TIME_CONSTANT_SECONDS; + const double diffusion_coefficient = + std::sqrt(2.0 * mean_reversion_rate * stationary_variance); + std::normal_distribution distribution(0.0, 1.0); + bias += -mean_reversion_rate * bias * dt_seconds + + diffusion_coefficient * std::sqrt(dt_seconds) * distribution(rng); +} + +// Combines a channel's drifting bias with a smaller fresh white-noise component, both +// drawn from the same total variance per BIAS_VARIANCE_FRACTION. +double sampleCorrelatedNoise(std::mt19937& rng, double& bias, double dt_seconds, + double total_variance) +{ + stepDriftingBias(rng, bias, dt_seconds, BIAS_VARIANCE_FRACTION * total_variance); + return bias + + sampleGaussianNoise(rng, (1.0 - BIAS_VARIANCE_FRACTION) * total_variance); +} +} // namespace + ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, const robot_constants::RobotConstants& robot_constants, std::unique_ptr& realism_config, @@ -28,7 +77,8 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, field(Field::createField(field_type)), blue_robot_with_ball(std::nullopt), yellow_robot_with_ball(std::nullopt), - ramping(ramping) + ramping(ramping), + noise_rng_(std::random_device{}()) { std::string full_filename = CONFIG_DIRECTORY; @@ -280,14 +330,14 @@ void ErForceSimulator::setRobots( { if (side == gameController::Team::BLUE) { - auto robot_primitive_executor = - std::make_shared(robot_constants, id); + auto robot_primitive_executor = std::make_shared( + robot_constants, id, TeamColour::BLUE); blue_primitive_executor_map.insert({id, robot_primitive_executor}); } else { - auto robot_primitive_executor = - std::make_shared(robot_constants, id); + auto robot_primitive_executor = std::make_shared( + robot_constants, id, TeamColour::YELLOW); yellow_primitive_executor_map.insert({id, robot_primitive_executor}); } } @@ -309,6 +359,7 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( { setRobotPrimitive(robot_id, primitive_set_msg, yellow_primitive_executor_map, robot_map.at(robot_id)); + updateLocalizerVisionFromPrimitive(robot_id, primitive, yellow_localizer_map); } } } @@ -329,6 +380,7 @@ void ErForceSimulator::setBlueRobotPrimitiveSet( { setRobotPrimitive(robot_id, primitive_set_msg, blue_primitive_executor_map, robot_map.at(robot_id)); + updateLocalizerVisionFromPrimitive(robot_id, primitive, blue_localizer_map); } } } @@ -356,6 +408,30 @@ void ErForceSimulator::setRobotPrimitive( } } +void ErForceSimulator::updateLocalizerVisionFromPrimitive( + RobotId id, const TbotsProto::Primitive& primitive, + std::unordered_map& localizer_map) +{ + if (!primitive.has_move()) + { + return; + } + + auto localizer_it = localizer_map.find(id); + if (localizer_it == localizer_map.end()) + { + return; + } + + const Point position = + createPoint(primitive.move().xy_traj_params().start_position()); + const Angle orientation = + createAngle(primitive.move().w_traj_params().start_angle()); + + localizer_it->second.localizer->update( + RobotLocalizer::VisionData{position, orientation, RTT_S / 2}); +} + SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( std::unordered_map>& robot_primitive_executor_map, @@ -370,6 +446,12 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( : sim_state.yellow_robots(); const auto robot_map = getRobotIdToRobotStateMap(sim_robots, side); + const TeamColour team_colour = + (side == gameController::Team::BLUE) ? TeamColour::BLUE : TeamColour::YELLOW; + auto& localizer_map = (side == gameController::Team::BLUE) ? blue_localizer_map + : yellow_localizer_map; + updateRobotLocalizers(localizer_map, robot_map, time_step, team_colour); + for (auto& [robot_id, primitive_executor] : robot_primitive_executor_map) { std::unique_ptr direct_control; @@ -424,6 +506,80 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( return robot_control; } +void ErForceSimulator::updateRobotLocalizers( + std::unordered_map& localizer_map, + const std::map& robot_map, const Duration& time_step, + TeamColour team_colour) +{ + const std::string plotjuggler_tag = + (team_colour == TeamColour::BLUE) ? "_blue_estimated" : "_yellow_estimated"; + + for (const auto& [robot_id, ground_truth] : robot_map) + { + auto localizer_it = localizer_map.find(robot_id); + if (localizer_it == localizer_map.end()) + { + auto localizer = + std::make_shared(RobotLocalizer::RobotLocalizerConfig{ + robot_constants.kalman_process_noise_variance_rad_per_s_4, + robot_constants.kalman_vision_noise_variance_rad_2, + robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2}); + localizer_it = + localizer_map.insert({robot_id, SimulatedLocalization{localizer, + SensorBias{}}}) + .first; + } + SimulatedLocalization& localization = localizer_it->second; + RobotLocalizer& localizer = *localization.localizer; + SensorBias& bias = localization.bias; + + const double motor_variance = + IMU_MOTOR_NOISE_SCALE_FACTOR * + robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2; + const double imu_variance = + IMU_MOTOR_NOISE_SCALE_FACTOR * ImuService::IMU_VARIANCE; + const double dt_seconds = time_step.toSeconds(); + + // IMU: noisy angular velocity, scaled up from the filter's own assumed + // variance (see IMU_MOTOR_NOISE_SCALE_FACTOR). + localizer.update(RobotLocalizer::ImuData{ + ground_truth.angularVelocity() + + AngularVelocity::fromRadians(sampleCorrelatedNoise( + noise_rng_, bias.imu_angular_velocity, dt_seconds, imu_variance))}); + + // Motor sensors: noisy global-frame velocity (ground truth velocity() is + // already global, so no local<->global conversion is needed here, unlike real + // Thunderloop, which converts a local motor reading into global using the + // filter's own orientation estimate). + const Vector motor_velocity_noise( + sampleCorrelatedNoise(noise_rng_, bias.motor_velocity_x, dt_seconds, + motor_variance), + sampleCorrelatedNoise(noise_rng_, bias.motor_velocity_y, dt_seconds, + motor_variance)); + localizer.update(RobotLocalizer::MotorData{ + ground_truth.velocity() + motor_velocity_noise, + ground_truth.angularVelocity() + + AngularVelocity::fromRadians(sampleCorrelatedNoise( + noise_rng_, bias.motor_angular_velocity, dt_seconds, + motor_variance))}); + + // Predict step: matches real Thunderloop, which currently passes a zero + // control input (see RobotLocalizer::step call in thunderloop.cpp). Using a + // ground-truth-derived acceleration here instead would give the filter a + // noise-free "cheat" channel to fall back on whenever it distrusts the + // (deliberately noisy) measurements, undermining the whole point of this + // side-channel comparison. + localizer.step(Vector(), time_step); + + // Vision is NOT synthesized here - see updateLocalizerVisionFromPrimitive(), + // which feeds this localizer the actual vision-derived position the AI used + // to plan this robot's trajectory, whenever a new primitive arrives. + + RobotLocalizer::logToPlotJuggler(robot_id, localizer.getRobotState(), + plotjuggler_tag); + } +} + std::unique_ptr ErForceSimulator::getRampedVelocityPrimitive( const Vector current_local_velocity, diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 6660e2685c..d841d33a1d 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -1,10 +1,13 @@ #pragma once +#include + #include "extlibs/er_force_sim/src/amun/simulator/simulator.h" #include "proto/robot_status_msg.pb.h" #include "proto/ssl_vision_wrapper.pb.h" #include "proto/tbots_software_msgs.pb.h" #include "software/embedded/primitive_executor.h" +#include "software/embedded/robot_localizer.h" #include "software/physics/euclidean_to_wheel.h" #include "software/world/field.h" #include "software/world/robot_state.h" @@ -207,6 +210,69 @@ class ErForceSimulator TbotsProto::DirectControlPrimitive& target_velocity_primitive, Duration time_to_ramp); + /** + * Slowly-drifting per-channel sensor biases (an Ornstein-Uhlenbeck process each), + * modeling correlated real-world error sources like wheel slip or calibration + * drift that persist over time, rather than resetting every sample. Pure + * independent-per-tick white noise gets averaged away almost completely by the + * Kalman filter at a 300 Hz update rate, which understates real tracking error. + */ + struct SensorBias + { + double motor_velocity_x = 0.0; + double motor_velocity_y = 0.0; + double motor_angular_velocity = 0.0; + double imu_angular_velocity = 0.0; + }; + + /** + * Per-robot state for the simulated RobotLocalizer side-channel, persisted across + * ticks. + */ + struct SimulatedLocalization + { + std::shared_ptr localizer; + + // Persistent drifting biases for this robot's synthesized sensors. + SensorBias bias; + }; + + /** + * Steps a RobotLocalizer per robot in robot_map with synthesized noisy motor/imu + * readings derived from ground truth, purely as a side-channel for comparing the + * filter's estimate against ground truth (logged to PlotJuggler). Ground truth + * still drives the robot's actual simulated control; this does not feed back into + * it. Vision updates are NOT synthesized here — see + * updateLocalizerVisionFromPrimitive(), which feeds the localizer the same + * vision-derived position the AI actually used to plan the robot's trajectory. + * + * @param localizer_map The per-robot localizer state to update, kept across ticks + * @param robot_map Ground truth state for each robot this tick + * @param time_step The time step to advance the localizers by + * @param team_colour The team these robots belong to, embedded in the PlotJuggler + * key so yellow and blue robots sharing an ID don't collide onto the same key + */ + void updateRobotLocalizers( + std::unordered_map& localizer_map, + const std::map& robot_map, const Duration& time_step, + TeamColour team_colour); + + /** + * Feeds a robot's RobotLocalizer side-channel the vision-derived start + * position/orientation embedded in a newly-arrived move primitive (the same value + * the AI used to plan this trajectory), rather than synthesizing vision noise + * ourselves. Does nothing if the primitive isn't a move primitive, or if this + * robot doesn't have a localizer yet (it's lazily created on the next physics + * tick by updateRobotLocalizers()). + * + * @param id The id of the robot the primitive is for + * @param primitive The newly-arrived primitive + * @param localizer_map The per-robot localizer state for this robot's team + */ + void updateLocalizerVisionFromPrimitive( + RobotId id, const TbotsProto::Primitive& primitive, + std::unordered_map& localizer_map); + // Map of Robot id to Primitive Executor std::unordered_map> yellow_primitive_executor_map; @@ -244,6 +310,15 @@ class ErForceSimulator std::unordered_map blue_prev_ramp_velocities; std::unordered_map yellow_prev_ramp_velocities; + // Per-robot RobotLocalizer side-channel state, kept across ticks. Purely for + // comparing the filter's estimate against ground truth via PlotJuggler; never + // fed back into control. + std::unordered_map blue_localizer_map; + std::unordered_map yellow_localizer_map; + + // RNG for synthesizing Gaussian sensor noise for the RobotLocalizer side-channel. + std::mt19937 noise_rng_; + const std::string CONFIG_FILE = "simulator/2020"; const std::string CONFIG_DIRECTORY = "extlibs/er_force_sim/config/"; }; From 686601933377f6a22320825dc4c29a8590091837 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 09:14:25 -0700 Subject: [PATCH 21/39] add csv ogging --- src/software/simulation/er_force_simulator.cpp | 18 ++++++++++++++++++ src/software/simulation/er_force_simulator.h | 7 +++++++ 2 files changed, 25 insertions(+) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 39e13319f5..36374a54a8 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -65,6 +65,8 @@ double sampleCorrelatedNoise(std::mt19937& rng, double& bias, double dt_seconds, } } // namespace +const std::string ErForceSimulator::CSV_OUTPUT_PATH = "/tmp/offense_play_test_master.csv"; + ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, const robot_constants::RobotConstants& robot_constants, std::unique_ptr& realism_config, @@ -80,6 +82,12 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, ramping(ramping), noise_rng_(std::random_device{}()) { + robot_localizer_csv_.open(CSV_OUTPUT_PATH); + robot_localizer_csv_ << "team,robot_id,estimated_x,actual_x,estimated_y,actual_y," + "estimated_vel_x,actual_vel_x,estimated_vel_y,actual_vel_y\n"; + LOG(INFO) << "Logging RobotLocalizer estimate-vs-ground-truth data to " + << CSV_OUTPUT_PATH; + std::string full_filename = CONFIG_DIRECTORY; if (field_type == TbotsProto::FieldType::DIV_A) @@ -577,6 +585,16 @@ void ErForceSimulator::updateRobotLocalizers( RobotLocalizer::logToPlotJuggler(robot_id, localizer.getRobotState(), plotjuggler_tag); + + robot_localizer_csv_ << (team_colour == TeamColour::BLUE ? "blue" : "yellow") + << ',' << robot_id << ',' << localizer.getPosition().x() + << ',' << ground_truth.position().x() << ',' + << localizer.getPosition().y() << ',' + << ground_truth.position().y() << ',' + << localizer.getVelocity().x() << ',' + << ground_truth.velocity().x() << ',' + << localizer.getVelocity().y() << ',' + << ground_truth.velocity().y() << '\n'; } } diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index d841d33a1d..f8e427a5a2 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include "extlibs/er_force_sim/src/amun/simulator/simulator.h" @@ -319,6 +320,12 @@ class ErForceSimulator // RNG for synthesizing Gaussian sensor noise for the RobotLocalizer side-channel. std::mt19937 noise_rng_; + // Per-tick estimated-vs-ground-truth log for the RobotLocalizer side-channel. Opened + // once at construction (truncating any previous run's data) and appended to on every + // updateRobotLocalizers() call; see CSV_OUTPUT_PATH. + std::ofstream robot_localizer_csv_; + static const std::string CSV_OUTPUT_PATH; + const std::string CONFIG_FILE = "simulator/2020"; const std::string CONFIG_DIRECTORY = "extlibs/er_force_sim/config/"; }; From 72c23aa1ede155aa63b17f4b4f0f5ca5f6eb1616 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 09:37:49 -0700 Subject: [PATCH 22/39] add testgin --- src/software/simulation/er_force_simulator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 36374a54a8..510255526a 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -65,7 +65,7 @@ double sampleCorrelatedNoise(std::mt19937& rng, double& bias, double dt_seconds, } } // namespace -const std::string ErForceSimulator::CSV_OUTPUT_PATH = "/tmp/offense_play_test_master.csv"; +const std::string ErForceSimulator::CSV_OUTPUT_PATH = "/tmp/master_test_new.csv"; ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, const robot_constants::RobotConstants& robot_constants, From 07ba21e58d3f60a1c14d8093565c07a390855add Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 13:42:12 -0700 Subject: [PATCH 23/39] merge and se real ocalzer for plot juggler --- src/software/embedded/BUILD | 1 + src/software/embedded/primitive_executor.cpp | 7 ------- src/software/embedded/robot_localizer.cpp | 1 - src/software/embedded/robot_localizer.h | 13 ++++++------- src/software/embedded/thunderloop.cpp | 4 +--- .../sensor_fusion/filter/extended_kalman_filter.hpp | 4 ++-- src/software/simulation/er_force_simulator.cpp | 7 +++---- 7 files changed, 13 insertions(+), 24 deletions(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 80ec2c897e..e6524cf00e 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -128,6 +128,7 @@ cc_library( "//software/geom:angular_velocity", "//software/geom:point", "//software/geom:vector", + "//software/physics:velocity_conversion_util", "//software/sensor_fusion/filter:extended_kalman_filter", "//software/sensor_fusion/filter:kalman_filter", "//software/world:robot_state", diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 2a6291b246..4fbad709cb 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -65,13 +65,6 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m void PrimitiveExecutor::updateRobotState(const RobotState& robot_state) { robot_state_ = robot_state; - - // Team colour is embedded in the key since the simulator runs both teams (which - // number robots independently) in one process/log stream; without it, e.g. yellow - // robot 0 and blue robot 0 would collide onto the same PlotJuggler key. - const std::string team_tag = - (team_colour_ == TeamColour::YELLOW) ? "_yellow" : "_blue"; - RobotLocalizer::logToPlotJuggler(robot_id_, robot_state_, team_tag); } Vector PrimitiveExecutor::getPrevCommandedVelocity() const diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 699cc2f6b7..81730fe79c 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -223,7 +223,6 @@ RobotState RobotLocalizer::getRobotState() const return RobotState(getPosition(), getGlobalVelocity(), getOrientation(), getAngularVelocity()); } - // TODO: Investigate proces models/variances/etc void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) { diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 1fe11af9ab..20c65be43e 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -162,16 +162,15 @@ class RobotLocalizer RobotState getRobotState() const; /** - * Logs a robot's position and velocity to PlotJuggler, with the robot ID embedded - * in each key (e.g. "vel_x_robot_4"). + * Logs this localizer's estimated state to PlotJuggler, with the robot ID embedded + * in each key (e.g. "vel_x_robot_4"). Logs both the raw local-frame velocity state + * and the converted global-frame velocity, so the two can be compared. * - * @param robot_id The ID of the robot the state belongs to - * @param robot_state The robot state to log + * @param robot_id The ID of the robot this localizer belongs to * @param tag Optional suffix appended after the robot ID (e.g. "_estimated"), to - * distinguish multiple state sources logged for the same robot + * distinguish multiple localizers logged for the same robot */ - static void logToPlotJuggler(RobotId robot_id, const RobotState& robot_state, - const std::string& tag = ""); + void logToPlotJuggler(RobotId robot_id, const std::string& tag = "") const; private: /** diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index f8ca0bb5ad..e85da69676 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -221,9 +221,7 @@ void Thunderloop::runLoop() updateRobotLocalizer(robot_status_); primitive_executor_->updateRobotState(robot_localizer_->getRobotState()); - - Point position = robot_localizer_->getPosition(); - Vector velocity = robot_localizer_->getVelocity(); + robot_localizer_->logToPlotJuggler(robot_status_.robot_id()); const TbotsProto::DirectControlPrimitive direct_control_primitive = primitive_executor_->stepPrimitive(robot_status_, delta_time); diff --git a/src/software/sensor_fusion/filter/extended_kalman_filter.hpp b/src/software/sensor_fusion/filter/extended_kalman_filter.hpp index 4680bbbf51..9f31af8415 100644 --- a/src/software/sensor_fusion/filter/extended_kalman_filter.hpp +++ b/src/software/sensor_fusion/filter/extended_kalman_filter.hpp @@ -48,13 +48,13 @@ class ExtendedKalmanFilter * The process model f(x): propagates a state forward by one time step. */ using ProcessModelFunction = - std::function(Eigen::Vector, double dt)>; + std::function(Eigen::Vector)>; /** * The Jacobian of the process model (F = df/dx), evaluated at a given state. */ using ProcessModelJacobianFunction = - std::function(Eigen::Vector, double dt)>; + std::function(Eigen::Vector)>; /** * Creates an extended Kalman filter with all internal matrices and vectors set diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 510255526a..36381fde52 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -583,17 +583,16 @@ void ErForceSimulator::updateRobotLocalizers( // which feeds this localizer the actual vision-derived position the AI used // to plan this robot's trajectory, whenever a new primitive arrives. - RobotLocalizer::logToPlotJuggler(robot_id, localizer.getRobotState(), - plotjuggler_tag); + localizer.logToPlotJuggler(robot_id, plotjuggler_tag); robot_localizer_csv_ << (team_colour == TeamColour::BLUE ? "blue" : "yellow") << ',' << robot_id << ',' << localizer.getPosition().x() << ',' << ground_truth.position().x() << ',' << localizer.getPosition().y() << ',' << ground_truth.position().y() << ',' - << localizer.getVelocity().x() << ',' + << localizer.getGlobalVelocity().x() << ',' << ground_truth.velocity().x() << ',' - << localizer.getVelocity().y() << ',' + << localizer.getGlobalVelocity().y() << ',' << ground_truth.velocity().y() << '\n'; } } From 487155118e9de5012dad45219cec887537d15e87 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 13:47:25 -0700 Subject: [PATCH 24/39] fi logign --- src/software/embedded/robot_localizer.h | 19 ++++++++++++++++--- .../simulation/er_force_simulator.cpp | 4 ++++ 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 20c65be43e..302e0df06e 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -162,9 +162,8 @@ class RobotLocalizer RobotState getRobotState() const; /** - * Logs this localizer's estimated state to PlotJuggler, with the robot ID embedded - * in each key (e.g. "vel_x_robot_4"). Logs both the raw local-frame velocity state - * and the converted global-frame velocity, so the two can be compared. + * Logs this localizer's estimated position and global-frame velocity to + * PlotJuggler, with the robot ID embedded in each key (e.g. "vel_x_robot_4"). * * @param robot_id The ID of the robot this localizer belongs to * @param tag Optional suffix appended after the robot ID (e.g. "_estimated"), to @@ -172,6 +171,20 @@ class RobotLocalizer */ void logToPlotJuggler(RobotId robot_id, const std::string& tag = "") const; + /** + * Logs an arbitrary robot state to PlotJuggler, with the robot ID embedded in each + * key (e.g. "vel_x_robot_4"). Useful for logging e.g. ground truth alongside a + * RobotLocalizer's own estimate (see logToPlotJuggler), since ground truth isn't + * backed by a RobotLocalizer instance. + * + * @param robot_id The ID of the robot the state belongs to + * @param robot_state The robot state to log + * @param tag Optional suffix appended after the robot ID (e.g. "_ground_truth"), to + * distinguish multiple state sources logged for the same robot + */ + static void logRobotStateToPlotJuggler(RobotId robot_id, const RobotState& robot_state, + const std::string& tag = ""); + private: /** * Update the Kalman filter with the robot's position and orientation from vision. diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 36381fde52..e4ec0ee841 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -521,6 +521,8 @@ void ErForceSimulator::updateRobotLocalizers( { const std::string plotjuggler_tag = (team_colour == TeamColour::BLUE) ? "_blue_estimated" : "_yellow_estimated"; + const std::string ground_truth_plotjuggler_tag = + (team_colour == TeamColour::BLUE) ? "_blue_ground_truth" : "_yellow_ground_truth"; for (const auto& [robot_id, ground_truth] : robot_map) { @@ -584,6 +586,8 @@ void ErForceSimulator::updateRobotLocalizers( // to plan this robot's trajectory, whenever a new primitive arrives. localizer.logToPlotJuggler(robot_id, plotjuggler_tag); + RobotLocalizer::logRobotStateToPlotJuggler(robot_id, ground_truth, + ground_truth_plotjuggler_tag); robot_localizer_csv_ << (team_colour == TeamColour::BLUE ? "blue" : "yellow") << ',' << robot_id << ',' << localizer.getPosition().x() From 1e9da2187d4d4feee438885ce488d0171e465c07 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 14:06:50 -0700 Subject: [PATCH 25/39] x --- src/software/simulation/er_force_simulator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index e4ec0ee841..61153ebb99 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -65,7 +65,7 @@ double sampleCorrelatedNoise(std::mt19937& rng, double& bias, double dt_seconds, } } // namespace -const std::string ErForceSimulator::CSV_OUTPUT_PATH = "/tmp/master_test_new.csv"; +const std::string ErForceSimulator::CSV_OUTPUT_PATH = "/tmp/sim_test_new.csv"; ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, const robot_constants::RobotConstants& robot_constants, From 71ddbfa14d99cc18ee32ec6db011bab7fef2979c Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Mon, 14 Sep 2026 19:55:37 -0700 Subject: [PATCH 26/39] cleanup --- src/shared/robot_constants.cpp | 6 +- src/shared/robot_constants.h | 1 + src/software/embedded/robot_localizer.cpp | 9 +- src/software/embedded/thunderloop.cpp | 19 +- .../simulation/er_force_simulator.cpp | 177 +----------------- src/software/simulation/er_force_simulator.h | 82 -------- 6 files changed, 27 insertions(+), 267 deletions(-) diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp index c803c238ff..416527e45b 100644 --- a/src/shared/robot_constants.cpp +++ b/src/shared/robot_constants.cpp @@ -53,8 +53,10 @@ RobotConstants createRobotConstants() // Kalman filter variances for robot localizer .kalman_process_noise_variance_rad_per_s_4 = 1.0f, - .kalman_vision_noise_variance_rad_2 = 0.01f * 0.01f, - .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f}; + .kalman_vision_noise_variance_rad_2 = 0.03f, + .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f, + .kalman_motor_sensor_noise_variance_m_per_s_2 = 0.05f + }; } #elif CHECK_VERSION(2021) constexpr RobotConstants createRobotConstants() diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index bfea2224dc..9b7de95b07 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -131,6 +131,7 @@ struct RobotConstants float kalman_vision_noise_variance_rad_2; float kalman_motor_sensor_noise_variance_rad_per_s_2; + float kalman_motor_sensor_noise_variance_m_per_s_2; }; /** diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 81730fe79c..9846d85ff1 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -17,9 +17,12 @@ RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) filter_.measurement_covariance = Eigen::Vector( - config.vision_noise_variance, config.vision_noise_variance, - config.vision_noise_variance, config.motor_sensor_noise_variance, - config.motor_sensor_noise_variance, config.motor_sensor_noise_variance, + 0.0001, + 0.0001, + 0.0001, + 0.5, + 0.5, + 0.5, ImuService::IMU_VARIANCE) .asDiagonal(); } diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index e85da69676..dbb1c86cb6 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -272,13 +272,24 @@ void Thunderloop::updateRobotLocalizer(const TbotsProto::RobotStatus& robot_stat { // Seperate update is okay because measurement model is linear if (robot_status.has_imu_status()){ + AngularVelocity res = createAngularVelocity(robot_status.imu_status().angular_velocity()); + if (res <0.1){ + res = 0; + } robot_localizer_->update(RobotLocalizer::ImuData{ - createAngularVelocity(robot_status.imu_status().angular_velocity())}); + createAngularVelocity(0)}); } if (robot_status.has_motor_status()) - { + Vector velocity = robot_status.motor_status().local_velocity(); + if ( velocity.x() <0.05 && velocity.y() <0.05 ) { + velocity = Vector(0,0); + } + AngularVelocity angular_velocity = robot_status.motor_status().angular_velocity(); + if ( angular_velocity.toRadians() <0.1 ) { + angular_velocity = Angle::zero(); + } robot_localizer_->update(RobotLocalizer::MotorData{ - createVector(robot_status.motor_status().local_velocity()), - createAngularVelocity(robot_status.motor_status().angular_velocity())}); + velocity, angular_velocity +}); } } diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 61153ebb99..c964678e18 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -14,59 +14,10 @@ #include "proto/message_translation/tbots_protobuf.h" #include "proto/robot_status_msg.pb.h" #include "shared/constants.h" -#include "software/embedded/services/imu.h" #include "software/logger/logger.h" #include "software/physics/velocity_conversion_util.h" #include "software/world/robot_state.h" -namespace -{ -double sampleGaussianNoise(std::mt19937& rng, double variance) -{ - std::normal_distribution distribution(0.0, std::sqrt(variance)); - return distribution(rng); -} - -// Most of a synthesized sensor channel's assumed variance is modeled as a slowly -// drifting bias (an Ornstein-Uhlenbeck process) rather than fresh white noise, since -// real error sources like wheel slip or calibration drift persist over time instead of -// resetting every sample; the rest is left as fast white noise for sample-to-sample -// jitter. -constexpr double BIAS_VARIANCE_FRACTION = 0.9; -constexpr double BIAS_TIME_CONSTANT_SECONDS = 0.5; - -// IMU/motor noise is scaled up from the filter's own assumed variance so the -// synthesized sensors show a visible, meaningful divergence from ground truth instead -// of being dominated by (real, correct) vision corrections. -constexpr double IMU_MOTOR_NOISE_SCALE_FACTOR = 3.0; - -// Advances a single drifting bias value by one Euler-Maruyama step of an -// Ornstein-Uhlenbeck process, whose stationary variance equals `stationary_variance` -// and whose fluctuations decorrelate over roughly `BIAS_TIME_CONSTANT_SECONDS`. -void stepDriftingBias(std::mt19937& rng, double& bias, double dt_seconds, - double stationary_variance) -{ - const double mean_reversion_rate = 1.0 / BIAS_TIME_CONSTANT_SECONDS; - const double diffusion_coefficient = - std::sqrt(2.0 * mean_reversion_rate * stationary_variance); - std::normal_distribution distribution(0.0, 1.0); - bias += -mean_reversion_rate * bias * dt_seconds + - diffusion_coefficient * std::sqrt(dt_seconds) * distribution(rng); -} - -// Combines a channel's drifting bias with a smaller fresh white-noise component, both -// drawn from the same total variance per BIAS_VARIANCE_FRACTION. -double sampleCorrelatedNoise(std::mt19937& rng, double& bias, double dt_seconds, - double total_variance) -{ - stepDriftingBias(rng, bias, dt_seconds, BIAS_VARIANCE_FRACTION * total_variance); - return bias + - sampleGaussianNoise(rng, (1.0 - BIAS_VARIANCE_FRACTION) * total_variance); -} -} // namespace - -const std::string ErForceSimulator::CSV_OUTPUT_PATH = "/tmp/sim_test_new.csv"; - ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, const robot_constants::RobotConstants& robot_constants, std::unique_ptr& realism_config, @@ -79,15 +30,8 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, field(Field::createField(field_type)), blue_robot_with_ball(std::nullopt), yellow_robot_with_ball(std::nullopt), - ramping(ramping), - noise_rng_(std::random_device{}()) + ramping(ramping) { - robot_localizer_csv_.open(CSV_OUTPUT_PATH); - robot_localizer_csv_ << "team,robot_id,estimated_x,actual_x,estimated_y,actual_y," - "estimated_vel_x,actual_vel_x,estimated_vel_y,actual_vel_y\n"; - LOG(INFO) << "Logging RobotLocalizer estimate-vs-ground-truth data to " - << CSV_OUTPUT_PATH; - std::string full_filename = CONFIG_DIRECTORY; if (field_type == TbotsProto::FieldType::DIV_A) @@ -367,7 +311,6 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( { setRobotPrimitive(robot_id, primitive_set_msg, yellow_primitive_executor_map, robot_map.at(robot_id)); - updateLocalizerVisionFromPrimitive(robot_id, primitive, yellow_localizer_map); } } } @@ -388,7 +331,6 @@ void ErForceSimulator::setBlueRobotPrimitiveSet( { setRobotPrimitive(robot_id, primitive_set_msg, blue_primitive_executor_map, robot_map.at(robot_id)); - updateLocalizerVisionFromPrimitive(robot_id, primitive, blue_localizer_map); } } } @@ -416,30 +358,6 @@ void ErForceSimulator::setRobotPrimitive( } } -void ErForceSimulator::updateLocalizerVisionFromPrimitive( - RobotId id, const TbotsProto::Primitive& primitive, - std::unordered_map& localizer_map) -{ - if (!primitive.has_move()) - { - return; - } - - auto localizer_it = localizer_map.find(id); - if (localizer_it == localizer_map.end()) - { - return; - } - - const Point position = - createPoint(primitive.move().xy_traj_params().start_position()); - const Angle orientation = - createAngle(primitive.move().w_traj_params().start_angle()); - - localizer_it->second.localizer->update( - RobotLocalizer::VisionData{position, orientation, RTT_S / 2}); -} - SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( std::unordered_map>& robot_primitive_executor_map, @@ -454,12 +372,6 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( : sim_state.yellow_robots(); const auto robot_map = getRobotIdToRobotStateMap(sim_robots, side); - const TeamColour team_colour = - (side == gameController::Team::BLUE) ? TeamColour::BLUE : TeamColour::YELLOW; - auto& localizer_map = (side == gameController::Team::BLUE) ? blue_localizer_map - : yellow_localizer_map; - updateRobotLocalizers(localizer_map, robot_map, time_step, team_colour); - for (auto& [robot_id, primitive_executor] : robot_primitive_executor_map) { std::unique_ptr direct_control; @@ -514,93 +426,6 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( return robot_control; } -void ErForceSimulator::updateRobotLocalizers( - std::unordered_map& localizer_map, - const std::map& robot_map, const Duration& time_step, - TeamColour team_colour) -{ - const std::string plotjuggler_tag = - (team_colour == TeamColour::BLUE) ? "_blue_estimated" : "_yellow_estimated"; - const std::string ground_truth_plotjuggler_tag = - (team_colour == TeamColour::BLUE) ? "_blue_ground_truth" : "_yellow_ground_truth"; - - for (const auto& [robot_id, ground_truth] : robot_map) - { - auto localizer_it = localizer_map.find(robot_id); - if (localizer_it == localizer_map.end()) - { - auto localizer = - std::make_shared(RobotLocalizer::RobotLocalizerConfig{ - robot_constants.kalman_process_noise_variance_rad_per_s_4, - robot_constants.kalman_vision_noise_variance_rad_2, - robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2}); - localizer_it = - localizer_map.insert({robot_id, SimulatedLocalization{localizer, - SensorBias{}}}) - .first; - } - SimulatedLocalization& localization = localizer_it->second; - RobotLocalizer& localizer = *localization.localizer; - SensorBias& bias = localization.bias; - - const double motor_variance = - IMU_MOTOR_NOISE_SCALE_FACTOR * - robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2; - const double imu_variance = - IMU_MOTOR_NOISE_SCALE_FACTOR * ImuService::IMU_VARIANCE; - const double dt_seconds = time_step.toSeconds(); - - // IMU: noisy angular velocity, scaled up from the filter's own assumed - // variance (see IMU_MOTOR_NOISE_SCALE_FACTOR). - localizer.update(RobotLocalizer::ImuData{ - ground_truth.angularVelocity() + - AngularVelocity::fromRadians(sampleCorrelatedNoise( - noise_rng_, bias.imu_angular_velocity, dt_seconds, imu_variance))}); - - // Motor sensors: noisy global-frame velocity (ground truth velocity() is - // already global, so no local<->global conversion is needed here, unlike real - // Thunderloop, which converts a local motor reading into global using the - // filter's own orientation estimate). - const Vector motor_velocity_noise( - sampleCorrelatedNoise(noise_rng_, bias.motor_velocity_x, dt_seconds, - motor_variance), - sampleCorrelatedNoise(noise_rng_, bias.motor_velocity_y, dt_seconds, - motor_variance)); - localizer.update(RobotLocalizer::MotorData{ - ground_truth.velocity() + motor_velocity_noise, - ground_truth.angularVelocity() + - AngularVelocity::fromRadians(sampleCorrelatedNoise( - noise_rng_, bias.motor_angular_velocity, dt_seconds, - motor_variance))}); - - // Predict step: matches real Thunderloop, which currently passes a zero - // control input (see RobotLocalizer::step call in thunderloop.cpp). Using a - // ground-truth-derived acceleration here instead would give the filter a - // noise-free "cheat" channel to fall back on whenever it distrusts the - // (deliberately noisy) measurements, undermining the whole point of this - // side-channel comparison. - localizer.step(Vector(), time_step); - - // Vision is NOT synthesized here - see updateLocalizerVisionFromPrimitive(), - // which feeds this localizer the actual vision-derived position the AI used - // to plan this robot's trajectory, whenever a new primitive arrives. - - localizer.logToPlotJuggler(robot_id, plotjuggler_tag); - RobotLocalizer::logRobotStateToPlotJuggler(robot_id, ground_truth, - ground_truth_plotjuggler_tag); - - robot_localizer_csv_ << (team_colour == TeamColour::BLUE ? "blue" : "yellow") - << ',' << robot_id << ',' << localizer.getPosition().x() - << ',' << ground_truth.position().x() << ',' - << localizer.getPosition().y() << ',' - << ground_truth.position().y() << ',' - << localizer.getGlobalVelocity().x() << ',' - << ground_truth.velocity().x() << ',' - << localizer.getGlobalVelocity().y() << ',' - << ground_truth.velocity().y() << '\n'; - } -} - std::unique_ptr ErForceSimulator::getRampedVelocityPrimitive( const Vector current_local_velocity, diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index f8e427a5a2..6660e2685c 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -1,14 +1,10 @@ #pragma once -#include -#include - #include "extlibs/er_force_sim/src/amun/simulator/simulator.h" #include "proto/robot_status_msg.pb.h" #include "proto/ssl_vision_wrapper.pb.h" #include "proto/tbots_software_msgs.pb.h" #include "software/embedded/primitive_executor.h" -#include "software/embedded/robot_localizer.h" #include "software/physics/euclidean_to_wheel.h" #include "software/world/field.h" #include "software/world/robot_state.h" @@ -211,69 +207,6 @@ class ErForceSimulator TbotsProto::DirectControlPrimitive& target_velocity_primitive, Duration time_to_ramp); - /** - * Slowly-drifting per-channel sensor biases (an Ornstein-Uhlenbeck process each), - * modeling correlated real-world error sources like wheel slip or calibration - * drift that persist over time, rather than resetting every sample. Pure - * independent-per-tick white noise gets averaged away almost completely by the - * Kalman filter at a 300 Hz update rate, which understates real tracking error. - */ - struct SensorBias - { - double motor_velocity_x = 0.0; - double motor_velocity_y = 0.0; - double motor_angular_velocity = 0.0; - double imu_angular_velocity = 0.0; - }; - - /** - * Per-robot state for the simulated RobotLocalizer side-channel, persisted across - * ticks. - */ - struct SimulatedLocalization - { - std::shared_ptr localizer; - - // Persistent drifting biases for this robot's synthesized sensors. - SensorBias bias; - }; - - /** - * Steps a RobotLocalizer per robot in robot_map with synthesized noisy motor/imu - * readings derived from ground truth, purely as a side-channel for comparing the - * filter's estimate against ground truth (logged to PlotJuggler). Ground truth - * still drives the robot's actual simulated control; this does not feed back into - * it. Vision updates are NOT synthesized here — see - * updateLocalizerVisionFromPrimitive(), which feeds the localizer the same - * vision-derived position the AI actually used to plan the robot's trajectory. - * - * @param localizer_map The per-robot localizer state to update, kept across ticks - * @param robot_map Ground truth state for each robot this tick - * @param time_step The time step to advance the localizers by - * @param team_colour The team these robots belong to, embedded in the PlotJuggler - * key so yellow and blue robots sharing an ID don't collide onto the same key - */ - void updateRobotLocalizers( - std::unordered_map& localizer_map, - const std::map& robot_map, const Duration& time_step, - TeamColour team_colour); - - /** - * Feeds a robot's RobotLocalizer side-channel the vision-derived start - * position/orientation embedded in a newly-arrived move primitive (the same value - * the AI used to plan this trajectory), rather than synthesizing vision noise - * ourselves. Does nothing if the primitive isn't a move primitive, or if this - * robot doesn't have a localizer yet (it's lazily created on the next physics - * tick by updateRobotLocalizers()). - * - * @param id The id of the robot the primitive is for - * @param primitive The newly-arrived primitive - * @param localizer_map The per-robot localizer state for this robot's team - */ - void updateLocalizerVisionFromPrimitive( - RobotId id, const TbotsProto::Primitive& primitive, - std::unordered_map& localizer_map); - // Map of Robot id to Primitive Executor std::unordered_map> yellow_primitive_executor_map; @@ -311,21 +244,6 @@ class ErForceSimulator std::unordered_map blue_prev_ramp_velocities; std::unordered_map yellow_prev_ramp_velocities; - // Per-robot RobotLocalizer side-channel state, kept across ticks. Purely for - // comparing the filter's estimate against ground truth via PlotJuggler; never - // fed back into control. - std::unordered_map blue_localizer_map; - std::unordered_map yellow_localizer_map; - - // RNG for synthesizing Gaussian sensor noise for the RobotLocalizer side-channel. - std::mt19937 noise_rng_; - - // Per-tick estimated-vs-ground-truth log for the RobotLocalizer side-channel. Opened - // once at construction (truncating any previous run's data) and appended to on every - // updateRobotLocalizers() call; see CSV_OUTPUT_PATH. - std::ofstream robot_localizer_csv_; - static const std::string CSV_OUTPUT_PATH; - const std::string CONFIG_FILE = "simulator/2020"; const std::string CONFIG_DIRECTORY = "extlibs/er_force_sim/config/"; }; From b03907940bb1f98aaaedf9b1b3d0bbedd2798c3d Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Mon, 14 Sep 2026 20:28:24 -0700 Subject: [PATCH 27/39] q --- src/shared/constants.h | 2 +- src/shared/robot_constants.cpp | 2 +- src/software/embedded/BUILD | 1 - src/software/embedded/robot_localizer.cpp | 11 +++------- src/software/embedded/robot_localizer.h | 26 ----------------------- src/software/embedded/thunderloop.cpp | 1 - 6 files changed, 5 insertions(+), 38 deletions(-) diff --git a/src/shared/constants.h b/src/shared/constants.h index 5c4c68df6f..c31b062b2d 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -30,7 +30,7 @@ static const std::string LOOPBACK_INTERFACE = "lo"; #endif // PlotJuggler's default host and port -static const std::string PLOTJUGGLER_GUI_DEFAULT_HOST = "127.0.0.1"; +static const std::string PLOTJUGGLER_GUI_DEFAULT_HOST = "ff02::c3d0:42d2:aaaa"; static const short unsigned int PLOTJUGGLER_GUI_DEFAULT_PORT = 9870; // ProtoLogger constants for replay files diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp index 416527e45b..cbca9ea90d 100644 --- a/src/shared/robot_constants.cpp +++ b/src/shared/robot_constants.cpp @@ -53,7 +53,7 @@ RobotConstants createRobotConstants() // Kalman filter variances for robot localizer .kalman_process_noise_variance_rad_per_s_4 = 1.0f, - .kalman_vision_noise_variance_rad_2 = 0.03f, + .kalman_vision_noise_variance_rad_2 = 0.0001f, .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f, .kalman_motor_sensor_noise_variance_m_per_s_2 = 0.05f }; diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index e6524cf00e..6f236fc70d 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -120,7 +120,6 @@ cc_library( hdrs = ["robot_localizer.h"], deps = [ "//proto:tbots_cc_proto", - "//proto/message_translation:tbots_protobuf", "//proto/primitive:primitive_msg_factory", "//software:constants", "//software/embedded/services:imu", diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 9846d85ff1..6dd4d96b19 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -1,10 +1,8 @@ #include "robot_localizer.h" -#include "software/logger/logger.h" #include #include "proto/message_translation/tbots_geometry.h" -#include "proto/message_translation/tbots_protobuf.h" #include "shared/constants.h" #include "software/physics/velocity_conversion_util.h" @@ -17,12 +15,9 @@ RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) filter_.measurement_covariance = Eigen::Vector( - 0.0001, - 0.0001, - 0.0001, - 0.5, - 0.5, - 0.5, + config.vision_noise_variance, config.vision_noise_variance, + config.vision_noise_variance, config.motor_sensor_noise_variance, + config.motor_sensor_noise_variance, config.motor_sensor_noise_variance, ImuService::IMU_VARIANCE) .asDiagonal(); } diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 302e0df06e..14d023ca80 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -3,10 +3,8 @@ #include #include #include -#include #include "proto/primitive.pb.h" -#include "proto/robot_status_msg.pb.h" #include "software/embedded/services/imu.h" #include "software/geom/angle.h" #include "software/geom/point.h" @@ -161,30 +159,6 @@ class RobotLocalizer */ RobotState getRobotState() const; - /** - * Logs this localizer's estimated position and global-frame velocity to - * PlotJuggler, with the robot ID embedded in each key (e.g. "vel_x_robot_4"). - * - * @param robot_id The ID of the robot this localizer belongs to - * @param tag Optional suffix appended after the robot ID (e.g. "_estimated"), to - * distinguish multiple localizers logged for the same robot - */ - void logToPlotJuggler(RobotId robot_id, const std::string& tag = "") const; - - /** - * Logs an arbitrary robot state to PlotJuggler, with the robot ID embedded in each - * key (e.g. "vel_x_robot_4"). Useful for logging e.g. ground truth alongside a - * RobotLocalizer's own estimate (see logToPlotJuggler), since ground truth isn't - * backed by a RobotLocalizer instance. - * - * @param robot_id The ID of the robot the state belongs to - * @param robot_state The robot state to log - * @param tag Optional suffix appended after the robot ID (e.g. "_ground_truth"), to - * distinguish multiple state sources logged for the same robot - */ - static void logRobotStateToPlotJuggler(RobotId robot_id, const RobotState& robot_state, - const std::string& tag = ""); - private: /** * Update the Kalman filter with the robot's position and orientation from vision. diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index dbb1c86cb6..c3348d124a 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -221,7 +221,6 @@ void Thunderloop::runLoop() updateRobotLocalizer(robot_status_); primitive_executor_->updateRobotState(robot_localizer_->getRobotState()); - robot_localizer_->logToPlotJuggler(robot_status_.robot_id()); const TbotsProto::DirectControlPrimitive direct_control_primitive = primitive_executor_->stepPrimitive(robot_status_, delta_time); From 07a86952a5567b71564157f5e7f44318682711e2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Tue, 15 Sep 2026 03:41:29 +0000 Subject: [PATCH 28/39] [pre-commit.ci lite] apply automatic fixes --- src/shared/robot_constants.cpp | 3 +- src/software/embedded/robot_localizer.cpp | 51 ++++++++----------- src/software/embedded/robot_localizer.h | 2 +- .../embedded/robot_localizer_test.cpp | 8 +-- src/software/embedded/thunderloop.cpp | 42 +++++++-------- 5 files changed, 51 insertions(+), 55 deletions(-) diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp index cbca9ea90d..77949c94f6 100644 --- a/src/shared/robot_constants.cpp +++ b/src/shared/robot_constants.cpp @@ -55,8 +55,7 @@ RobotConstants createRobotConstants() .kalman_process_noise_variance_rad_per_s_4 = 1.0f, .kalman_vision_noise_variance_rad_2 = 0.0001f, .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f, - .kalman_motor_sensor_noise_variance_m_per_s_2 = 0.05f - }; + .kalman_motor_sensor_noise_variance_m_per_s_2 = 0.05f}; } #elif CHECK_VERSION(2021) constexpr RobotConstants createRobotConstants() diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 6dd4d96b19..8317b52149 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -57,8 +57,8 @@ void RobotLocalizer::update(const VisionData& data) [&](const FilterStep& step) { return (current_time_seconds_ - step.time_seconds) >= data.age_seconds; }); - // If rollback point is at the start, vision is newer than all history steps - // So we empty history and apply vision + // If rollback point is at the start, vision is newer than all history steps + // So we empty history and apply vision if (rollback_point == history.begin()) { updateFilterWithVision(data.position, data.orientation); @@ -66,8 +66,8 @@ void RobotLocalizer::update(const VisionData& data) return; } - // If rollback point is at the end, vision is older than all history steps - // So rollback ever step + // If rollback point is at the end, vision is older than all history steps + // So rollback ever step if (rollback_point == history.end()) { rollback_point = std::prev(history.end()); @@ -127,7 +127,7 @@ void RobotLocalizer::updateFilterWithVision(const Point& position, measurement(static_cast(MeasurementIndex::VISION_Y_POSITION)) = position.y(); - // Integrating omega for position makes angule goes out of bounds so we wrap it around + // Integrating omega for position makes angule goes out of bounds so we wrap it around measurement(static_cast(MeasurementIndex::VISION_ORIENTATION)) = orientation_estimate + (orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians(); @@ -221,7 +221,7 @@ RobotState RobotLocalizer::getRobotState() const return RobotState(getPosition(), getGlobalVelocity(), getOrientation(), getAngularVelocity()); } -// TODO: Investigate proces models/variances/etc +// TODO: Investigate process models/variances/etc void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) { // Velocity is estimated in the robot's local frame (see StateIndex), but position @@ -235,12 +235,9 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) filter_.process_model_function = [delta_time_seconds](Eigen::Vector state) { - const double theta = - state(static_cast(StateIndex::ORIENTATION)); - const double local_vx = - state(static_cast(StateIndex::X_VELOCITY)); - const double local_vy = - state(static_cast(StateIndex::Y_VELOCITY)); + const double theta = state(static_cast(StateIndex::ORIENTATION)); + const double local_vx = state(static_cast(StateIndex::X_VELOCITY)); + const double local_vy = state(static_cast(StateIndex::Y_VELOCITY)); Eigen::Vector next_state = Eigen::Vector::Zero(); @@ -265,20 +262,19 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) filter_.process_model_jacobian_function = [delta_time_seconds](Eigen::Vector state) { - const auto x_position_index = static_cast(StateIndex::X_POSITION); - const auto y_position_index = static_cast(StateIndex::Y_POSITION); - const auto orientation_index = - static_cast(StateIndex::ORIENTATION); - const auto x_velocity_index = static_cast(StateIndex::X_VELOCITY); - const auto y_velocity_index = static_cast(StateIndex::Y_VELOCITY); + const auto x_position_index = static_cast(StateIndex::X_POSITION); + const auto y_position_index = static_cast(StateIndex::Y_POSITION); + const auto orientation_index = static_cast(StateIndex::ORIENTATION); + const auto x_velocity_index = static_cast(StateIndex::X_VELOCITY); + const auto y_velocity_index = static_cast(StateIndex::Y_VELOCITY); const auto angular_velocity_index = static_cast(StateIndex::ANGULAR_VELOCITY); - const double theta = state(orientation_index); - const double local_vx = state(x_velocity_index); - const double local_vy = state(y_velocity_index); - const double cos_theta = std::cos(theta); - const double sin_theta = std::sin(theta); + const double theta = state(orientation_index); + const double local_vx = state(x_velocity_index); + const double local_vy = state(y_velocity_index); + const double cos_theta = std::cos(theta); + const double sin_theta = std::sin(theta); Eigen::Matrix jacobian = Eigen::Matrix::Identity(); @@ -354,18 +350,15 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) control_model.setZero(); control_model(static_cast(StateIndex::X_VELOCITY), - static_cast(ControlIndex::X_VELOCITY_TARGET)) = - cos_theta; + static_cast(ControlIndex::X_VELOCITY_TARGET)) = cos_theta; control_model(static_cast(StateIndex::X_VELOCITY), - static_cast(ControlIndex::Y_VELOCITY_TARGET)) = - sin_theta; + static_cast(ControlIndex::Y_VELOCITY_TARGET)) = sin_theta; control_model(static_cast(StateIndex::Y_VELOCITY), static_cast(ControlIndex::X_VELOCITY_TARGET)) = -sin_theta; control_model(static_cast(StateIndex::Y_VELOCITY), - static_cast(ControlIndex::Y_VELOCITY_TARGET)) = - cos_theta; + static_cast(ControlIndex::Y_VELOCITY_TARGET)) = cos_theta; } void RobotLocalizer::generateMeasurementModel(FilterStepType source) diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 14d023ca80..6d76b6fada 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -213,7 +213,7 @@ class RobotLocalizer // during replay (see generateMeasurementModel). std::optional> measurement; - // Post operation state + // Post operation state Eigen::Vector state_estimate; Eigen::Matrix state_covariance; diff --git a/src/software/embedded/robot_localizer_test.cpp b/src/software/embedded/robot_localizer_test.cpp index 52efe91fcb..e7b5df2339 100644 --- a/src/software/embedded/robot_localizer_test.cpp +++ b/src/software/embedded/robot_localizer_test.cpp @@ -44,7 +44,8 @@ RobotLocalizer runConstantVelocity(bool feed_vision, double vision_age = RTT_S / const Vector local_velocity = globalToLocalVelocity(true_velocity, true_orientation); - localizer.update(RobotLocalizer::MotorData{local_velocity, AngularVelocity::zero()}); + localizer.update( + RobotLocalizer::MotorData{local_velocity, AngularVelocity::zero()}); localizer.predict(Vector(0.0, 0.0), Duration::fromSeconds(DT)); @@ -69,8 +70,9 @@ TEST(RobotLocalizer, tracks_constant_forward_velocity) const RobotLocalizer localizer = runConstantVelocity(/*feed_vision=*/true); std::cerr << "[motor+vision] pos=(" << localizer.getPosition().x() << ", " - << localizer.getPosition().y() << ") vel=(" << localizer.getGlobalVelocity().x() - << ", " << localizer.getGlobalVelocity().y() + << localizer.getPosition().y() << ") vel=(" + << localizer.getGlobalVelocity().x() << ", " + << localizer.getGlobalVelocity().y() << ") orient=" << localizer.getOrientation().toDegrees() << "deg\n"; // NOTE: we assert on velocity and orientation, not absolute position. RobotLocalizer diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index c3348d124a..2175c398cc 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -269,26 +269,28 @@ void Thunderloop::updateRobotLocalizer(const TbotsProto::Primitive& primitive) void Thunderloop::updateRobotLocalizer(const TbotsProto::RobotStatus& robot_status) { - // Seperate update is okay because measurement model is linear - if (robot_status.has_imu_status()){ - AngularVelocity res = createAngularVelocity(robot_status.imu_status().angular_velocity()); - if (res <0.1){ - res = 0; - } - robot_localizer_->update(RobotLocalizer::ImuData{ - createAngularVelocity(0)}); - } + // Separate update is okay because measurement model is linear + if (robot_status.has_imu_status()) + { + AngularVelocity res = + createAngularVelocity(robot_status.imu_status().angular_velocity()); + if (res < 0.1) + { + res = 0; + } + robot_localizer_->update(RobotLocalizer::ImuData{createAngularVelocity(0)}); + } if (robot_status.has_motor_status()) - Vector velocity = robot_status.motor_status().local_velocity(); - if ( velocity.x() <0.05 && velocity.y() <0.05 ) { - velocity = Vector(0,0); - } - AngularVelocity angular_velocity = robot_status.motor_status().angular_velocity(); - if ( angular_velocity.toRadians() <0.1 ) { - angular_velocity = Angle::zero(); - } - robot_localizer_->update(RobotLocalizer::MotorData{ - velocity, angular_velocity -}); + Vector velocity = robot_status.motor_status().local_velocity(); + if (velocity.x() < 0.05 && velocity.y() < 0.05) + { + velocity = Vector(0, 0); + } + AngularVelocity angular_velocity = robot_status.motor_status().angular_velocity(); + if (angular_velocity.toRadians() < 0.1) + { + angular_velocity = Angle::zero(); } + robot_localizer_->update(RobotLocalizer::MotorData{velocity, angular_velocity}); +} } From 2c8abf360c58d5d9ea9376a87c2a1831bd36f572 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Mon, 14 Sep 2026 22:15:15 -0700 Subject: [PATCH 29/39] fix --- src/software/embedded/thunderloop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 40a4da3e96..28d491fce5 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -271,7 +271,7 @@ void Thunderloop::updateRobotLocalizer(const TbotsProto::RobotStatus& robot_stat { // Seperate update is okay because measurement model is linear if (robot_status.has_imu_status()){ - robot_localizer_.update(RobotLocalizer::ImuData{ + robot_localizer_->update(RobotLocalizer::ImuData{ createAngularVelocity(robot_status.imu_status().angular_velocity()) }) From 5f7ee4c5b1b4e71981547a5df2cbb49877fc6edf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Tue, 15 Sep 2026 05:21:23 +0000 Subject: [PATCH 30/39] [pre-commit.ci lite] apply automatic fixes --- src/software/embedded/robot_localizer.cpp | 26 ++++++++++------------- src/software/embedded/robot_localizer.h | 2 +- src/software/embedded/thunderloop.cpp | 13 ++++++------ 3 files changed, 18 insertions(+), 23 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index b12db39256..b01bd064e1 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -55,8 +55,8 @@ void RobotLocalizer::update(const VisionData& data) [&](const FilterStep& step) { return (current_time_seconds_ - step.time_seconds) >= data.age_seconds; }); - // If rollback point is at the start, vision is newer than all history steps - // So we empty history and apply vision + // If rollback point is at the start, vision is newer than all history steps + // So we empty history and apply vision if (rollback_point == history.begin()) { updateFilterWithVision(data.position, data.orientation); @@ -64,8 +64,8 @@ void RobotLocalizer::update(const VisionData& data) return; } - // If rollback point is at the end, vision is older than all history steps - // So rollback ever step + // If rollback point is at the end, vision is older than all history steps + // So rollback ever step if (rollback_point == history.end()) { rollback_point = std::prev(history.end()); @@ -125,7 +125,7 @@ void RobotLocalizer::updateFilterWithVision(const Point& position, measurement(static_cast(MeasurementIndex::VISION_Y_POSITION)) = position.y(); - // Integrating omega for position makes angule goes out of bounds so we wrap it around + // Integrating omega for position makes angule goes out of bounds so we wrap it around measurement(static_cast(MeasurementIndex::VISION_ORIENTATION)) = orientation_estimate + (orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians(); @@ -215,18 +215,14 @@ RobotState RobotLocalizer::getRobotState() const getAngularVelocity()); } -// TODO: Investigate proces models/variances/etc +// TODO: Investigate process models/variances/etc void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) { - // In the current model, we use target velocity as our new velocity of the preiction state, and position is derived from it. - // Therefore, process model keeps the positions and we don't predict it using estimated velocities - filter_.process_model << - 1, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, - 0, 0, 1, 0, 0, delta_time_seconds, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1; + // In the current model, we use target velocity as our new velocity of the preiction + // state, and position is derived from it. Therefore, process model keeps the + // positions and we don't predict it using estimated velocities + filter_.process_model << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, + delta_time_seconds, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; // clang-format on const double delta_time_squared = delta_time_seconds * delta_time_seconds; diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 695632d560..5dd92cff9f 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -185,7 +185,7 @@ class RobotLocalizer // during replay (see generateMeasurementModel). std::optional> measurement; - // Post operation state + // Post operation state Eigen::Vector state_estimate; Eigen::Matrix state_covariance; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 28d491fce5..3fc758b41d 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -269,13 +269,12 @@ void Thunderloop::updateRobotLocalizer(const TbotsProto::Primitive& primitive) void Thunderloop::updateRobotLocalizer(const TbotsProto::RobotStatus& robot_status) { - // Seperate update is okay because measurement model is linear - if (robot_status.has_imu_status()){ - robot_localizer_->update(RobotLocalizer::ImuData{ - createAngularVelocity(robot_status.imu_status().angular_velocity()) - }) - - } + // Separate update is okay because measurement model is linear + if (robot_status.has_imu_status()) + { + robot_localizer_->update(RobotLocalizer::ImuData{ + createAngularVelocity(robot_status.imu_status().angular_velocity())}) + } if (robot_status.has_motor_status()) { robot_localizer_->update(RobotLocalizer::MotorData{ From e1ec566bc5c1b1bb246f6361d5b3fa73b16deaaa Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Mon, 14 Sep 2026 22:27:16 -0700 Subject: [PATCH 31/39] move folder --- src/software/embedded/BUILD | 31 --------------- src/software/embedded/robot_localizer/BUILD | 39 +++++++++++++++++++ .../{ => robot_localizer}/robot_localizer.cpp | 0 .../{ => robot_localizer}/robot_localizer.h | 13 ++++--- .../robot_localizer_test.cpp | 0 5 files changed, 47 insertions(+), 36 deletions(-) create mode 100644 src/software/embedded/robot_localizer/BUILD rename src/software/embedded/{ => robot_localizer}/robot_localizer.cpp (100%) rename src/software/embedded/{ => robot_localizer}/robot_localizer.h (95%) rename src/software/embedded/{ => robot_localizer}/robot_localizer_test.cpp (100%) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 6f236fc70d..2044684f54 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -114,34 +114,3 @@ filegroup( srcs = ["hash_thunderloop_binary.sh"], ) -cc_library( - name = "robot_localizer", - srcs = ["robot_localizer.cpp"], - hdrs = ["robot_localizer.h"], - deps = [ - "//proto:tbots_cc_proto", - "//proto/primitive:primitive_msg_factory", - "//software:constants", - "//software/embedded/services:imu", - "//software/geom:angle", - "//software/geom:angular_velocity", - "//software/geom:point", - "//software/geom:vector", - "//software/physics:velocity_conversion_util", - "//software/sensor_fusion/filter:extended_kalman_filter", - "//software/sensor_fusion/filter:kalman_filter", - "//software/world:robot_state", - "@eigen", - ], -) - -cc_test( - name = "robot_localizer_test", - srcs = ["robot_localizer_test.cpp"], - deps = [ - ":robot_localizer", - "//shared:constants", - "//shared/test_util:tbots_gtest_main", - "//software/physics:velocity_conversion_util", - ], -) diff --git a/src/software/embedded/robot_localizer/BUILD b/src/software/embedded/robot_localizer/BUILD new file mode 100644 index 0000000000..8eab73470f --- /dev/null +++ b/src/software/embedded/robot_localizer/BUILD @@ -0,0 +1,39 @@ +load("@bazel_skylib//rules:common_settings.bzl", "bool_flag", "string_flag") +load("@rules_cc//cc:cc_binary.bzl", "cc_binary") +load("@rules_cc//cc:cc_library.bzl", "cc_library") +load("@rules_cc//cc:cc_test.bzl", "cc_test") +load("@rules_shell//shell:sh_binary.bzl", "sh_binary") + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "robot_localizer", + srcs = ["robot_localizer.cpp"], + hdrs = ["robot_localizer.h"], + deps = [ + "//proto:tbots_cc_proto", + "//proto/primitive:primitive_msg_factory", + "//software:constants", + "//software/embedded/services:imu", + "//software/geom:angle", + "//software/geom:angular_velocity", + "//software/geom:point", + "//software/geom:vector", + "//software/physics:velocity_conversion_util", + "//software/sensor_fusion/filter:extended_kalman_filter", + "//software/sensor_fusion/filter:kalman_filter", + "//software/world:robot_state", + "@eigen", + ], +) + +cc_test( + name = "robot_localizer_test", + srcs = ["robot_localizer_test.cpp"], + deps = [ + ":robot_localizer", + "//shared:constants", + "//shared/test_util:tbots_gtest_main", + "//software/physics:velocity_conversion_util", + ], +) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer/robot_localizer.cpp similarity index 100% rename from src/software/embedded/robot_localizer.cpp rename to src/software/embedded/robot_localizer/robot_localizer.cpp diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer/robot_localizer.h similarity index 95% rename from src/software/embedded/robot_localizer.h rename to src/software/embedded/robot_localizer/robot_localizer.h index 6d76b6fada..6550201d37 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer/robot_localizer.h @@ -14,14 +14,9 @@ #include "software/util/make_enum/make_enum.hpp" #include "software/world/robot_state.h" -// X_POSITION/Y_POSITION are in world space; X_VELOCITY/Y_VELOCITY are in the robot's -// local frame (see velocity_conversion_util.h), matching what the motor sensors report -// directly and avoiding a lossy conversion through the orientation estimate. MAKE_ENUM(StateIndex, X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY, ANGULAR_VELOCITY); -// MOTOR_X_VELOCITY/MOTOR_Y_VELOCITY are in the robot's local frame, matching -// StateIndex::X_VELOCITY/Y_VELOCITY. MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTATION, MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, IMU_ANGULAR_VELOCITY); @@ -30,6 +25,14 @@ MAKE_ENUM(ControlIndex, X_VELOCITY_TARGET, Y_VELOCITY_TARGET); MAKE_ENUM(FilterStepType, PREDICT, MOTOR_DATA, IMU_DATA, VISION_DATA); +static constexpr VISION_X_INITIAL_VARIANCE_M = 0.00001; +static constexpr VISION_Y_INITIAL_VARIANCE_M = 0.00001; +static constexpr VISION_THETA_INITIAL_VARIANCE_RAD = 0.00001; +static constexpr MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; +static constexpr MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; +static constexpr MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; + +static constexpr PROCESS_MODEL_INITIAL_VARIANCE = 0.5; /** * Estimates robot position, orientation, velocity, and angular velocity using an * extended Kalman filter. diff --git a/src/software/embedded/robot_localizer_test.cpp b/src/software/embedded/robot_localizer/robot_localizer_test.cpp similarity index 100% rename from src/software/embedded/robot_localizer_test.cpp rename to src/software/embedded/robot_localizer/robot_localizer_test.cpp From edb5a179435f696c9355c66ddb12adba5fa90f28 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Mon, 14 Sep 2026 22:35:51 -0700 Subject: [PATCH 32/39] localizer constants --- .../robot_localizer/robot_localizer.h | 19 ------- .../robot_localizer_constants.h | 53 +++++++++++++++++++ 2 files changed, 53 insertions(+), 19 deletions(-) create mode 100644 src/software/embedded/robot_localizer/robot_localizer_constants.h diff --git a/src/software/embedded/robot_localizer/robot_localizer.h b/src/software/embedded/robot_localizer/robot_localizer.h index 6550201d37..1a9085ba58 100644 --- a/src/software/embedded/robot_localizer/robot_localizer.h +++ b/src/software/embedded/robot_localizer/robot_localizer.h @@ -14,25 +14,6 @@ #include "software/util/make_enum/make_enum.hpp" #include "software/world/robot_state.h" -MAKE_ENUM(StateIndex, X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY, - ANGULAR_VELOCITY); - -MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTATION, - MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, - IMU_ANGULAR_VELOCITY); - -MAKE_ENUM(ControlIndex, X_VELOCITY_TARGET, Y_VELOCITY_TARGET); - -MAKE_ENUM(FilterStepType, PREDICT, MOTOR_DATA, IMU_DATA, VISION_DATA); - -static constexpr VISION_X_INITIAL_VARIANCE_M = 0.00001; -static constexpr VISION_Y_INITIAL_VARIANCE_M = 0.00001; -static constexpr VISION_THETA_INITIAL_VARIANCE_RAD = 0.00001; -static constexpr MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; -static constexpr MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; -static constexpr MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; - -static constexpr PROCESS_MODEL_INITIAL_VARIANCE = 0.5; /** * Estimates robot position, orientation, velocity, and angular velocity using an * extended Kalman filter. diff --git a/src/software/embedded/robot_localizer/robot_localizer_constants.h b/src/software/embedded/robot_localizer/robot_localizer_constants.h new file mode 100644 index 0000000000..78a02dbb3d --- /dev/null +++ b/src/software/embedded/robot_localizer/robot_localizer_constants.h @@ -0,0 +1,53 @@ +#include +MAKE_ENUM(StateIndex, X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY, + ANGULAR_VELOCITY); + +MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTATION, + MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, + IMU_ANGULAR_VELOCITY); + +MAKE_ENUM(ControlIndex, X_VELOCITY_TARGET, Y_VELOCITY_TARGET); + +MAKE_ENUM(FilterStepType, PREDICT, MOTOR_DATA, IMU_DATA, VISION_DATA); + + +static constexpr size_t STATE_SIZE = reflective_enum::size(); +static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size(); +static constexpr size_t CONTROL_SIZE = reflective_enum::size(); + +// Initial Covariances +static constexpr double VISION_X_INITIAL_VARIANCE_M = 0.00001; +static constexpr double VISION_Y_INITIAL_VARIANCE_M = 0.00001; +static constexpr double VISION_THETA_INITIAL_VARIANCE_RAD = 0.00001; + +static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; +static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; +static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; + +static constexpr double PROCESS_MODEL_INITIAL_VARIANCE = 0.5; + + +// Measurement models +static constexpr Eigen::Matrix VISION_MEASUREMENT_COVARIANCE << +1,0,0,0,0,0,0, +0,1,0,0,0,0,0, +0,0,1,0,0,0,0, +0,0,0,0,0,0,0, +0,0,0,0,0,0,0, +0,0,0,0,0,0,0; + +static constexpr Eigen::Matrix MOTOR_MEASUREMENT_COVARIANCE << +0,0,0,0,0,0,0, +0,0,0,0,0,0,0, +0,0,0,0,0,0,0; +0,0,0,1,0,0,0, +0,0,0,0,1,0,0, +0,0,0,0,0,1,0, + +static constexpr Eigen::Matrix IMU_MEASUREMENT_COVARIANCE << +0,0,0,0,0,0,0, +0,0,0,0,0,0,0, +0,0,0,0,0,0,0; +0,0,0,0,0,0,0, +0,0,0,0,0,0,0, +0,0,0,0,0,0,1, From 9e205e1b33004343645bcc863992b20aac936977 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Mon, 14 Sep 2026 22:39:38 -0700 Subject: [PATCH 33/39] use variables --- .../robot_localizer/robot_localizer.cpp | 29 +++---------------- .../robot_localizer/robot_localizer.h | 17 +---------- .../robot_localizer_constants.h | 14 ++++----- 3 files changed, 12 insertions(+), 48 deletions(-) diff --git a/src/software/embedded/robot_localizer/robot_localizer.cpp b/src/software/embedded/robot_localizer/robot_localizer.cpp index 8317b52149..0d975fd009 100644 --- a/src/software/embedded/robot_localizer/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer/robot_localizer.cpp @@ -6,9 +6,7 @@ #include "shared/constants.h" #include "software/physics/velocity_conversion_util.h" -RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) - : process_linear_velocity_noise_variance_(config.process_noise_variance), - process_angular_acceleration_noise_variance_(config.process_noise_variance) +RobotLocalizer::RobotLocalizer() { filter_.state_covariance = Eigen::Vector(1, 1, 1, 1, 1, 1).asDiagonal(); @@ -368,34 +366,15 @@ void RobotLocalizer::generateMeasurementModel(FilterStepType source) switch (source) { case FilterStepType::VISION_DATA: - filter_.measurement_model( - static_cast(MeasurementIndex::VISION_X_POSITION), - static_cast(StateIndex::X_POSITION)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::VISION_Y_POSITION), - static_cast(StateIndex::Y_POSITION)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::VISION_ORIENTATION), - static_cast(StateIndex::ORIENTATION)) = 1; + filter_.measurement_model = VISION_MEASUREMENT_MODEL; break; case FilterStepType::MOTOR_DATA: - filter_.measurement_model( - static_cast(MeasurementIndex::MOTOR_X_VELOCITY), - static_cast(StateIndex::X_VELOCITY)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::MOTOR_Y_VELOCITY), - static_cast(StateIndex::Y_VELOCITY)) = 1; - filter_.measurement_model( - static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY), - static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + filter_.measurement_model = MOTOR_MEASUREMENT_MODEL; break; case FilterStepType::IMU_DATA: - filter_.measurement_model( - static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY), - static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + filter_.measurement_model = IMU_MEASUREMENT_MODEL; break; case FilterStepType::PREDICT: - // Never called with PREDICT; predict steps use generatedPredictionMatrices. break; } } diff --git a/src/software/embedded/robot_localizer/robot_localizer.h b/src/software/embedded/robot_localizer/robot_localizer.h index 1a9085ba58..0ba695b848 100644 --- a/src/software/embedded/robot_localizer/robot_localizer.h +++ b/src/software/embedded/robot_localizer/robot_localizer.h @@ -50,12 +50,6 @@ class RobotLocalizer AngularVelocity angular_velocity; }; - struct RobotLocalizerConfig - { - double process_noise_variance; - double vision_noise_variance; - double motor_sensor_noise_variance; - }; /** * Creates a new robot localizer. @@ -64,7 +58,7 @@ class RobotLocalizer * * @param config Configuration for the localizer variances. */ - explicit RobotLocalizer(const RobotLocalizerConfig& config); + explicit RobotLocalizer(); /** * Runs one prediction step over the given elapsed time. @@ -176,9 +170,6 @@ class RobotLocalizer */ void generateMeasurementModel(FilterStepType source); - static constexpr size_t STATE_SIZE = reflective_enum::size(); - static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size(); - static constexpr size_t CONTROL_SIZE = reflective_enum::size(); /** * Snapshot of a Kalman filter predict/update step needed for rollback/replay. @@ -206,12 +197,6 @@ class RobotLocalizer ExtendedKalmanFilter filter_; - // Process noise variance used in prediction. The linear term models how much - // actual velocity deviates from the commanded target velocity (a rate, per unit - // time); the angular term models unmeasured angular acceleration disturbance. - double process_linear_velocity_noise_variance_; - double process_angular_acceleration_noise_variance_; - // History is ordered newest-first (front is the most recent step) std::deque history; diff --git a/src/software/embedded/robot_localizer/robot_localizer_constants.h b/src/software/embedded/robot_localizer/robot_localizer_constants.h index 78a02dbb3d..dd5575a7be 100644 --- a/src/software/embedded/robot_localizer/robot_localizer_constants.h +++ b/src/software/embedded/robot_localizer/robot_localizer_constants.h @@ -28,7 +28,7 @@ static constexpr double PROCESS_MODEL_INITIAL_VARIANCE = 0.5; // Measurement models -static constexpr Eigen::Matrix VISION_MEASUREMENT_COVARIANCE << +static constexpr Eigen::Matrix VISION_MEASUREMENT_MODEL << 1,0,0,0,0,0,0, 0,1,0,0,0,0,0, 0,0,1,0,0,0,0, @@ -36,18 +36,18 @@ static constexpr Eigen::Matrix VISIO 0,0,0,0,0,0,0, 0,0,0,0,0,0,0; -static constexpr Eigen::Matrix MOTOR_MEASUREMENT_COVARIANCE << +static constexpr Eigen::Matrix MOTOR_MEASUREMENT_MODEL << +0,0,0,0,0,0,0, 0,0,0,0,0,0,0, 0,0,0,0,0,0,0, -0,0,0,0,0,0,0; 0,0,0,1,0,0,0, 0,0,0,0,1,0,0, -0,0,0,0,0,1,0, +0,0,0,0,0,1,0; -static constexpr Eigen::Matrix IMU_MEASUREMENT_COVARIANCE << +static constexpr Eigen::Matrix IMU_MEASUREMENT_MODEL<< +0,0,0,0,0,0,0, 0,0,0,0,0,0,0, 0,0,0,0,0,0,0, -0,0,0,0,0,0,0; 0,0,0,0,0,0,0, 0,0,0,0,0,0,0, -0,0,0,0,0,0,1, +0,0,0,0,0,0,1; From 2feaa75ee208ac9848a919b826624aebf1900c01 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Mon, 14 Sep 2026 22:42:53 -0700 Subject: [PATCH 34/39] use constants --- src/software/embedded/robot_localizer/robot_localizer.cpp | 6 +++--- .../embedded/robot_localizer/robot_localizer_constants.h | 2 +- .../embedded/robot_localizer/robot_localizer_test.cpp | 8 +------- src/software/embedded/thunderloop.cpp | 5 +---- 4 files changed, 6 insertions(+), 15 deletions(-) diff --git a/src/software/embedded/robot_localizer/robot_localizer.cpp b/src/software/embedded/robot_localizer/robot_localizer.cpp index 0d975fd009..0056159113 100644 --- a/src/software/embedded/robot_localizer/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer/robot_localizer.cpp @@ -13,9 +13,9 @@ RobotLocalizer::RobotLocalizer() filter_.measurement_covariance = Eigen::Vector( - config.vision_noise_variance, config.vision_noise_variance, - config.vision_noise_variance, config.motor_sensor_noise_variance, - config.motor_sensor_noise_variance, config.motor_sensor_noise_variance, + VISION_X_INITIAL_VARIANCE_M, VISION_Y_INITIAL_VARIANCE_M, + VISION_THETA_INITIAL_VARIANCE_RAD, MOTOR_X_INITIAL_VARIANCE_M, + MOTOR_Y_INITIAL_VARIANCE_M, MOTOR_THETA_INITIAL_VARIANCE_RAD, ImuService::IMU_VARIANCE) .asDiagonal(); } diff --git a/src/software/embedded/robot_localizer/robot_localizer_constants.h b/src/software/embedded/robot_localizer/robot_localizer_constants.h index dd5575a7be..e2f5accefc 100644 --- a/src/software/embedded/robot_localizer/robot_localizer_constants.h +++ b/src/software/embedded/robot_localizer/robot_localizer_constants.h @@ -22,7 +22,7 @@ static constexpr double VISION_THETA_INITIAL_VARIANCE_RAD = 0.00001; static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; -static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; +static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_RAD = 0.5; static constexpr double PROCESS_MODEL_INITIAL_VARIANCE = 0.5; diff --git a/src/software/embedded/robot_localizer/robot_localizer_test.cpp b/src/software/embedded/robot_localizer/robot_localizer_test.cpp index e7b5df2339..6f4e68fd5d 100644 --- a/src/software/embedded/robot_localizer/robot_localizer_test.cpp +++ b/src/software/embedded/robot_localizer/robot_localizer_test.cpp @@ -11,12 +11,6 @@ namespace { // Mirror the values thunderloop constructs the localizer with (DivB constants). -RobotLocalizer::RobotLocalizerConfig makeConfig() -{ - return RobotLocalizer::RobotLocalizerConfig{/*process_noise_variance=*/1.0, - /*vision_noise_variance=*/0.01 * 0.01, - /*motor_sensor_noise_variance=*/0.5}; -} constexpr double LOOP_HZ = 300.0; constexpr double DT = 1.0 / LOOP_HZ; @@ -27,7 +21,7 @@ constexpr double DT = 1.0 / LOOP_HZ; // provided (isolates whether the periodic vision fix corrupts the velocity estimate). RobotLocalizer runConstantVelocity(bool feed_vision, double vision_age = RTT_S / 2) { - RobotLocalizer localizer(makeConfig()); + RobotLocalizer localizer(); const Vector true_velocity(1.0, 0.0); const Angle true_orientation = Angle::zero(); diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 2175c398cc..d26fbdce3e 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -148,10 +148,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, LOG(INFO) << "THUNDERLOOP: IMU Service initialized!"; robot_localizer_ = - std::make_unique(RobotLocalizer::RobotLocalizerConfig{ - robot_constants.kalman_process_noise_variance_rad_per_s_4, - robot_constants.kalman_vision_noise_variance_rad_2, - robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2}); + std::make_unique(); LOG(INFO) << "THUNDERLOOP: Robot Localizer initialized!"; primitive_executor_ = std::make_unique(robot_constants, robot_id); From ba792151ac90574a4ebf8764519eb4a27dd1018c Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Mon, 14 Sep 2026 22:45:06 -0700 Subject: [PATCH 35/39] implemetnq --- src/software/embedded/robot_localizer/robot_localizer.cpp | 2 +- .../embedded/robot_localizer/robot_localizer_constants.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/robot_localizer/robot_localizer.cpp b/src/software/embedded/robot_localizer/robot_localizer.cpp index 0056159113..ed3efe8790 100644 --- a/src/software/embedded/robot_localizer/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer/robot_localizer.cpp @@ -9,7 +9,7 @@ RobotLocalizer::RobotLocalizer() { filter_.state_covariance = - Eigen::Vector(1, 1, 1, 1, 1, 1).asDiagonal(); + PROCESS_MODEL_INITIAL_VARIANCE * Eigen::Vector(1, 1, 1, 1, 1, 1).asDiagonal(); filter_.measurement_covariance = Eigen::Vector( diff --git a/src/software/embedded/robot_localizer/robot_localizer_constants.h b/src/software/embedded/robot_localizer/robot_localizer_constants.h index e2f5accefc..910d1c11ff 100644 --- a/src/software/embedded/robot_localizer/robot_localizer_constants.h +++ b/src/software/embedded/robot_localizer/robot_localizer_constants.h @@ -24,7 +24,7 @@ static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_RAD = 0.5; -static constexpr double PROCESS_MODEL_INITIAL_VARIANCE = 0.5; +static constexpr double PROCESS_MODEL_INITIAL_VARIANCE = 1; // Measurement models From 19bb6059d6cbc0265c7c36d9f75677f417a784c4 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Mon, 14 Sep 2026 22:46:18 -0700 Subject: [PATCH 36/39] update path --- src/software/embedded/robot_localizer/robot_localizer.h | 1 + src/software/embedded/thunderloop.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/software/embedded/robot_localizer/robot_localizer.h b/src/software/embedded/robot_localizer/robot_localizer.h index 0ba695b848..9e84f1cc66 100644 --- a/src/software/embedded/robot_localizer/robot_localizer.h +++ b/src/software/embedded/robot_localizer/robot_localizer.h @@ -13,6 +13,7 @@ #include "software/time/duration.h" #include "software/util/make_enum/make_enum.hpp" #include "software/world/robot_state.h" +#include "software/enmbedded/robot_localizer/robot_localizer_constants.h" /** * Estimates robot position, orientation, velocity, and angular velocity using an diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 68e4ff5766..137e63a468 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -4,7 +4,7 @@ #include "shared/robot_constants.h" #include "software/embedded/primitive_executor.h" -#include "software/embedded/robot_localizer.h" +#include "software/embedded/robot_localizer/robot_localizer.h" #include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" #include "software/embedded/services/network/network.h" From 151e20448617e8a48047ef7756f9c914eb9efc42 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Mon, 14 Sep 2026 23:03:25 -0700 Subject: [PATCH 37/39] add more constants and fix build --- src/software/embedded/BUILD | 4 +- src/software/embedded/primitive_executor.cpp | 2 +- src/software/embedded/robot_localizer/BUILD | 2 +- .../robot_localizer/robot_localizer.cpp | 29 ++++----- .../robot_localizer/robot_localizer.h | 2 +- .../robot_localizer_constants.h | 65 +++++++++++-------- .../robot_localizer/robot_localizer_test.cpp | 2 +- 7 files changed, 58 insertions(+), 48 deletions(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 2044684f54..bdd2a95b4f 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -39,7 +39,7 @@ cc_library( "//proto/primitive:primitive_msg_factory", "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular", "//software/ai/navigator/trajectory:trajectory_path", - "//software/embedded:robot_localizer", + "//software/embedded/robot_localizer:robot_localizer", "//software/embedded/motion_control:orientation_controller", "//software/embedded/motion_control:position_controller", "//software/math:math_functions", @@ -72,7 +72,7 @@ cc_library( }), deps = [ ":primitive_executor", - ":robot_localizer", + "//software/embedded/robot_localizer:robot_localizer", "//proto:tbots_cc_proto", "//software/embedded/services:imu", "//software/embedded/services:motor", diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 4fbad709cb..401e23d8e5 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -8,7 +8,7 @@ #include "proto/primitive/primitive_msg_factory.h" #include "proto/tbots_software_msgs.pb.h" #include "proto/visualization.pb.h" -#include "software/embedded/robot_localizer.h" +#include "software/embedded/robot_localizer/robot_localizer.h" #include "software/geom/algorithms/distance.h" #include "software/logger/logger.h" #include "software/physics/velocity_conversion_util.h" diff --git a/src/software/embedded/robot_localizer/BUILD b/src/software/embedded/robot_localizer/BUILD index 8eab73470f..5be401f50d 100644 --- a/src/software/embedded/robot_localizer/BUILD +++ b/src/software/embedded/robot_localizer/BUILD @@ -9,7 +9,7 @@ package(default_visibility = ["//visibility:public"]) cc_library( name = "robot_localizer", srcs = ["robot_localizer.cpp"], - hdrs = ["robot_localizer.h"], + hdrs = ["robot_localizer.h", "robot_localizer_constants.h"], deps = [ "//proto:tbots_cc_proto", "//proto/primitive:primitive_msg_factory", diff --git a/src/software/embedded/robot_localizer/robot_localizer.cpp b/src/software/embedded/robot_localizer/robot_localizer.cpp index ed3efe8790..c63ab78d24 100644 --- a/src/software/embedded/robot_localizer/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer/robot_localizer.cpp @@ -14,8 +14,8 @@ RobotLocalizer::RobotLocalizer() filter_.measurement_covariance = Eigen::Vector( VISION_X_INITIAL_VARIANCE_M, VISION_Y_INITIAL_VARIANCE_M, - VISION_THETA_INITIAL_VARIANCE_RAD, MOTOR_X_INITIAL_VARIANCE_M, - MOTOR_Y_INITIAL_VARIANCE_M, MOTOR_THETA_INITIAL_VARIANCE_RAD, + VISION_THETA_INITIAL_VARIANCE_RAD, MOTOR_X_INITIAL_VARIANCE_M_S, + MOTOR_Y_INITIAL_VARIANCE_M_S, MOTOR_THETA_INITIAL_VARIANCE_RAD_S, ImuService::IMU_VARIANCE) .asDiagonal(); } @@ -302,25 +302,24 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) const double delta_time_cubed = delta_time_squared * delta_time_seconds; const double delta_time_fourth = delta_time_cubed * delta_time_seconds; - // Linear terms model velocity itself as the noisy quantity (how much actual - // velocity deviates from the commanded target velocity), integrated once into - // position, rather than a noisy acceleration integrated twice. const double linear_position_variance = - delta_time_cubed * process_linear_velocity_noise_variance_; + delta_time_cubed * PROCESS_LINEAR_VELOCITY_NOISE_VARIANCE; + const double linear_position_velocity_covariance = - delta_time_squared * process_linear_velocity_noise_variance_; + delta_time_squared * PROCESS_LINEAR_VELOCITY_NOISE_VARIANCE; + const double linear_velocity_variance = - delta_time_seconds * process_linear_velocity_noise_variance_; - - // Angular terms are unchanged: angular velocity has no control input, so it's - // still modeled as a noisy acceleration integrated twice. + delta_time_seconds * PROCESS_LINEAR_VELOCITY_NOISE_VARIANCE; + const double angular_position_variance = - delta_time_fourth / 4 * process_angular_acceleration_noise_variance_; + (delta_time_fourth / 4.0) * PROCESS_ANGULAR_ACCELERATION_NOISE_VARIANCE; + const double angular_position_velocity_covariance = - delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; + (delta_time_cubed / 2.0) * PROCESS_ANGULAR_ACCELERATION_NOISE_VARIANCE; + const double angular_velocity_variance = - delta_time_squared * process_angular_acceleration_noise_variance_; - + delta_time_squared * PROCESS_ANGULAR_ACCELERATION_NOISE_VARIANCE; + // State order: X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY, // ANGULAR_VELOCITY // clang-format off diff --git a/src/software/embedded/robot_localizer/robot_localizer.h b/src/software/embedded/robot_localizer/robot_localizer.h index 9e84f1cc66..0035697b8b 100644 --- a/src/software/embedded/robot_localizer/robot_localizer.h +++ b/src/software/embedded/robot_localizer/robot_localizer.h @@ -13,7 +13,7 @@ #include "software/time/duration.h" #include "software/util/make_enum/make_enum.hpp" #include "software/world/robot_state.h" -#include "software/enmbedded/robot_localizer/robot_localizer_constants.h" +#include "software/embedded/robot_localizer/robot_localizer_constants.h" /** * Estimates robot position, orientation, velocity, and angular velocity using an diff --git a/src/software/embedded/robot_localizer/robot_localizer_constants.h b/src/software/embedded/robot_localizer/robot_localizer_constants.h index 910d1c11ff..eb7e17637a 100644 --- a/src/software/embedded/robot_localizer/robot_localizer_constants.h +++ b/src/software/embedded/robot_localizer/robot_localizer_constants.h @@ -20,34 +20,45 @@ static constexpr double VISION_X_INITIAL_VARIANCE_M = 0.00001; static constexpr double VISION_Y_INITIAL_VARIANCE_M = 0.00001; static constexpr double VISION_THETA_INITIAL_VARIANCE_RAD = 0.00001; -static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; -static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_S = 0.5; -static constexpr double MOTOR_THETA_INITIAL_VARIANCE_M_RAD = 0.5; +static constexpr double MOTOR_X_INITIAL_VARIANCE_M_S = 0.5; +static constexpr double MOTOR_Y_INITIAL_VARIANCE_M_S = 0.5; +static constexpr double MOTOR_THETA_INITIAL_VARIANCE_RAD_S = 0.5; static constexpr double PROCESS_MODEL_INITIAL_VARIANCE = 1; -// Measurement models -static constexpr Eigen::Matrix VISION_MEASUREMENT_MODEL << -1,0,0,0,0,0,0, -0,1,0,0,0,0,0, -0,0,1,0,0,0,0, -0,0,0,0,0,0,0, -0,0,0,0,0,0,0, -0,0,0,0,0,0,0; - -static constexpr Eigen::Matrix MOTOR_MEASUREMENT_MODEL << -0,0,0,0,0,0,0, -0,0,0,0,0,0,0, -0,0,0,0,0,0,0, -0,0,0,1,0,0,0, -0,0,0,0,1,0,0, -0,0,0,0,0,1,0; - -static constexpr Eigen::Matrix IMU_MEASUREMENT_MODEL<< -0,0,0,0,0,0,0, -0,0,0,0,0,0,0, -0,0,0,0,0,0,0, -0,0,0,0,0,0,0, -0,0,0,0,0,0,0, -0,0,0,0,0,0,1; +static constexpr double PROCESS_LINEAR_VELOCITY_NOISE_VARIANCE = 1; +static constexpr double PROCESS_ANGULAR_ACCELERATION_NOISE_VARIANCE = 1; + +static const Eigen::Matrix VISION_MEASUREMENT_MODEL = []{ + Eigen::Matrix m; + m << 1,0,0,0,0,0,0, + 0,1,0,0,0,0,0, + 0,0,1,0,0,0,0, + 0,0,0,0,0,0,0, + 0,0,0,0,0,0,0, + 0,0,0,0,0,0,0; + return m; +}(); + +static const Eigen::Matrix MOTOR_MEASUREMENT_MODEL = []{ + Eigen::Matrix m; + m << 0,0,0,0,0,0,0, + 0,0,0,0,0,0,0, + 0,0,0,0,0,0,0, + 0,0,0,1,0,0,0, + 0,0,0,0,1,0,0, + 0,0,0,0,0,1,0; + return m; +}(); + +static const Eigen::Matrix IMU_MEASUREMENT_MODEL = []{ + Eigen::Matrix m; + m << 0,0,0,0,0,0,0, + 0,0,0,0,0,0,0, + 0,0,0,0,0,0,0, + 0,0,0,0,0,0,0, + 0,0,0,0,0,0,0, + 0,0,0,0,0,0,1; + return m; +}(); diff --git a/src/software/embedded/robot_localizer/robot_localizer_test.cpp b/src/software/embedded/robot_localizer/robot_localizer_test.cpp index 6f4e68fd5d..31e2a1733b 100644 --- a/src/software/embedded/robot_localizer/robot_localizer_test.cpp +++ b/src/software/embedded/robot_localizer/robot_localizer_test.cpp @@ -1,4 +1,4 @@ -#include "software/embedded/robot_localizer.h" +#include "robot_localizer.h" #include From 91f0e6f975ac91f7728e4523d48fba7ab364b802 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Mon, 14 Sep 2026 23:07:27 -0700 Subject: [PATCH 38/39] fix bug --- src/software/embedded/thunderloop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 28d491fce5..b4a864d518 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -273,7 +273,7 @@ void Thunderloop::updateRobotLocalizer(const TbotsProto::RobotStatus& robot_stat if (robot_status.has_imu_status()){ robot_localizer_->update(RobotLocalizer::ImuData{ createAngularVelocity(robot_status.imu_status().angular_velocity()) - }) + }); } if (robot_status.has_motor_status()) From 754876cbf7a9bf303ae95fdc51b203077166bfd2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Tue, 15 Sep 2026 05:21:23 +0000 Subject: [PATCH 39/39] [pre-commit.ci lite] apply automatic fixes --- src/software/embedded/robot_localizer.cpp | 26 ++++++++++------------- src/software/embedded/robot_localizer.h | 2 +- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index b12db39256..b01bd064e1 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -55,8 +55,8 @@ void RobotLocalizer::update(const VisionData& data) [&](const FilterStep& step) { return (current_time_seconds_ - step.time_seconds) >= data.age_seconds; }); - // If rollback point is at the start, vision is newer than all history steps - // So we empty history and apply vision + // If rollback point is at the start, vision is newer than all history steps + // So we empty history and apply vision if (rollback_point == history.begin()) { updateFilterWithVision(data.position, data.orientation); @@ -64,8 +64,8 @@ void RobotLocalizer::update(const VisionData& data) return; } - // If rollback point is at the end, vision is older than all history steps - // So rollback ever step + // If rollback point is at the end, vision is older than all history steps + // So rollback ever step if (rollback_point == history.end()) { rollback_point = std::prev(history.end()); @@ -125,7 +125,7 @@ void RobotLocalizer::updateFilterWithVision(const Point& position, measurement(static_cast(MeasurementIndex::VISION_Y_POSITION)) = position.y(); - // Integrating omega for position makes angule goes out of bounds so we wrap it around + // Integrating omega for position makes angule goes out of bounds so we wrap it around measurement(static_cast(MeasurementIndex::VISION_ORIENTATION)) = orientation_estimate + (orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians(); @@ -215,18 +215,14 @@ RobotState RobotLocalizer::getRobotState() const getAngularVelocity()); } -// TODO: Investigate proces models/variances/etc +// TODO: Investigate process models/variances/etc void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds) { - // In the current model, we use target velocity as our new velocity of the preiction state, and position is derived from it. - // Therefore, process model keeps the positions and we don't predict it using estimated velocities - filter_.process_model << - 1, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, - 0, 0, 1, 0, 0, delta_time_seconds, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1; + // In the current model, we use target velocity as our new velocity of the preiction + // state, and position is derived from it. Therefore, process model keeps the + // positions and we don't predict it using estimated velocities + filter_.process_model << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, + delta_time_seconds, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; // clang-format on const double delta_time_squared = delta_time_seconds * delta_time_seconds; diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 695632d560..5dd92cff9f 100644 --- a/src/software/embedded/robot_localizer.h +++ b/src/software/embedded/robot_localizer.h @@ -185,7 +185,7 @@ class RobotLocalizer // during replay (see generateMeasurementModel). std::optional> measurement; - // Post operation state + // Post operation state Eigen::Vector state_estimate; Eigen::Matrix state_covariance;