From 1f57f9232b108e42245c3679814b0b2a87c57842 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sat, 12 Sep 2026 23:27:48 -0700 Subject: [PATCH 01/14] i thik works --- 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 | 14 ++ 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, 296 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 64ea96171e..f6f8fd1f95 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", @@ -126,6 +128,7 @@ cc_library( "//software/geom:angular_velocity", "//software/geom:point", "//software/geom:vector", + "//software/logger", "//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 3035bf4969..00ee92d6cb 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::stepTargetLinearVelocity(const Duration& delta_time) diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index b3b07a4548..69916827a8 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. @@ -110,6 +116,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 cda68b5607..cd95fb0f9a 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -1,6 +1,8 @@ #include "robot_localizer.h" +#include "software/logger/logger.h" #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" @@ -327,3 +329,15 @@ RobotState RobotLocalizer::getRobotState() const return RobotState(getPosition(), getVelocity(), getOrientation(), getAngularVelocity()); } + +void RobotLocalizer::logToPlotJuggler(RobotId robot_id, const RobotState& robot_state, + const std::string& tag) +{ + const std::string robot_suffix = "_robot_" + std::to_string(robot_id) + tag; + + LOG(PLOTJUGGLER) << *createPlotJugglerValue( + {{"pos_x" + robot_suffix, robot_state.position().x()}, + {"pos_y" + robot_suffix, robot_state.position().y()}, + {"vel_x" + robot_suffix, robot_state.velocity().x()}, + {"vel_y" + robot_suffix, robot_state.velocity().y()}}); +} diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h index 07a718adf1..00b462f97e 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" @@ -135,6 +136,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 fd88172a42..950baa292f 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -16,6 +16,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/physics/velocity_conversion_util.h" @@ -219,6 +220,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 88f775671d43ffda1efa10896c2db1a8ece65f6c Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 09:14:25 -0700 Subject: [PATCH 02/14] 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 00d34a6b35cd7edf8b771e57a640282d6e1e83d0 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 09:37:49 -0700 Subject: [PATCH 03/14] 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 c67876a771..1ad2ac27e2 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 833aabbddea196baf82a74017837256fb15eded0 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Sun, 13 Sep 2026 13:12:46 -0700 Subject: [PATCH 04/14] 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 67ca3784977f4cbc2c52714e864c59f29fe3a4d0 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 13:16:36 -0700 Subject: [PATCH 05/14] 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 32ecf4beed2ad41ced0b06920ba6a55b365435aa Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 13:17:40 -0700 Subject: [PATCH 06/14] 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 840825c6d5cb359762b15600d5165d8d0420537e Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 13:28:51 -0700 Subject: [PATCH 07/14] 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 e89fba498147d4f0b6c4fc51de284863584d259c Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 13:42:12 -0700 Subject: [PATCH 08/14] 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 | 17 +++++++++++------ src/software/embedded/robot_localizer.h | 13 ++++++------- src/software/embedded/thunderloop.cpp | 4 +--- .../filter/extended_kalman_filter.hpp | 4 ++-- src/software/simulation/er_force_simulator.cpp | 7 +++---- 7 files changed, 24 insertions(+), 29 deletions(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index b2457a1ffa..a8ee0cc12c 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/logger", "//software/sensor_fusion/filter:kalman_filter", 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 31e5b4f6d0..a625bb2db0 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -224,16 +224,21 @@ RobotState RobotLocalizer::getRobotState() const getAngularVelocity()); } -void RobotLocalizer::logToPlotJuggler(RobotId robot_id, const RobotState& robot_state, - const std::string& tag) +void RobotLocalizer::logToPlotJuggler(RobotId robot_id, const std::string& tag) const { const std::string robot_suffix = "_robot_" + std::to_string(robot_id) + tag; + const Point position = getPosition(); + const Vector global_velocity = getGlobalVelocity(); + const Vector local_velocity = getLocalVelocity(); + LOG(PLOTJUGGLER) << *createPlotJugglerValue( - {{"pos_x" + robot_suffix, robot_state.position().x()}, - {"pos_y" + robot_suffix, robot_state.position().y()}, - {"vel_x" + robot_suffix, robot_state.velocity().x()}, - {"vel_y" + robot_suffix, robot_state.velocity().y()}}); + {{"pos_x" + robot_suffix, position.x()}, + {"pos_y" + robot_suffix, position.y()}, + {"vel_x" + robot_suffix, global_velocity.x()}, + {"vel_y" + robot_suffix, global_velocity.y()}, + {"local_vel_x" + robot_suffix, local_velocity.x()}, + {"local_vel_y" + robot_suffix, local_velocity.y()}}); } // TODO: Investigate proces models/variances/etc 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 1ad2ac27e2..44e4f52bee 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -593,17 +593,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 1ea093f22142eefc49bcc62b811d5acd89944f3e Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 13:47:25 -0700 Subject: [PATCH 09/14] fi logign --- src/software/embedded/robot_localizer.cpp | 18 ++++++++++++++---- src/software/embedded/robot_localizer.h | 19 ++++++++++++++++--- .../simulation/er_force_simulator.cpp | 4 ++++ 3 files changed, 34 insertions(+), 7 deletions(-) diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index a625bb2db0..f9e089f15f 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -230,15 +230,25 @@ void RobotLocalizer::logToPlotJuggler(RobotId robot_id, const std::string& tag) const Point position = getPosition(); const Vector global_velocity = getGlobalVelocity(); - const Vector local_velocity = getLocalVelocity(); LOG(PLOTJUGGLER) << *createPlotJugglerValue( {{"pos_x" + robot_suffix, position.x()}, {"pos_y" + robot_suffix, position.y()}, {"vel_x" + robot_suffix, global_velocity.x()}, - {"vel_y" + robot_suffix, global_velocity.y()}, - {"local_vel_x" + robot_suffix, local_velocity.x()}, - {"local_vel_y" + robot_suffix, local_velocity.y()}}); + {"vel_y" + robot_suffix, global_velocity.y()}}); +} + +void RobotLocalizer::logRobotStateToPlotJuggler(RobotId robot_id, + const RobotState& robot_state, + const std::string& tag) +{ + const std::string robot_suffix = "_robot_" + std::to_string(robot_id) + tag; + + LOG(PLOTJUGGLER) << *createPlotJugglerValue( + {{"pos_x" + robot_suffix, robot_state.position().x()}, + {"pos_y" + robot_suffix, robot_state.position().y()}, + {"vel_x" + robot_suffix, robot_state.velocity().x()}, + {"vel_y" + robot_suffix, robot_state.velocity().y()}}); } // TODO: Investigate proces models/variances/etc 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 44e4f52bee..3b2fa2c7cc 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -524,6 +524,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) { @@ -594,6 +596,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 93b0328a2614eb98f64637dc317019b441fd2e09 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 13 Sep 2026 14:06:50 -0700 Subject: [PATCH 10/14] 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 3b2fa2c7cc..626e5be8e9 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 251c02e657493b363673ba5b20b4ed5bf4f9930d Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Mon, 14 Sep 2026 19:55:37 -0700 Subject: [PATCH 11/14] 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 | 187 +----------------- src/software/simulation/er_force_simulator.h | 87 -------- 6 files changed, 27 insertions(+), 282 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 f9e089f15f..9861af0e29 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 626e5be8e9..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,13 +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, - robot_primitive_executor_map); - for (auto& [robot_id, primitive_executor] : robot_primitive_executor_map) { std::unique_ptr direct_control; @@ -515,102 +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::unordered_map>& - robot_primitive_executor_map) -{ - 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 drives predict() with the - // commanded velocity from PrimitiveExecutor::getPrevCommandedVelocity(). Using - // a ground-truth-derived velocity 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, so this reads the same commanded value real - // hardware would use. - Vector target_velocity; - auto primitive_executor_it = robot_primitive_executor_map.find(robot_id); - if (primitive_executor_it != robot_primitive_executor_map.end()) - { - target_velocity = primitive_executor_it->second->getPrevCommandedVelocity(); - } - localizer.predict(target_velocity, 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 3542309d17..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,74 +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 - * @param robot_primitive_executor_map Map of robot IDs to the robot's primitive - * executor, used to read the commanded velocity each localizer's predict step - * needs (see PrimitiveExecutor::getPrevCommandedVelocity) - */ - void updateRobotLocalizers( - std::unordered_map& localizer_map, - const std::map& robot_map, const Duration& time_step, - TeamColour team_colour, - const std::unordered_map>& - robot_primitive_executor_map); - - /** - * 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; @@ -316,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 43b38f1f871affc385a40c2e5f7cfb505f37a702 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Mon, 14 Sep 2026 20:28:24 -0700 Subject: [PATCH 12/14] q --- src/shared/constants.h | 2 +- src/shared/robot_constants.cpp | 2 +- src/software/embedded/BUILD | 2 -- src/software/embedded/robot_localizer.cpp | 38 ++--------------------- src/software/embedded/robot_localizer.h | 26 ---------------- src/software/embedded/thunderloop.cpp | 1 - 6 files changed, 5 insertions(+), 66 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 a8ee0cc12c..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", @@ -130,7 +129,6 @@ cc_library( "//software/geom:vector", "//software/physics:velocity_conversion_util", "//software/sensor_fusion/filter:extended_kalman_filter", - "//software/logger", "//software/sensor_fusion/filter:kalman_filter", "//software/world:robot_state", "@eigen", diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 9861af0e29..fb154b740a 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(); } @@ -227,33 +222,6 @@ RobotState RobotLocalizer::getRobotState() const getAngularVelocity()); } -void RobotLocalizer::logToPlotJuggler(RobotId robot_id, const std::string& tag) const -{ - const std::string robot_suffix = "_robot_" + std::to_string(robot_id) + tag; - - const Point position = getPosition(); - const Vector global_velocity = getGlobalVelocity(); - - LOG(PLOTJUGGLER) << *createPlotJugglerValue( - {{"pos_x" + robot_suffix, position.x()}, - {"pos_y" + robot_suffix, position.y()}, - {"vel_x" + robot_suffix, global_velocity.x()}, - {"vel_y" + robot_suffix, global_velocity.y()}}); -} - -void RobotLocalizer::logRobotStateToPlotJuggler(RobotId robot_id, - const RobotState& robot_state, - const std::string& tag) -{ - const std::string robot_suffix = "_robot_" + std::to_string(robot_id) + tag; - - LOG(PLOTJUGGLER) << *createPlotJugglerValue( - {{"pos_x" + robot_suffix, robot_state.position().x()}, - {"pos_y" + robot_suffix, robot_state.position().y()}, - {"vel_x" + robot_suffix, robot_state.velocity().x()}, - {"vel_y" + robot_suffix, robot_state.velocity().y()}}); -} - // 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 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 49349c8a8f01693352c17cacd13a5c1d7a532444 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Wed, 23 Sep 2026 21:20:13 -0700 Subject: [PATCH 13/14] cleanup --- src/software/embedded/BUILD | 1 - src/software/simulation/BUILD | 1 - 2 files changed, 2 deletions(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 6f236fc70d..416a1be908 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -46,7 +46,6 @@ cc_library( "//software/physics:velocity_conversion_util", "//software/time:duration", "//software/world:robot_state", - "//software/world:team_colour", "@tracy", ], ) diff --git a/src/software/simulation/BUILD b/src/software/simulation/BUILD index c0adfc8b8a..f69088c53a 100644 --- a/src/software/simulation/BUILD +++ b/src/software/simulation/BUILD @@ -25,7 +25,6 @@ cc_library( "//software/physics:velocity_conversion_util", "//software/world", "//software/world:field", - "//software/world:team_colour", ], ) From 508c76747fd360060130cd0185c96b0df80c17bc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Thu, 24 Sep 2026 04:25:47 +0000 Subject: [PATCH 14/14] [pre-commit.ci lite] apply automatic fixes --- src/shared/robot_constants.cpp | 3 +- src/software/embedded/robot_localizer.cpp | 39 ++++++++----------- .../embedded/robot_localizer_test.cpp | 8 ++-- 3 files changed, 22 insertions(+), 28 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 8da4d2293f..016bc9fb3d 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -232,12 +232,9 @@ void RobotLocalizer::updateFilterPredictionMatrices(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(); @@ -262,20 +259,19 @@ void RobotLocalizer::updateFilterPredictionMatrices(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(); @@ -351,18 +347,15 @@ void RobotLocalizer::updateFilterPredictionMatrices(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::updateFilterMeasurementModel(FilterStepType source) diff --git a/src/software/embedded/robot_localizer_test.cpp b/src/software/embedded/robot_localizer_test.cpp index 554d41c470..76c7961716 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()}); // predict() uses the commanded (target) velocity directly as the new velocity // estimate (see RobotLocalizer::updateFilterPredictionMatrices). @@ -71,8 +72,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"; EXPECT_NEAR(localizer.getOrientation().toDegrees(), 0.0, 10.0);