Pass time elapsed to robot localizer

Author: williamckha

src/software/embedded/primitive_executor.cpp

@@ -51,11 +51,11 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m
 
         trajectory_path_ = new_trajectory_path;
         position_controller_.reset();
-        time_since_linear_trajectory_creation_s_ = 0.5;
+        time_since_linear_trajectory_creation_s_ = VISION_TO_ROBOT_DELAY_S;
 
         angular_trajectory_ = new_angular_trajectory;
         orientation_controller_.reset();
-        time_since_angular_trajectory_creation_s_ = 0.5;
+        time_since_angular_trajectory_creation_s_ = VISION_TO_ROBOT_DELAY_S;
     }
 
     const auto update_end = std::chrono::steady_clock::now();
@@ -136,7 +136,7 @@ TbotsProto::DirectControlPrimitive PrimitiveExecutor::stepPrimitive(
             createAngularVelocity(robot_status.imu_status().angular_velocity())});
     }
 
-    robot_localizer_.step(Vector());
+    robot_localizer_.step(Vector(), delta_time_s);
 
     TbotsProto::PrimitiveExecutorStatus& prim_exec_status =
         *(robot_status.mutable_primitive_executor_status());

src/software/embedded/robot_localizer.cpp

@@ -1,7 +1,5 @@
 #include "robot_localizer.h"
 
-#include <chrono>
-
 RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config)
     : process_linear_acceleration_noise_variance_(config.process_noise_variance),
       process_angular_acceleration_noise_variance_(config.process_noise_variance)
@@ -16,24 +14,21 @@ RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config)
             config.motor_sensor_noise_variance, config.motor_sensor_noise_variance,
             ImuService::IMU_VARIANCE)
             .asDiagonal();
-
-    last_step_time_ = std::chrono::steady_clock::now();
 }
 
-void RobotLocalizer::step(const Vector& linear_acceleration)
+void RobotLocalizer::step(const Vector& linear_acceleration,
+                          const double delta_time_seconds)
 {
+    current_time_seconds_ += delta_time_seconds;
+
     FilterStep step{
         .prediction       = std::make_optional<FilterStep::Predict>(),
         .update           = std::nullopt,
         .state_estimate   = filter_.state_estimate,
         .state_covariance = filter_.state_covariance,
-        .time             = std::chrono::steady_clock::now(),
+        .time_seconds     = current_time_seconds_,
     };
 
-    const std::chrono::duration<double> delta_time = step.time - last_step_time_;
-    const double delta_time_seconds                = delta_time.count();
-    last_step_time_                                = step.time;
-
     // clang-format off
     step.prediction->process_model <<
         1, 0, 0, delta_time_seconds, 0, 0,
@@ -136,12 +131,10 @@ void RobotLocalizer::update(const VisionData& data)
         return;
     }
 
-    const auto current_time = std::chrono::steady_clock::now();
-    const auto sample_age   = std::chrono::duration<double>(data.age_seconds);
-
     auto rollback_point = std::find_if(
         history.begin(), history.end(),
-        [&](const FilterStep& step) { return (current_time - step.time) >= sample_age; });
+        [&](const FilterStep& step)
+        { return (current_time_seconds_ - step.time_seconds) >= data.age_seconds; });
 
     if (rollback_point == history.begin())
     {
@@ -231,29 +224,7 @@ void RobotLocalizer::updateFilterWithVision(const Point& position,
         static_cast<Eigen::Index>(MeasurementIndex::VISION_ORIENTATION),
         static_cast<Eigen::Index>(StateIndex::ORIENTATION)) = 1;
 
-    // Vision observes pose (position + orientation), not velocity. The velocity states
-    // are observed directly by the wheel encoders (and IMU for angular velocity), which
-    // are accurate. Letting the vision position/orientation correction also adjust the
-    // velocity states through the position<->velocity covariance coupling is unstable
-    // here: the tight vision noise leaves the position correction lagging, and that
-    // persistent lag is fed into the velocity estimate every frame, inflating it (a
-    // robot moving 1 m/s was estimated at ~1.8 m/s, and worse without delay
-    // compensation). So we hold the velocity states fixed across the vision update.
-    const double x_velocity =
-        filter_.state_estimate(static_cast<Eigen::Index>(StateIndex::X_VELOCITY));
-    const double y_velocity =
-        filter_.state_estimate(static_cast<Eigen::Index>(StateIndex::Y_VELOCITY));
-    const double angular_velocity =
-        filter_.state_estimate(static_cast<Eigen::Index>(StateIndex::ANGULAR_VELOCITY));
-
     filter_.update(measurement);
-
-    filter_.state_estimate(static_cast<Eigen::Index>(StateIndex::X_VELOCITY)) =
-        x_velocity;
-    filter_.state_estimate(static_cast<Eigen::Index>(StateIndex::Y_VELOCITY)) =
-        y_velocity;
-    filter_.state_estimate(static_cast<Eigen::Index>(StateIndex::ANGULAR_VELOCITY)) =
-        angular_velocity;
 }
 
 void RobotLocalizer::update(const MotorData& data)
