Skip to content

autoware_speed_scale_corrector#

Overview#

This package performs vehicle speed scale correction by estimating and correcting speed scale factors. It compares velocities calculated from odometry with velocities reported by the vehicle's velocity sensors to correct discrepancies caused by sensor individual differences and calibration errors.

Algorithm#

The speed scale estimation follows these detailed steps:

1. Data Collection and Buffering#

  • Collects pose information (PoseWithCovarianceStamped), IMU data, and vehicle velocity reports (VelocityReport)
  • Maintains internal buffers for each data stream with timestamps
  • Extracts time series data: position (x, y) from poses, angular velocity (z) from IMU, and longitudinal velocity from velocity reports

2. Time Synchronization#

  • Finds the common time interval across all sensor data streams using interval intersection
  • Ensures sufficient time window (≥ time_window parameter) is available for reliable estimation
  • Early returns with previous scale factor if data is insufficient or time windows don't overlap

3. Data Preprocessing#

  • Applies Gaussian smoothing (σ=0.7) to all sensor data to reduce noise:
    • Position data (x, y coordinates)
    • IMU angular velocity
    • Vehicle velocity reports
  • Uses a sliding window Gaussian kernel with 3σ cutoff for efficient computation

4. Trajectory Interpolation#

  • Creates cubic spline interpolators for smoothed position data (x, y)
  • Creates linear interpolators for angular velocity and velocity data
  • Generates uniform time samples within the valid time window at specified intervals

5. State Vector Creation#

For each time sample, computes:

  • Odometry velocity: Magnitude of position derivatives

    \[ v_{odom} = \sqrt{\left(\frac{dx}{dt}\right)^2 + \left(\frac{dy}{dt}\right)^2} \]
  • Distance between points: Euclidean distance from previous position
  • Integrated distance from velocity: Trapezoidal integration of velocity reports
  • Angular velocity: Interpolated IMU z-axis rotation

6. Constraint Validation#

Validates that all states satisfy operational constraints to ensure reliable estimation:

  • Angular velocity constraint:

    \[ |\omega| \leq \omega_{max} \]

    (avoids estimation during sharp turns)

  • Speed constraints:

    \[ v_{min} \leq v \leq v_{max} \]

    (ensures sufficient motion for accurate differentiation and avoids extreme speeds)

  • Speed change constraint:

    \[ |\Delta v| \leq \Delta v_{max} \]

    (filters out periods of rapid acceleration/deceleration where tire slip may occur)

7. Scale Factor Estimation#

  • Computes instantaneous scale factor:

    \[ s = \frac{d_{odom}}{d_{velocity}} \]

  • Updates running average:

    \[ \bar{s}_{new} = \frac{\bar{s}_{old} \times n + s_{current}}{n + 1} \]
  • Clears sensor buffers after successful estimation

Inputs / Outputs#

Input Topics#

Name Type Description
~/input/pose_with_covariance geometry_msgs::msg::PoseWithCovarianceStamped Pose information (odometry)
~/input/velocity_report autoware_vehicle_msgs::msg::VelocityReport Vehicle velocity report
~/input/imu sensor_msgs::msg::Imu IMU sensor data

Output Topics#

Name Type Description
~/output/scale_factor autoware_internal_debug_msgs::msg::Float32Stamped Estimated speed scale factor
~/output/debug_info autoware_internal_debug_msgs::msg::StringStamped Debug information

Parameters#

Name Type Description Default Range
time_window float Time window for data collection and analysis in seconds. Must be sufficient for reliable estimation. Unit is [s]. 4 ≥0.0
time_interval float Time interval for uniform sampling within the time window in seconds. Unit is [s]. 0.1 ≥0.0
initial_speed_scale_factor float Initial speed scale factor value used before estimation begins. Typically set to 1.0 for no initial correction. 1 ≥0.0
max_angular_velocity float Maximum allowed angular velocity in rad/s. States exceeding this value are filtered out to avoid estimation during sharp turns. Unit is [rad/s]. 1 ≥0.0
max_speed float Maximum allowed vehicle speed in m/s. States exceeding this value are filtered out to avoid extreme speeds that may cause inaccurate estimation. Unit is [m/s]. 15 ≥0.0
min_speed float Minimum required vehicle speed in m/s. States below this value are filtered out to ensure sufficient motion for accurate differentiation. Unit is [m/s]. 2 ≥0.0
max_speed_change float Maximum allowed speed change in m/s^2. States exceeding this value are filtered out to avoid periods of rapid acceleration/deceleration where tire slip may occur. Unit is [m/s^2]. 1 ≥0.0