MHE
MHE Visualizer¶
The MHE visualizer will visualize the reference trajectory as well as the estimated trajectory. Note that in the case of an MHE, the trajectory refers to a trajectory of estimated states. The current state estimate, reference and estimated trajectories can be shown as either a point or arrow. If a Kalman filter is being used as an internal estimator, the covariance of the current state estimate can be visualized by also setting the needed EKF visualization parameters (see example config below).
Note: The position estimate is displayed by the parent class CarEstimatorVisualizer. If requested, the state estimate is rendered as an arrow which also highlights the yaw angle.
Configuration¶
The reference and estimated state estimates can be visualized using points or arrows (use_arrows). The color of the reference and estimated state estimates can set in the config as well. By default, the estimated state estimates are color coordinated based on the estimated velocity of the state. The velocity values for such a color mapping can be set under double_min_velocityand double_max_velocity.
Example estimator_config.yaml
# Regular MHE settings
# ============ VISUALIZER ============
visualizer:
# car_mhe_visualizer will plot the reference and estimated state estimates.
type: car_mhe_visualizer
# === Parameters for Base Visualizer ===
rate: 10
# Visualizer specific parameters
frame_id: crs_frame # default
namespace: ekf # default
use_arrows: true # If true, use arrows to visualize planned and reference yaw angle. This option is a lot slower and may introduce visual lags
# Color of the estimated position
est_r: 0
est_g: 0
est_b: 0
est_a: 1
# ==== Special Parameters for mhe visualizer ====
planned:
size_x: 0.04 # 0.07 for arrows
size_y: 0.04 # 0.02 for arrows
size_z: 0.04 # 0.02 for arrows
reference:
r: 1
g: 0
b: 0
a: 1
size_x: 0.04 # 0.07 for arrows
size_y: 0.04 # 0.02 for arrows
size_z: 0.04 # 0.02 for arrows
double_min_velocity: 1.5 # Min velocity for visualization (this or below gets mapped to blue)
double_max_velocity: 2.5 # Max velocity for visualization (this or above gets mapped to red)
# ==== Special Parameters for ekf visualizer ====
# Color of the covariance ellipsoid
cov_r: 0
cov_g: 0
cov_b: 1
cov_a: 0.3
cov_scale: 100 # scales covariance ellipsoid by 100
Ros Information¶
Launch File
| Node Names | Description |
|---|---|
| estimator_node | Runs the state estimator |
Launch File Arguments
| Argument | Default | Description |
|---|---|---|
| estimator_config | $(find ros_estimators)/config/pacejka_car_ekf.yaml | Config to load for the EKF. |
| input_topic | control_input | Topic where the control input will be published. |
| vicon_topic | vicon | Topic where the vicom measurement will be published. |
| imu_topic | imu | Topic where the imu measurement will be published. |
| imu_yaw_rate_topic | imu | Topic where the yaw rate of the imu measurement will be published. |
| wheel_encoder_topic | wheel_encoders | Topic where the wheel encoders measurement will be published. |
| lighthouse_topic | lighthouse | Topic where the lighthouse measurement will be published. |
| state_est_topic | estimation_node/best_state | Topic where the state estimate is published. |
| world_frame | world | Name of common frame for all objects (e.g. base for vicon measurements). |
| track_frame | world | Name of the frame where in which the car track origin is published. |
| update_track_transform | false | If true, continuously update the transformation between 'world_frame' and 'track_frame'. This allows to e.g. move the track during an experiment. If false, only use the first published transform for the whole experiment. |
| estimator_name | estimation_node | |
| frame_name | frame_name |
Subscribers and Publishers
| Subscribed Topics | Description |
|---|---|
| control_input | Control input |
| vicon | Vicon measurement |
| imu | Imu measurement |
| imu_yaw_rate | Yaw rate of imu measurement |
| wheel_encoders | Wheel encoder measurement |
| lighthouse | Lighthouse measurement |
| tf | Transform between 'world_frame' and 'track_frame' |
| Published Topics | Description |
|---|---|
| estimation_node/best_state | MHE state estimate |
| reference_state_trajectory | MHE reference trajectory |
| estimated_state_trajectory | MHE estimated trajectory |
| estimation_node/visualizer/state_estimate_visualizer | MHE state estimate |
Parameter Descriptions
| Parameters | Description |
|---|---|
| /estimation_node/model/P/is_diag | If True, the state covariance matrix is given as its diagonal entries |
| /estimation_node/model/P/value | Value of the state covariance matrix |
| /estimation_node/model/Q/is_diag | If True, the process noise covariance matrix is given as its diagonal entries |
| /estimation_node/model/Q/value | Value of the process noise covariance matrix |
| /estimation_node/model/R_vicon/is_diag | If True, the measurement noise covariance matrix is given as its diagonal entries |
| /estimation_node/model/R_vicon/value | Value of the measurement noise covariance matrix |
| /estimation_node/model/R_imu/is_diag | If True, the measurement noise covariance matrix is given as its diagonal entries |
| /estimation_node/model/R_imu/value | Value of the measurement noise covariance matrix |
| /estimation_node/model/R_imu_yaw_rate/is_diag | If True, the measurement noise covariance matrix is given as its diagonal entries |
| /estimation_node/model/R_imu_yaw_rate/value | Value of the measurement noise covariance matrix |
| /estimation_node/model/R_wheel_encoders/is_diag | If True, the measurement noise covariance matrix is given as its diagonal entries |
| /estimation_node/model/R_wheel_encoders/value | Value of the measurement noise covariance matrix |
| /estimation_node/model/R_lighthouse/is_diag | If True, the measurement noise covariance matrix is given as its diagonal entries |
| /estimation_node/model/R_lighthouse/value | Value of the measurement noise covariance matrix |
| /estimation_node/car_frame_name | ekf_estimate |
| /estimation_node/eta | Discount factor |
| /estimation_node/initial_input/type | Type of initial input |
| /estimation_node/initial_input/value | Value of initial input |
| /estimation_node/initial_state/type | Type of initial state |
| /estimation_node/initial_state/value | Type of initial sate |
| /estimation_node/internal_estimator/P_init | Value of initial state covariance matrix |
| /estimation_node/internal_estimator/initial_input/type | Type of initial input for internal estimator |
| /estimation_node/internal_estimator/initial_input/value | Value of initial input for internal estimator |
| /estimation_node/internal_estimator/initial_state/type | Type of initial state for internal estimator |
| /estimation_node/internal_estimator/initial_state/value | Value of initial state for internal estimator |
| /estimation_node/internal_estimator/type | Type of internal estimator |
| /estimation_node/internal_filter_type | Type of internal filter |
| /estimation_node/lag_compensation_time | Lag compensation time |
| /estimation_node/log_diagnostic_data | If True, will log diagnostic data, such as thresholds |
| /estimation_node/max_buffer_size | Buffer size of MHE |
| /estimation_node/measurement_timeout_threshold | |
| /estimation_node/model/type | Model Type |
| /estimation_node/pub_rate | Publishing rate of estimator |
| /estimation_node/sensors/imu_yaw_rate/MHE_outlier_rejection/outlier_threshold | Threshold for outlier rejection in MHE of imu yaw rate |
| /estimation_node/sensors/imu_yaw_rate/MHE_outlier_rejection/use_outlier_rejection | If True, MHE will reject outlier measruements |
| /estimation_node/sensors/imu_yaw_rate/R/is_diag | If True, Measurement noise covariance matrix used in internal estimator is given as a diagonal matrix |
| /estimation_node/sensors/imu_yaw_rate/R/value | Value of measurement noise covariance matrix used in internal estimator |
| /estimation_node/sensors/imu_yaw_rate/key | Sensor key |
| /estimation_node/sensors/imu_yaw_rate/outlier_rejection/max_consecutive_outliers | Maximum number of consecutive measurements that can be rejected before needing to accept a measurement in internal estimator |
| /estimation_node/sensors/imu_yaw_rate/outlier_rejection/outlier_rejection_type | Type of outlier rejection in internal estimator |
| /estimation_node/sensors/imu_yaw_rate/outlier_rejection/outlier_threshold | Outlier rejection threshold for internal estimator |
| /estimation_node/sensors/imu_yaw_rate/outlier_rejection/use_outlier_rejection | If True, the internal estimator will reject outliers |
| /estimation_node/sensors/lighthouse/MHE_outlier_rejection/outlier_threshold | Threshold for outlier rejection in MHE of lighthouse |
| /estimation_node/sensors/lighthouse/MHE_outlier_rejection/use_outlier_rejection | If True, the MHE will reject outlier measurements |
| /estimation_node/sensors/lighthouse/base_stations | List of base stations used |
| /estimation_node/sensors/lighthouse/bs0/P_bs/value | Value of base station position |
| /estimation_node/sensors/lighthouse/bs0/R/value | Value of measurement covariance matrix |
| /estimation_node/sensors/lighthouse/bs0/R_bs/value | Value of base station rotation |
| /estimation_node/sensors/lighthouse/bs0/bs_ID | Base station ID |
| /estimation_node/sensors/lighthouse/bs0/dt1 | First light plane tilt |
| /estimation_node/sensors/lighthouse/bs0/dt2 | Second light plane tilt |
| /estimation_node/sensors/lighthouse/key | Sensor key |
| /estimation_node/sensors/lighthouse/outlier_rejection/max_consecutive_outliers | Maximum number of consecutive measurements that can be rejected before needing to accept a measurement in internal estimator |
| /estimation_node/sensors/lighthouse/outlier_rejection/outlier_rejection_type | Outlier rejection type for internal estimator |
| /estimation_node/sensors/lighthouse/outlier_rejection/outlier_threshold | Outlier rejection threshold for internal estimator |
| /estimation_node/sensors/lighthouse/outlier_rejection/use_outlier_rejection | If True, the internal estimator will reject outliers |
| /estimation_node/sensors/lighthouse/sensor_pos/value | Lighthouse sensor position |
| /estimation_node/sensors/sensor_names | List of used sensors |
| /estimation_node/sensors/vicon/MHE_outlier_rejection/outlier_threshold | Outlier rejection threshold for MHE |
| /estimation_node/sensors/vicon/MHE_outlier_rejection/use_outlier_rejection | If True, the MHE will reject outliers |
| /estimation_node/sensors/vicon/R/is_diag | If True, the measurement noise covariance matrix is given as its diagonal entries |
| /estimation_node/sensors/vicon/R/value | Value of measurement covariance matrix |
| /estimation_node/sensors/vicon/key | Sensor key |
| /estimation_node/sensors/vicon/outlier_rejection/max_consecutive_outliers | Maximum number of consecutive measurements that can be rejected before needing to accept a measurement in internal estimator |
| /estimation_node/sensors/vicon/outlier_rejection/outlier_rejection_type | Outlier rejection type for internal estimator |
| /estimation_node/sensors/vicon/outlier_rejection/outlier_threshold | Outlier rejection threshold for internal estimator |
| /estimation_node/sensors/vicon/outlier_rejection/use_outlier_rejection | If True, the internal estimator will reject outliers |
| /estimation_node/sensors/wheel_encoders/MHE_outlier_rejection/outlier_threshold | Outlier rejection threshold for MHE |
| /estimation_node/sensors/wheel_encoders/MHE_outlier_rejection/use_outlier_rejection | If True, the MHE will reject outliers |
| /estimation_node/sensors/wheel_encoders/R/is_diag | If True, the measurement noise covariance matrix is given as its diagonal entries |
| /estimation_node/sensors/wheel_encoders/R/value | Value of measurement covariance matrix |
| /estimation_node/sensors/wheel_encoders/key | Sensor key |
| /estimation_node/sensors/wheel_encoders/outlier_rejection/max_consecutive_outliers | Maximum number of consecutive measurements that can be rejected before needing to accept a measurement in internal estimator |
| /estimation_node/sensors/wheel_encoders/outlier_rejection/outlier_rejection_type | Outlier rejection type for internal estimator |
| /estimation_node/sensors/wheel_encoders/outlier_rejection/outlier_threshold | Outlier rejection threshold for internal estimator |
| /estimation_node/sensors/wheel_encoders/outlier_rejection/use_outlier_rejection | If True, the internal estimator will reject outliers |
| /estimation_node/solver_type | Solver type for MHE |
| /estimation_node/start_delay | Time in seconds, during which only the internal estimator is used until the MHE is turned on |
| /estimation_node/track_frame | Track frame |
| /estimation_node/type | Estimator Type |
| /estimation_node/update_track_transform | |
| /estimation_node/use_internal_estimator | If True, the internal estimator estimate is used in the cost of the MHE |
| /estimation_node/use_internal_filter | If True, the MHE filters the measurements |
| /estimation_node/warmstart_iterations | Number of warm start iterations for the MHE |
| estimation_node/world_frame | World frame |