@@ -286,7 +257,7 @@ void RobotLocalizer::update(const MotorData& data)
         .update           = update,
         .state_estimate   = filter_.state_estimate,
         .state_covariance = filter_.state_covariance,
-        .time             = std::chrono::steady_clock::now(),
+        .time_seconds     = current_time_seconds_,
     };
 
     history.push_front(step);
@@ -313,7 +284,7 @@ void RobotLocalizer::update(const ImuData& data)
         .update           = update,
         .state_estimate   = filter_.state_estimate,
         .state_covariance = filter_.state_covariance,
-        .time             = std::chrono::steady_clock::now(),
+        .time_seconds     = current_time_seconds_,
     };
 
     history.push_front(step);

src/software/embedded/robot_localizer.h

@@ -68,11 +68,12 @@ class RobotLocalizer
     explicit RobotLocalizer(const RobotLocalizerConfig& config);
 
     /**
-     * Runs one prediction step using elapsed time since the previous call.
+     * Runs one prediction step over the given elapsed time.
      *
      * @param linear_acceleration The current linear acceleration of the robot
+     * @param delta_time_seconds The elapsed time since the previous step, in seconds
      */
-    void step(const Vector& linear_acceleration);
+    void step(const Vector& linear_acceleration, double delta_time_seconds);
 
     /**
      * Update the robot's position and orientation from data reported by vision.
@@ -162,7 +163,7 @@ class RobotLocalizer
         Eigen::Vector<double, STATE_SIZE> state_estimate;
         Eigen::Matrix<double, STATE_SIZE, STATE_SIZE> state_covariance;
 
-        std::chrono::time_point<std::chrono::steady_clock> time;
+        double time_seconds;
     };
 
     KalmanFilter<STATE_SIZE, MEASUREMENT_SIZE, CONTROL_SIZE> filter_;
@@ -171,8 +172,8 @@ class RobotLocalizer
     double process_linear_acceleration_noise_variance_;
     double process_angular_acceleration_noise_variance_;
 
-    std::chrono::time_point<std::chrono::steady_clock> last_step_time_;
-
     // History is ordered newest-first (front is the most recent step)
     std::deque<FilterStep> history;
+
+    double current_time_seconds_ = 0.0;
 };

src/software/embedded/robot_localizer_test.cpp

@@ -47,7 +47,7 @@ RobotLocalizer runConstantVelocity(bool feed_vision, double vision_age = RTT_S /
             localToGlobalVelocity(local_velocity, localizer.getOrientation()),
             AngularVelocity::zero()});
 
-        localizer.step(Vector(0.0, 0.0));
+        localizer.step(Vector(0.0, 0.0), DT);
 
         // Periodic vision fix (~60 Hz). Feed the position from RTT_S/2 ago, consistent
         // with the reported age.

src/software/simulation/er_force_simulator.cpp

@@ -478,13 +478,13 @@ void ErForceSimulator::stepSimulation(const Duration& time_step)
 {
     current_time = current_time + time_step;
 
-    SSLSimulationProto::RobotControl yellow_robot_control = updateSimulatorRobots(
-        yellow_primitive_executor_map, *yellow_team_world_msg, time_step,
-        gameController::Team::YELLOW);
+    SSLSimulationProto::RobotControl yellow_robot_control =
+        updateSimulatorRobots(yellow_primitive_executor_map, *yellow_team_world_msg,
+                              time_step, gameController::Team::YELLOW);
 
-    SSLSimulationProto::RobotControl blue_robot_control = updateSimulatorRobots(
-        blue_primitive_executor_map, *blue_team_world_msg, time_step,
-        gameController::Team::BLUE);
+    SSLSimulationProto::RobotControl blue_robot_control =
+        updateSimulatorRobots(blue_primitive_executor_map, *blue_team_world_msg,
+                              time_step, gameController::Team::BLUE);
 
     auto yellow_radio_responses =
         er_force_sim->acceptYellowRobotControlCommand(yellow_robot_control);

src/software/simulation/er_force_simulator.h

@@ -28,10 +28,10 @@ class ErForceSimulator
      * @param robot_constants The robot constants
      * @param realism_config realism configuration
      */
-    explicit ErForceSimulator(
-        const TbotsProto::FieldType& field_type,
-        const robot_constants::RobotConstants& robot_constants,
-        std::unique_ptr<RealismConfigErForce>& realism_config, const bool ramping = true);
+    explicit ErForceSimulator(const TbotsProto::FieldType& field_type,
+                              const robot_constants::RobotConstants& robot_constants,
+                              std::unique_ptr<RealismConfigErForce>& realism_config,
+                              const bool ramping = true);
     ErForceSimulator()  = delete;
     ~ErForceSimulator() = default;