Steer Offset Estimator#
Purpose#
The role of this node is to automatically estimate the steer_offset used in the lateral controller. The value estimated is the residual bias (on top of the static mechanical bias specified in vehicle calibration files). It also allow for auto or manual calibration of total system bias in real time.
Inner-workings / Algorithms#
This module estimates the steering offset using a Kalman Filter algorithm based on vehicle kinematic model constraints.
Kinematic Model#
The vehicle kinematic model relates steering angle to angular velocity:
Where:
- \(\omega\): Angular velocity (yaw rate) [rad/s]
- \(v\): Vehicle velocity [m/s]
- \(L\): Wheelbase [m]
- \(\delta\): Steering angle [rad]
Problem Formulation#
Due to mechanical tolerances and sensor calibration errors, there exists a steering offset \(\delta_{offset}\). The true relationship becomes:
The algorithm estimates \(\delta_{offset}\) by minimizing the error between observed and predicted angular velocity.
Kalman Filter Algorithm#
The Kalman Filter algorithm updates the offset estimate and covariance recursively with time and measurement updates:
-
Regressor and measurement formulation:
\[ \phi = \frac{v}{L} \]\[ y = \omega_{observed} - \phi \times \delta_{measured} \]
-
Time update (process model):
\[ P_{prior} = P_{k-1} + Q \]
-
Measurement update denominator:
\[ denom = R + \phi^2 \times P_{prior} \]
-
Kalman gain calculation:
\[ K = \frac{P_{prior} \times \phi}{denom} \]
-
Innovation (residual) and state update:
\[ residual = y - \phi \times \delta_{offset,prev} \]\[ \delta_{offset,new} = \delta_{offset,prev} + K \times residual \]
-
Covariance update:
\[ P_k = P_{prior} - \frac{P_{prior} \times \phi^2 \times P_{prior}}{denom} \]
Where:
- \(P\): Estimation covariance matrix (scalar in this 1D case)
- \(Q\): Process noise covariance (allows parameter drift)
- \(R\): Measurement noise covariance
- \(K\): Kalman gain
- \(k\): Current time step
Algorithm Constraints#
The algorithm only updates when:
- Both pose and steering data are available
- Vehicle velocity >
min_velocity(ensures reliable kinematic model) - \(|\delta_{\text{measured}}|\) <
max_steer(avoids nonlinear tire behavior) - \(|\dot{\delta}_{\text{measured}}|\) <
max_steer_rate(avoids "zero-crossing" errors during transient maneuvers) - angular velocity <
max_ang_velocity(avoids tire slip and dynamic noise)
This Kalman Filter approach provides continuous, real-time calibration of steering offset during normal driving operations, with process noise allowing adaptation to changing conditions and measurement noise handling sensor uncertainties.
Steering Offset Update#
Once the estimator converges (i.e covariance < covariance_th), if the difference between the new offset estimate and last published estimate is greater than update_offset_th, then the new offset estimate is published to allow the controller to get the latest value to improve the control performance.
System Calibration#
In addition to publishing the offset update for control purposes, the node allows the user to apply the estimated offset to the vehicle system through three distinct modes and a set of safety validation gates.
Calibration Modes#
- OFF: No system calibration, the estimator only provides offset updates for control purposes.
- MANUAL: Calibration is triggered only via a dedicated ROS 2 service call (
~/trigger_steer_offset_calibration). - AUTO: The system automatically triggers a calibration update when the internal stability and confidence conditions are met.
Calibration Execution Logic#
A calibration is executed when a service call is received, or in case of AUTO mode update conditions are satisfied.
When calibration is triggered the node will update the persistent YAML file (if parameter is enabled) and publish the offset update value on the topic ~/steering_offset_update.
Calibration is executed only if specific validation checks are satisfied.
1. Manual Mode Checks#
When a service call is received, the following checks are performed:
- Confidence: The estimation covariance must be below
covariance_threshold. - Safety: The total offset must not exceed
max_offset_limit.
2. Auto Mode Checks#
In addition to the Manual gates, Auto mode applies more restrictive temporal and significance filters:
- Steady State: Valid estimation results must be continuous for at least
min_steady_duration. - Update Interval: Time since the last update must exceed
min_update_interval. - Significance: The difference between the current estimate and the registered offset must exceed
error_threshold.
Interfaces#
Inputs / Outputs#
Input#
| Name | Type | Description |
|---|---|---|
~/input/pose |
geometry_msgs::msg::PoseStamped |
vehicle pose |
~/input/steer |
autoware_vehicle_msgs::msg::SteeringReport |
steering |
Output#
| Name | Type | Description |
|---|---|---|
~/output/steering_offset |
autoware_internal_debug_msgs::msg::Float32Stamped |
steering offset |
~/output/steering_offset_covariance |
autoware_internal_debug_msgs::msg::Float32Stamped |
covariance of steering offset |
~/output/steering_offset_update |
autoware_internal_debug_msgs::msg::Float32Stamped |
updated steering offset value |
~/output/debug_info |
autoware_internal_debug_msgs::msg::StringStamped |
debug info |
Services#
| Name | Type | Description |
|---|---|---|
~/trigger_steer_offset_calibration |
std_srvs::srv::Trigger |
trigger steering offset calibration |
Parameters#
| Name | Type | Description | Default | Range |
|---|---|---|---|---|
| initial_covariance | float | steer offset is larger than tolerance | 1000.0 | ≥0.0 |
| update_hz | float | update hz of steer data | 10.0 | ≥0.0 |
| initial_offset | float | initial steer offset value | 0.0 | N/A |
| process_noise_covariance | float | process noise covariance for Kalman filter | 5e-8 | ≥0.0 |
| measurement_noise_covariance | float | measurement noise covariance for Kalman filter | 1.0 | ≥0.0 |
| denominator_floor | float | minimum value for denominator to avoid numerical issues | 1.0e-12 | ≥0.0 |
| covariance_floor | float | minimum value for covariance to avoid numerical issues | 1.0e-12 | ≥0.0 |
| min_velocity | float | velocity below this value is not used | 1.0 | ≥0.0 |
| max_steer | float | offset estimation is skipped if steering is above this value | 0.02 | ≥0.0 |
| max_steer_rate | float | offset estimation is skipped if steering rate is above this value | 0.01 | ≥0.0 |
| max_ang_velocity | float | offset estimation is skipped if angular velocity is above this value | 0.02 | ≥0.0 |
| max_steer_buffer | float | duration to keep steering values in the buffer | 1.0 | ≥0.0 |
| max_pose_lag | float | max valid age of previous pose which can be used for twist calculation | 0.5 | ≥0.0 |
| calibration.mode | string | offset calibration mode | off | N/A |
| calibration.update_offset_th | float | error threshold for offset calibration | 0.001 | ≥0.0 |
| calibration.covariance_th | float | covariance threshold for offset calibration | 0.0015 | ≥0.0 |
| calibration.min_steady_duration | float | min duration of continuous estimation data for offset calibration | 10.0 | ≥0.0 |
| calibration.max_offset_limit | float | max offset limit for offset calibration | 0.05 | ≥0.0 |
| calibration.min_update_interval | float | min interval between offset calibration updates | 100.0 | ≥0.0 |
| calibration.warning_offset_th | float | offset threshold to trigger a warning | 0.005 | ≥0.0 |