Configuration¶
The following sections highlight the structure of all .yaml configuration files used in this framework.
Elements marked with [, ] are optional.
Matrix¶
Noise Model¶
Examples
Multivariate Gaussian Zero Noise Model¶
Sensor Model¶
Examples
Vicon Measurement Model¶
Dynamic Model¶
type: <pacejka_discrete|kinematic_discrete|rocket_6_dof_discrete>
Q: <Matrix> # 6x6 for pacejka, 4x4 for kinematic, 16x16 for rocket_6_dof
model_params:
<Pacejka Parameters | Kinematic Parameters | Rocket 6-DoF Parameters>
Examples
Pacejka Example¶
type: "pacejka_discrete"
# Q (process noise covariance matrix)
Q:
value:
[
[0.001, 0, 0, 0, 0, 0],
[0, 0.001, 0, 0, 0, 0],
[0, 0, 0.001, 0, 0, 0],
[0, 0, 0, 0.0001, 0, 0],
[0, 0, 0, 0, 0.0001, 0],
[0, 0, 0, 0, 0, 0.0001],
]
is_diag: false
model_params:
# size params
lr: 0.038
lf: 0.052
m: 0.181
I: 0.000505
# lateral force params
Df: 0.65
Cf: 1.5
Bf: 5.2
Dr: 1.0
Cr: 1.45
Br: 8.5
# longitudinal force params
Cm1: 0.98028992
Cm2: 0.01814131
Cd: 0.02750696
Croll: 0.08518052
tau: 0.6 # to make this compatible with kinematic model, not required
Kinematic Example¶
type: "kinematic_model"
# Q (process noise covariance matrix)
Q:
value:
[
[0.01, 0, 0, 0],
[0, 0.01, 0, 0],
[0, 0, 0.01, 0],
[0, 0, 0, 0.0001],
]
is_diag: false
model_params:
# size params
lr: 0.038
lf: 0.052
tau: 0.6
Rocket 6-DoF Example¶
type: "rocket_6_dof_discrete"
# Q (process noise covariance matrix)
Q:
value:
[
[4.2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 3.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 3.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1.8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 20, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 20, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 20, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 20, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2.7, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2.7, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1],
]
is_diag: false
model_params:
mass: 1.16
gravity_constant: 9.81
inertia_xx: 0.00210
inertia_yy: 0.10000
inertia_zz: 0.10000
thrust_cog_offset: 0.42000
thrust_dynamics_time_constant: 0.05000
Pacejka Parameters¶
lr: <double>
lf: <double>
m: <double>
I: <double>
# lateral force params
Df: <double>
Cf: <double>
Bf: <double>
Dr: <double>
Cr: <double>
Br: <double>
# longitudinal force params
Cm1: <double>
Cm2: <double>
Cd: <double>
Croll: <double>
Kinematic Parameters¶
Rocket 6-DoF Parameters¶
mass: <double>
gravity_constant: <double> # 9.81 ms^{-2} unless were flying on the moon or sth
inertia_xx: <double>
inertia_yy: <double>
inertia_zz: <double>
thrust_cog_offset: <double>
thrust_magnitude_time_constant: <double>
servo_angle_time_constant: <double>
# the rod going to the gimbal [mm]
gimbal_a: <double>
# Length of the rod which connects the servo to the gimbal [mm]
gimbal_b: <double>
# Straight line distance between the joint connecting the servo-gimbal
# rod to the gimbal and the center of the universal joint [mm]
gimbal_c: <double>
# Distance between the universal joint and the rotation axis of the servo in the body yz-plane [mm]
gimbal_d: <double>
# Distance along the body x-axis between the universal joint and the servo rotation axis [mm]
gimbal_e: <double>
Simulator¶
state_type: <pacejka_car|kinematic_car>
input_type: <pacejka_car|kinematic_car>
[process_noise]:
<Noise Model>
sensors:
sensor_names: List[<vicon|imu>] # <-- Must match provided sensor models
<sensor model>
[<sensor model>]
[...]
initial_state: List[double] # 6 entries for pacejka, 4 for kinematic
initial_input: [double, double] # [steer_input, torque_input]
frequency: double # Hz, speed of simulator
input_delay: <double> # input delay in seconds
do_collision_checks: <true|false> # check if car drives out of bounds, if yes slows down velocity
Examples
Kinematic Simulator¶
state_type: kinematic_car
input_type: kinematic_car
process_noise:
type: multivariate_gaussian
seed: 42
mean:
value: [[0], [0], [0], [0], [0], [0]]
sensors:
sensor_names: ["vicon"]
vicon:
R:
value: [[0.001], [0.001], [0.001]]
is_diag: true
frequency: 200 # Hz
key: vicon # optional, if not set us e name of sensor
measurement_noise:
type: multivariate_gaussian
delay: 0.0
initial_state: [0.15, -1.05, 0, 0.5]
initial_input: [0.0, 0.2]
frequency: 50 #Hz, speed of simulator
input_delay: 0.01 # input delay in seconds
do_collision_checks: true
Pacejka Simulator¶
state_type: pacejka_car
input_type: pacejka_car
process_noise:
type: multivariate_gaussian
seed: 42
mean:
value: [[0], [0], [0], [0], [0], [0]]
sensors:
sensor_names: ["vicon"]
vicon:
R:
value: [[0.001], [0.001], [0.001]]
is_diag: true
frequency: 200 # Hz
key: vicon # optional, if not set us e name of sensor
measurement_noise:
type: multivariate_gaussian
delay: 0.0
imu:
R:
value: [[0.001], [0.001], [0.001]]
is_diag: true
frequency: 50 #Hz
key: imu # optional, if not set us e name of sensor
measurement_noise:
type: multivariate_gaussian
seed: 1
mean:
value: [[0], [0], [0]]
delay: 0.0
initial_state: [0.15, -1.05, 0, 0.5, 0, 0]
initial_input: [0.0, 0.2]
frequency: 50 #Hz, speed of simulator
input_delay: 0.01 # input delay in seconds
do_collision_checks: true
Estimator¶
type: "discrete_ekf | sqrt_ekf | mhe | lowpass"
pub_rate: <double> # Hz
max_callback_rate: <double> # Max rate for which measurement callbacks are processed
measurement_timeout_threshold: <double> # Time after which the estimator warns about missing measurements
# initial state
initial_state:
type: "pacejka_car"
value: [double, double, double, double, double, double]
sensors:
sensor_names: ["vicon", "imu", "wheel_encoders"] # Other options: "imu_yaw_rate", "lighthouse"
[vicon]:
R: <Matrix> # 3x3
[imu]:
R: <Matrix> # 3x3
[imu_yaw_rate]:
R: <Matrix> # 1x1
[wheel_encoders]:
R: <Matrix> # 4x4
[lighthouse]:
R: <Matrix> # 4x4
<EKF Parameters | MHE Parameters | Lowpass Parameters>
EKF Parameters¶
initial_input:
type: "pacejka_car"
value: [double, double] # steer, torque
# initial P (state covariance matrix)
P_init: <Matrix> # 6x6
sensors:
sensor_names: [sensor_name]
[sensor_name]:
outlier_rejection:
use_outlier_rejection: <bool>
outlier_rejection_type: "cov_threshold"
outlier_threshold: <double>
max_consecutive_outliers: <int>
[model]:
<Pacejka Dynamic Model>
MHE Parameters¶
use_internal_estimator: false
use_variable_time_step: false
use_internal_filter: false
internal_filter_type: "median"
internal_estimator:
type: "discrete_ekf | sqrt_ekf"
initial_state:
type: "pacejka_car"
value: [double, double, double, double, double, double]
initial_input:
type: "pacejka_car"
value: [double, double] # steer, torque
P_init: <Matrix> # 6x6
# Cost for MHE
P: <Matrix> # 6x6
Q: <Matrix> # 6x6
R_vicon: <Matrix> # 3x3
R_imu: <Matrix> # 3x3
R_imu_yaw_rate: <Matrix> # 1x1
R_wheel_encoders: <Matrix> # 4x4
R_lighthouse: <Matrix> # 4x4
solver_type: "ACADOS"
start_delay: <double>
max_buffer_size: <int>
lag_compensation_time: <double>
warmstart_iterations: <int>
eta: <double>
sensors:
sensor_names: [sensor_name]
[sensor_name]:
MHE_outlier_rejection:
use_outlier_rejection: <bool>
outlier_threshold: <double>
Lowpass Parameters¶
b_dx: List[double] # numerator filter coefficient for velocity x
a_dx: List[double] # dnumerator filter coefficient for velocity x
b_dy: List[double] # numerator filter coefficient for velocity y
a_dy: List[double] # dnumerator filter coefficient for velocity y
b_dyaw: List[double] # numerator filter coefficient for yaw rate
a_dyaw: List[double] # dnumerator filter coefficient for yaw rate
b_yaw: List[double] # numerator filter coefficient for yaw
a_yaw: List[double] # dnumerator filter coefficient for yaw
Visualizer¶
node_rate: <double> # hz
show_track_angle: <true|false> # If yes, publishes arrow instead of line of track center. This slows down visualization
point_downsampling_factor: <int> # only render every ith point of the track
trajectory:
show_past_est_trajectory: <true|false>
show_past_gt_trajectory: <true|false>
number_of_past_samples: <int> # Use -1 for all past points
estimate:
color:
r: double
g: double
b: double
a: double
gt:
color:
r: double
g: double
b: double
a: double
Controllers¶
state_type: <pacejka_car>
input_type: <pacejka_car>
controller_type: <PID|FF_FB|MPCC>
max_rate: double
controller_params:
<PID|FF_FB|MPCC> Params
[model]:
<Dynamic Model>
visualizer:
<Controller Visualizer>
Controller Visualizer¶
type: <mpc|last_reference>
rate: double # Hz
frame_id: <string>
namespace: <string>
[<MPC Visualizer>]
[<Last Reference Visualizer>]
MPC Visualizer¶
double_min_velocity: <double> # Min velocity for visualization (this or below gets mapped to blue)
double_max_velocity: <double> # Max velocity for visualization (this or above gets mapped to red)
planned:
size_x: <double>
size_y: <double>
size_z: <double>
reference:
r: <double>
g: <double>
b: <double>
a: <double>
size_x: <double>
size_y: <double>
size_z: <double>
Last Reference Visualizer¶
# Color and size of the publisher marker
r: <double>
g: <double>
b: <double>
a: <double>
size_x: <double>
size_y: <double>
size_z: <double>
PID Controller¶
target_velocity: <double>
use_filter: <bool>
b_filter: List[double] # Steer filter numerator
a_filter: List[double] # Steer filter denominator
Kd: <double>
Kp: <double>
Ki: <double>
a_torque: <double>
b_torque: <double>
steer_limit: <double>
look_ahead_time: <double>
FF_FB Controller¶
target_velocity: <double>
use_filter: <bool>
b_filter: List[double] # Steer filter numerator
a_filter: List[double] # Steer filter denominator
Kd: <double>
Kp: <double>
Ki: <double>
a_torque: <double>
b_torque: <double>
steer_limit: <double>
K_torque_curv: <double>
mean_curv_dist: <int>
look_ahead_dist: <double>
lag_compensation_time: <double>
MPCC Controller¶
Q1: double
Q2: double
R1: double
R2: double
R3: double
q: double
lag_compensation_time: double
solver_type: <"ACADOS">
Safety Filter¶
state_type: <pacejka_car>
input_type: <pacejka_car>
safety_filter_type: <simple_collision_avoider/mpc_filter>
config:
[MPC Filter Config]
[MPC Visualizer]
MPC Filter Config¶
solver_type: <acados>
reference_method: <uniform|const_input>
dist_targ_multiplier: double # Multiplier that maps an applied torque to the expected driven distance
min_terminal_dist: double # Min distance the car must drive
max_terminal_dist: double # Max distance the car can drive
use_torque_filter: bool # If true, make sure driving distance does only decrease by max dist_decrement_max in each iteration
dist_decrement_max: double # Max distance decrementing each iteration
loookahead_time: double # By how much the state should be advanced
threshold: double # Threshold. If the input norm was modified more than this threhsold, a visual hint is shown with the planned trajectory
cost_torque: double
cost_steer: double
cost_delta_torque: double
cost_delta_steer: double