default_config.yaml

version: "1.0.4.0"

# Estimators used during takeoff
# Lateral state estimator:
# OPTFLOW, GPS, OPTFLOWGPS, RTK, VIO, VSLAM,  HECTOR
lateral_estimator: "GPS"

# Altitude state estimator:
# HEIGHT - rangefinder, PLANE - height above plane estimated from realsense, BRICK - height above BRICK
altitude_estimator: "HEIGHT"

# Heading state estimator:
# GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam
heading_estimator: "PIXHAWK" 

# Active estimators are started at node launch and can be switched to during flight
state_estimators:
  active: ["OPTFLOW", "GPS", "RTK", "VIO", "VSLAM", "BRICK", "BRICKFLOW", "HECTOR", "ICP", "ALOAM", "ALOAMGARM", "ALOAMREP", "LIOSAM"]

heading_estimators:
  active: ["PIXHAWK", "GYRO", "COMPASS", "OPTFLOW", "HECTOR", "BRICK", "VIO", "VSLAM", "ICP", "BRICKFLOW", "ALOAM", "ALOAMREP", "LIOSAM"]
  
altitude_estimators: 
  active: ["HEIGHT", "PLANE", "BRICK", "VIO", "ALOAM", "ALOAMGARM", "ALOAMREP", "BARO", "RTK", "LIOSAM"] 

# rates of publishers# #{
publish_rate:
  main: 100 # [Hz] main and aux odometry
  slow: 2 # [Hz] throttled main odometry
  diag: 10 # [Hz] diagnostics
  max_altitude: 10 # [Hz] maximum allowed altitude
  est_states: 10 # [Hz] inner states of estimators
# # #}

# debug parameters# #{
debug:
  publish_corrections: true # publish debug topics of measurement corrections
  publish_estimator_states: true # publish debug topics of measurement estimator states
  publish_fused_odom: true # otherwise publish unfused pixhawk odometry instead
  publish_pixhawk_velocity: false # pixhawk velocity instead of estimated velocity
  publish_servoing: false # publish diagnostics about visual servoing
  pass_rtk_as_odom: false # publish rtk instead of odometry dangerous outside simulation!
# # #}

# sensor z offset# #{
# (fallback when TF not available)
offset:
  garmin: -0.03 # [m]
  sonar: -0.03 # [m]
  fcu_height: 0.2 # [m] (above ground)
# # #}

# altitude estimation parameters# #{
 
altitude:

  # Do not fuse rangefinder measurements when the UAV is tilted more than this angle
  excessive_tilt: deg(30)

  # Max value of correction when corrections are being saturated (typically after toggling garmin corrections on)
  max_saturated_correction: 0.001 # [m]

  # Altitude limits
  max_default: 30.0 # Used only when safety area is not used
  max_optflow: 10.0 # Do not allow switching to optflow above this altitude
  max_allowed_flight_height_garmin: 10.0 # Do not allow flying higher with garmin-based altitude estimator

  # Custom TF rules
  use_rtk_altitude: false # has to be true for correct TF when using RTK altitude
  use_garmin_for_vio_tf: false # has to be true for correct TF when using Garmin height with VIO lateral estimator

# covariances# #{

  # Process covariance
  Q: [100.0, 0, 0,
      0, 100000.0, 0,
      0, 0, 100000.0]

  # Covariances of measurements
  R:
    height_range: [100.0]
    height_sonar: [100.0]
    height_plane: [100.0]
    height_brick: [100.0]
    height_vio: [0.001]
    vel_vio: [0.01]
    height_aloam: [1.0]
    height_liosam: [10000.0]
    height_baro: [1.0]
    height_rtk: [100.0]
    vel_baro: [100.0]
    vel_liosam_z: [1000.0]
    acc_imu: [100.0]

# # #}

# median filters# #{
  # Parameters of altitude median filters - buffer_size [samples], max_diff [m] (difference of input value from median to be considered valid)
  median_filter:

    garmin:
      buffer_size: 200
      max_diff: 2.0

    sonar:
      buffer_size: 200
      max_diff: 2.0

    plane:
      buffer_size: 40
      max_diff: 2.0

    brick:
      buffer_size: 200
      max_diff: 4.0

    vio:
      buffer_size: 200
      max_diff: 2.0

    aloam:
      buffer_size: 20
      max_diff: 2.0
    
    liosam:
      buffer_size: 20
      max_diff: 2.0

# # #}

# gates# #{
  # Gate for min and max values of measurements [m]
  gate:
    garmin:
      min: 0.05
      max: 40
      use_inno_gate: true # innovation gate (declining large difference between state and measurement)
      inno_gate_value: 5.0 # [m]

    sonar:
      min: 0.1
      max: 4.0

    plane:
      min: 0.5
      max: 20.0 # uses garmin above ~5 m

    brick:
      min: 0.2
      max: 5.0

    vio:
      min: -100.0
      max: 100.0

    aloam:
      min: -1000.0
      max: 1000.0

    liosam:
      min: -1000.0
      max: 1000.0

# # #}

# # #}

# lateral estimation parameters# #{
lateral:

# covariances# #{

# Process covariance
  Q: [0.001, 0, 0,
      0, 0.01, 0,
      0, 0, 0.01]

