EKF
EKF Visualizer¶
The EKF visualizer is an extension of the car visualizer. In addition to the position (and orientation), this visualizer also allows for the visualization of the covariance of the state. The covariance will appear as a oval over the state estimate visualization. We recommend scaling the covariance by a factor cov_scale since the covariance values of an EKF can be very small and might not be visible otherwise. While the scaled covariance will not represent the true uncertainty of the estimate, the change along e.g. the track can still provide useful insights.
The color and opacity of the covariance ellipse can be set in the config file as well.
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.
Showing estimated position, orientation and estimated position covariance
Configuration¶
For Kalman estimators, besides the generic arguments the user can choose to use the EKF visualizer by adding the following to the config.
Example estimator_config.yaml
# Regular EKF or Square Root EKF settings
# ============ VISUALIZER ============
visualizer:
# car_ekf_visualizer will plot the covariance of the position estimate as a ellipsoid
type: car_ekf_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
size_x: 0.05 # 0.07 for arrows
size_y: 0.05 # 0.02 for arrows
size_z: 0.05 # 0.02 for arrows
# ==== 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. |
| 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. |
Subscribers and Publishers
| Subscribed Topics | Description |
|---|---|
| control_input | Control input |
| mocap | Motion capture measurement |
| tf | Transform between 'world_frame' and 'track_frame' |
| Published Topics | Description |
|---|---|
| estimation_node/best_state | EKF state estimate |
Parameters Descriptions
| Parameters | Description |
|---|---|
| /estimation_node/P_init | Initial state covariance matrix of the ekf |
| /estimation_node/initial_input/type | Control input type |
| /estimation_node/initial_input/value | Control input value |
| /estimation_node/initial_state/type | Initial state type |
| /estimation_node/initial_state/value | Initial state value |
| /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/type | Model used for ekf |
| /estimation_node/pub_rate | Publishing rate of the ekf |
| /estimation_node/sensors/imu/R/is_diag | If True, the IMU measurement noise covariance matrix is given as its diagonal entries |
| /estimation_node/sensors/imu/R/value | Value of the IMU measurement noise covariance matrix |
| /estimation_node/sensors/imu/key | Key that identifies the IMU sensor |
| /estimation_node/sensors/sensor_names | Array of names of all available sensors |
| /estimation_node/sensors/vicon/R/is_diag | If True, the Vicon measurement noise covariance matrix is given as its diagonal entries |
| /estimation_node/sensors/vicon/R/value | Value of the Vicon measurement noise covariance matrix |
| /estimation_node/sensors/vicon/key | Key that identifies the Vicon sensor |
| /estimation_node/track_frame | Frame of the track |
| /estimation_node/type | Type of estimator used e.g. discrete_ekf |
| /estimation_node/update_track_transform | If True, the transform is updated continously, meaning the track can be moved during an expermiment. |
| /estimation_node/world_frame | Frame of the Vicon system |
| /model/Q/is_diag | If True, the process noise covariance matrix of is given as its diagonal entries |
| /model/Q/value | Value of the process noise covariance matrix |
| /model/model_params/Bf | Front tire stiffness factor |
| /model/model_params/Br | Rear tire stiffness factor |
| /model/model_params/Cd | Drag coefficient |
| /model/model_params/Cf | Front tire shape factor that controls the ‘stretching’ in the x direction |
| /model/model_params/Cm1 | Model parameter |
| /model/model_params/Cm2 | Model parameter |
| /model/model_params/Cr | Rear tire shape factor that controls the ‘stretching’ in the x direction |
| /model/model_params/Croll | Roll coefficient |
| /model/model_params/Df | Peak value for front tire. |
| /model/model_params/Dr | Peak value for rear tire. |
| /model/model_params/I | Inertia |
| /model/model_params/lf | Length from center to front wheel |
| /model/model_params/lr | Length from center to rear wheel |
| /model/model_params/m | Mass of car |
| /model/type | Model (e.g Pacejak) |