# Covariances of measurements
  R:
    vel_optflow: [1]
    pos_mavros: [0.01]
    vel_mavros: [100]
    tilt_mavros: [100]
    pos_vslam: [1]
    pos_vio: [0.01] #0.1
    vel_vio: [0.1] #10
    pos_object: [100]
    vel_object: [10000]
    pos_brick: [0.01]
    vel_brick: [100]
    pos_hector: [1]
    pos_tower: [1000]
    pos_aloam: [1]
    pos_liosam: [10]
    pos_uwb: [1]
    vel_liosam: [100]
    vel_icp: [10]
    pos_rtk: [0.001]
    vel_rtk: [1]
    acc_imu: [1000000000] # turned off in estimators

  # RTK estimator must have a different model
  rtk:

    A: [1, 0,
        0, 1]

    B: [0.01, 0,
        0, 0.01]

    H: [1, 0,
        0, 1]

    Q: [0.01, 0,
        0, 0.01]

    R: [1, 0,
        0, 1]

    P: [1, 0,
        0, 1]

# # #}

# median filters# #{
  # Parameters of lateral median filters - buffer_size [samples], max_diff [m/s] (difference of input value from median to be considered valid)
  median_filter:

    optflow:
      use: true
      buffer_size: 10
      max_diff: 2.0

    icp:
      use: true
      buffer_size: 20
      max_diff: 2.0

# # #}

# gates# #{
  # Gate for max absolute twist values of measurements in each axis (x, y) [m/s]
  gate:

    optflow: 
      max: 5.0

    icp:
      max: 5.0

# # #}

# correction saturation # #{
  saturate_mavros_position: false
  max_mavros_pos_correction: 0.5
  max_vio_pos_correction: 0.5
  max_object_pos_correction: 0.5
  max_brick_pos_correction: 1.0
  max_rtk_pos_correction: 0.5
  max_vslam_pos_correction: 0.2
  max_t265_vel: 10.0
  max_safe_brick_jump: 0.3

# # #}

# gps fallback# #{
  gps_fallback:
    allowed: false
    fallback_estimator: "OPTFLOW"
    cov_limit: 10.0 # limit covariance in lateral axes to toggle fallback
    bad_samples: 300 # samples with large covariance before fallback (100 samples ~ 1 second)
    altitude: 4.0 # go to altitude before fallback (optflow should work up to 10 m)
    altitude_wait_time: 5.0 # [s] timeout for reaching target altitude
    return_after_ok: true # return to GPS when covariance is back to normal
    cov_ok: 6.0 # threshold covariance for return from fallback
    good_samples: 300 # samples with normal covariance before returning from fallback (100 samples ~ 1 second)

# # #}

# optflow# #{
  optflow:
    optimized_low: false # use optimized optflow for takeoff and landing
    dynamic_cov: false # use dynamic measurement covariance estimated based on the altitude
    dynamic_cov_scale: 1 # scale the estimated covariance by this factor

# # #}

# hector# #{
  hector:
    reset_after_takeoff: false # Gets rid of map clutter accumulated during takeoff tilting
    reset_routine: false # Switches to ICP or OPTFLOW, stores offset, resets map, switches back to HECTOR

# # #}

# brick# #{
  brick:
    timeout: 0.5 # [s] timeout until fallback from BRICK after loosing detections# #}

# slam# #{
  slam:
    use_general_slam_origin: true # use slam_origin instead of aloam_origin/liosam_origin
    ouster_scan_delay: 0.0 # [seconds]
# # #}

# rtk # #{
  use_full_rtk: true # Full RTK for precise and reliable RTK stations only
  rtk_fuse_sps: true # Fuse RTK even without basestation corrections
# # #}

# # #}

# heading estimation parameters# #{
heading:

  gyro_fallback: true

# covariances# #{

  # Process covariance
  Q: [0.1, 0, 0,
      0, 0.1, 0,
      0, 0, 0.001]

  # Covariances of measurements
  R:
    hdg_compass: [0.1]
    hdg_hector: [0.01]
    hdg_tower: [0.01]
    hdg_aloam: [0.0001]
    hdg_liosam: [10]
    hdg_brick: [10]
    hdg_vio: [100] #100
    hdg_vslam: [1]
    rate_gyro: [0.1]
    rate_optflow: [10]
    rate_icp: [10]
    rate_liosam: [1]
# # #}

# median filters# #{
  # Parameters of heading median filters - buffer_size [samples], max_diff [m] (difference of input value from median to be considered valid)
  median_filter:

    optflow:
      use: true
      buffer_size: 100
      max_diff: 2.0

    icp:
      use: true
      buffer_size: 100
      max_diff: 2.0

# # #}

# gates# #{
  # Gate for max absolute hdg rate values of measurements [rad/s]
  gate:

    optflow: 
      max: 2.0

    icp:
      max: 2.0

# # #}

# correction saturation # #{
  max_brick_hdg_correction: deg(5)
  accum_hdg_brick_alpha: 0.1
  max_safe_brick_hdg_jump: deg(20)

# # #}

# # #}

# Time between mainTimer ticks to be considered as hiccup
hiccup_time_threshold: 0.03 # [s] 

scope_timer:

  enabled: false
  log_filename: "" # if empty, scope timers output to terminal; otherwise log to file