Configuration¶
FusionCore is configured with a single YAML file passed to the launch file:
Full parameter reference¶
fusioncore:
ros__parameters:
base_frame: base_link # must match your robot's base TF frame
odom_frame: odom
publish_rate: 100.0
publish.force_2d: true # zeroes Z position and Z velocity in published output. Use for ground robots.
publish.tf: true # set false to suppress the odom->base_link TF broadcast.
# /fusion/odom keeps publishing. Use when another node owns
# the odom->base_link transform, or when running two
# FusionCore instances where only one should broadcast TF.
# ── IMU ──────────────────────────────────────────────────────────────────
imu.gyro_noise: 0.005 # rad/s: from your IMU datasheet (ARW spec)
imu.accel_noise: 0.1 # m/s² : from your IMU datasheet (VRW spec)
imu.has_magnetometer: false # true for 9-axis (BNO08x, VectorNav, Xsens)
# false for 6-axis: yaw comes from gyro integration
imu.remove_gravitational_acceleration: false
# Set true if your IMU driver already subtracted gravity.
# Check: echo linear_acceleration.z at rest.
# ~9.8 m/s² → leave false (driver publishes raw)
# ~0.0 m/s² → set true (driver already removed it)
# NOTE: opposite of robot_localization's imu0_remove_gravitational_acceleration.
imu.frame_id: "" # override IMU TF frame. Leave empty (default) to use msg->header.frame_id.
#
# When to set this:
# Gazebo Harmonic TurtleBot3 publishes "waffle/imu_link/tb3_imu" instead
# of "imu_link". FusionCore can't find that frame in the TF tree.
# Fix: set imu.frame_id to your URDF frame name (e.g. "imu_link").
#
# WARNING: do NOT set this to "base_link".
# When imu.frame_id equals base_frame, FusionCore skips the TF lookup
# entirely and treats IMU measurements as already in base_link frame.
# If your IMU is mounted at any angle relative to base_link, its
# measurements will be fused with the wrong rotation, silently
# corrupting the orientation estimate. Leave empty unless your driver
# publishes with no frame_id at all.
# Optional second IMU. When non-empty, FusionCore subscribes to this topic
# and fuses each message as an independent measurement of the same state.
# Both IMUs must be in (or TF-transformable to) base_link frame.
# The second IMU uses the same noise model as the primary.
# Useful when your platform has two IMUs and you want redundancy without
# pre-merging them with imu_filter_madgwick or similar. Leave empty to disable.
imu2.topic: ""
imu2.frame_id: ""
imu2.remove_gravitational_acceleration: false
# ── Wheel encoders ────────────────────────────────────────────────────────
# Wheel odometry topic (nav_msgs/Odometry; only the twist is fused).
# The default is deliberately NOT the conventional /odom: FusionCore publishes
# its own fused odometry, so subscribing to /odom would invite a feedback loop
# with its own output. Point this at your driver's topic instead.
encoder.topic: "/odom/wheels" # e.g. "/odom" or "/diff_drive_controller/odom"
encoder.vel_noise: 0.05 # m/s
encoder.yaw_noise: 0.02 # rad/s
# Optional second velocity source (lidar odometry, visual odometry, etc.)
# Must publish nav_msgs/Odometry with velocity in twist field.
# FusionCore does NOT accept sensor_msgs/LaserScan or PointCloud2 directly.
# A scan-matching node must sit between your LiDAR and FusionCore:
#
# LaserScan / PointCloud2
# → KISS-ICP → /kiss/odometry (nav_msgs/Odometry)
# → rtabmap icp_odometry → /icp_odom (nav_msgs/Odometry)
# → rf2o_laser_odometry → /odom (nav_msgs/Odometry)
#
# Note: slam_toolbox publishes PoseWithCovarianceStamped, not Odometry.
# It cannot be used directly as encoder2; wrap it or use KISS-ICP instead.
encoder2.topic: "" # e.g. "/kiss/odometry" or "/icp_odom"
encoder2.vel_noise: 0.05 # m/s fallback when message covariance is zero
encoder2.yaw_noise: 0.02 # rad/s fallback when message covariance is zero
# ── GPS ───────────────────────────────────────────────────────────────────
# Primary GPS fix topic. Read as sensor_msgs/NavSatFix, or gps_msgs/GPSFix
# when gnss.use_gps_fix is true (below). Set this to your driver's topic
# rather than writing a launch remap.
gnss.fix_topic: "/gnss/fix" # e.g. "/fix", "/ublox/fix", "/gps/fix"
gnss.base_noise_xy: 1.0 # m: baseline sigma at HDOP=1
# scaled automatically by HDOP from the message
# standard autonomous GPS: 1.0–2.5
# RTK float: 0.5, RTK fixed: 0.015
gnss.base_noise_z: 2.0 # m
gnss.heading_noise: 0.02 # rad: for dual antenna heading
# Quality gate. Which pair applies depends on what your receiver publishes,
# and getting this wrong is silent: rejected fixes leave the filter dead
# reckoning with nothing but a throttled log line to say so.
gnss.min_sigma_xy: 0.02 # m. FLOOR on the receiver's reported sigma before
# it becomes R. Raise it when your receiver is
# over-confident: a u-blox M9N in SBAS mode was
# measured declaring 0.076 m while scattering
# 1.03 m standing still, 13.6x optimistic. R is
# built from this and the chi2 gate is judged
# against the same R, so believing it drags
# position and can make the gate reject good
# fixes. Measure your own scatter parked, and
# floor it there. A floor in metres is correct
# where a multiplier is not: it fixes the
# over-confident mode and leaves an honest one
# untouched.
gnss.min_sigma_z: 0.05 # m. Same, vertical.
gnss.max_sigma_xy: 25.0 # m of reported 1-sigma. THIS is the gate that runs
gnss.max_sigma_z: 50.0 # for sensor_msgs/NavSatFix, which carries no DOP.
# A standalone receiver reports 2-8 m horizontal and
# 10-25 m vertical in normal conditions, all usable.
gnss.max_hdop: 4.0 # dimensionless DOP. Only applies when the fix has no
gnss.max_vdop: 6.0 # covariance at all, i.e. gps_msgs/GPSFix reporting
# receiver-native DOP.
gnss.min_satellites: 4
gnss.min_fix_type: 1 # 1=GPS, 2=DGPS, 3=RTK_FLOAT, 4=RTK_FIXED
# NavSatFix: status=2 maps to RTK_FIXED. RTK_FLOAT (3)
# is unreachable via NavSatFix; use gnss.use_gps_fix
# below if your receiver publishes gps_msgs/GPSFix.
gnss.use_gps_fix: false # Set true when your driver publishes gps_msgs/GPSFix
# on gnss.fix_topic instead of sensor_msgs/NavSatFix.
# GPSFix unlocks RTK_FLOAT status, uses receiver-native
# hdop/vdop values, satellites_used, and err_horz/err_vert
# as a fallback covariance. Default false: NavSatFix works
# with all receivers. See GPS Receiver Setup below.
# Antenna lever arm: offset from base_link to GPS antenna in body frame
# x=forward, y=left, z=up (meters). Leave 0.0 if antenna is above base_link.
# Correction only activates after heading is independently validated.
gnss.lever_arm_x: 0.0
gnss.lever_arm_y: 0.0
gnss.lever_arm_z: 0.0
# Second GPS receiver (optional)
gnss.fix2_topic: ""
gnss.lever_arm2_x: 0.0
gnss.lever_arm2_y: 0.0
gnss.lever_arm2_z: 0.0
# GPS velocity (optional): fuses horizontal GPS speed as an independent
# measurement update, separate from wheel odometry.
# Accepts nav_msgs/Odometry with velocity in ENU frame:
# twist.linear.x = east velocity (m/s)
# twist.linear.y = north velocity (m/s)
# twist.covariance[0,7] used when positive; falls back to encoder noise otherwise
# FusionCore rotates ENU -> body frame internally using the current quaternion.
# Enables slip detection: innovation between GPS velocity and wheel odometry
# reveals wheel slip. Works with F9P, Septentrio, or any receiver that
# publishes velocity. Leave empty to disable.
gnss.velocity_topic: ""
# Radar Doppler velocity (optional): fuses ego-velocity from a 4D imaging radar
# (Continental ARS548, Oculii Eagle, Aptiv ESR, etc.) as an independent measurement.
# Accepts nav_msgs/Odometry with velocity in robot body frame:
# linear.x = forward speed (m/s), linear.y = lateral speed (m/s)
# A bridge node extracts ego-velocity from raw Doppler point cloud and publishes here.
# Works indoors and in all weather: rain, fog, dust, darkness.
# radar.vel_noise is used when the message covariance is zero or negative.
radar.velocity_topic: ""
radar.vel_noise: 0.1 # m/s fallback noise when message has no covariance
# Heading input: pick one or both
gnss.heading_topic: "" # sensor_msgs/Imu (dual antenna heading)
gnss.azimuth_topic: "" # compass_msgs/Azimuth (preferred REP-145 standard)
# ── Raw magnetometer heading ──────────────────────────────────────────────
# Fuses sensor_msgs/MagneticField directly into the UKF as a 1-DOF heading
# measurement. Applies hard/soft iron correction then tilt compensation using
# the current filter roll/pitch. Useful when GPS is unavailable and the robot
# is stationary (GPS track heading requires motion; magnetometer does not).
# Heading source hierarchy: DUAL_ANTENNA > MAGNETOMETER > GPS_TRACK.
# Requires calibration: collect data by rotating the robot through a full circle
# and use imu_calib (ROS) or magneto (desktop) to get hard_iron and soft_iron values.
magnetometer.enabled: false
magnetometer.topic: "/imu/mag" # sensor_msgs/MagneticField publisher
magnetometer.noise_rad: 0.05
# Standard deviation of heading estimate (rad) after correction.
# 0.05 rad (~3 deg) is typical for a well-calibrated sensor in benign conditions.
# Loosen to 0.15-0.30 near motors or variable magnetic fields.
magnetometer.chi2_threshold: 9.21
# Chi-squared outlier gate: chi2(1, 0.99) = 9.21 for 1-DOF heading.
# Rejects magnetic spikes. Tighten to 3.84 (chi2(1,0.95)) in clean environments.
magnetometer.declination_rad: 0.0
# Magnetic declination: offset from magnetic north to true north (rad).
# Positive east. Look up your location at https://www.magnetic-declination.com
# Leave 0.0 when FusionCore can self-correct via GPS: the constant heading
# offset is absorbed by the filter over time.
magnetometer.hard_iron: [0.0, 0.0, 0.0]
# Constant bias offset in body frame (Tesla): [x, y, z].
# Estimated by rotating the sensor through a full circle and computing
# (max + min) / 2 per axis. Use imu_calib or magneto for best results.
magnetometer.soft_iron: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
# 3x3 scale+rotation matrix in row-major order. Identity = disabled.
# Corrects for elliptical distortion in the magnetic field.
# Estimated alongside hard iron using imu_calib or magneto.
magnetometer.field_strength: 0.0
# Local Earth total-field magnitude, in the SAME units as the incoming reading
# (e.g. ~0.48 for Gauss, ~48 for microtesla). A clean reading's corrected
# magnitude equals this; a nearby motor or steel structure distorts it and
# produces a wrong heading the chi2 gate cannot reliably catch. When the
# magnitude deviates by more than field_tolerance the reading is rejected.
# Look up the total field at https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml
# 0.0 = disabled (no magnitude check).
magnetometer.field_tolerance: 0.2
# Allowed fractional deviation of the field magnitude before a reading is
# treated as locally disturbed. 0.2 = accept within +-20% of field_strength.
# ── GPS coast mode ────────────────────────────────────────────────────────
# During GPS blackouts, inflates process noise so P grows and the chi2 gate
# relaxes by the time GPS resumes. Prevents the filter from rejecting its
# own recovery fixes because dead-reckoning drift made innovations look like
# outliers. See How It Works for the full explanation.
gnss.coast_n: 3
# Consecutive chi2 GPS rejections before entering coast mode. 0 = disabled.
gnss.coast_min_gap_s: 1.0
# Rejection-triggered coast only fires if the rejection streak began after a
# GPS gap of at least this many seconds (the receiver actually went silent and
# the filter dead-reckoned). A continuously present GPS that keeps failing the
# chi2 gate is a persistent outlier (e.g. a sustained multipath spike), not
# filter drift, so inflating P to re-admit it would let the outlier defeat the
# gate. Gating on a preceding gap keeps a sustained spike rejected for its full
# duration while preserving post-outage re-acquisition. 0 = old behavior.
gnss.coast_q_factor: 10.0
# Q_position multiplier in coast mode. Controls how fast position uncertainty
# grows during the blackout. 10.0: after 228s, sigma_xy=48m (rejects 840m
# outliers, accepts 193m drift). After 461s, sigma_xy=68m (accepts 274m drift).
gnss.coast_timeout_s: 30.0
# Also enter coast if GPS is silent this long (seconds). Handles outages
# where the receiver stops publishing entirely. 0.0 = timeout trigger disabled.
gnss.coast_q_bias_factor: 100.0
# Q_gyro_bias multiplier in coast mode. Loosens bias confidence so encoder
# WZ can drive fast heading bias correction during the blackout. 100.0 typical.
gnss.coast_imu_wz_scale: 500.0
# R_imu[WZ,WZ] multiplier in coast mode. Makes IMU heading rate less trusted
# so encoder WZ dominates. 1.0 = disabled. 500.0 typical for long blackouts.
gnss.recovery_rejection_n: 15
# After this many consecutive rejections that follow a GNSS gap, inflate
# P[x,x] and P[y,y] so the next fix passes chi2 and corrects through a normal
# update. This is what lets the filter come back after a blackout long enough
# that its own drift makes every returning fix look like an outlier. It is
# gap-gated: a continuous spike cannot trigger it. Must be > gnss.coast_n.
# 0 = disabled.
gnss.p_inflate_sigma: 50.0
# Floor for that inflation (metres of XY sigma). The inflation is sized from
# the rejected innovation, so this is only a lower bound. A fixed value cannot
# work for both a 60 s outage and an eight minute one.
gnss.recovery_timeout_s: 0.0
# DEPRECATED and inert. The filter never read this parameter, and the
# position-injection recovery its old description promised does not exist.
# It is still declared so existing configs load, and the node warns if you
# set it. Post-blackout recovery is gnss.recovery_rejection_n.
gnss.track_heading_enabled: true
# Fuses GPS displacement bearing as a yaw pseudo-measurement whenever the
# robot has moved gnss.track_heading_min_dist meters since the last fusion.
# This is the primary mechanism for estimating encoder WZ bias without a
# dual-antenna GPS. Disable only with an independent heading source.
gnss.track_heading_min_dist: 5.0
# Minimum GPS displacement (m) between heading fusions.
gnss.track_heading_max_sigma: 0.4
# Maximum heading uncertainty to allow a fusion (radians). Computed as
# gps_noise / displacement. 0.4 rad = 23 degrees.
gnss.track_heading_min_speed: 0.2
# Minimum robot speed (m/s) for GPS displacement steps to count toward
# heading observability. Below this: could be GPS jitter, not real motion.
# Increase on high-vibration platforms (construction equipment, tracked robots).
gnss.track_heading_max_yaw_rate: 0.3
# Maximum yaw rate (rad/s) for displacement steps to count. During fast turns
# the bearing changes too quickly for a reliable heading measurement.
# Decrease for robots that make tight turns at slow speed.
gnss.track_heading_cross_check_deg: 15.0
# Reject a GPS track heading that disagrees with the current heading estimate
# by more than this, measured as the median of recent disagreements rather
# than a single sample so one bad bearing cannot veto a good source. Course
# over ground is not body heading: on any curved path the two differ by a real
# bias, and a biased measurement pulls the estimate wrong however honest its
# covariance is. This is the guard against fusing that bias for a whole run.
# Set to 0 to disable the cross-check.
# ── Lever arm heading gating ──────────────────────────────────────────────
gnss.lever_arm_max_heading_sigma_deg: 20.0
# Lever arm correction is only applied when heading uncertainty is below this.
# When heading degrades (e.g. during prolonged turns), rotating the lever arm
# by an uncertain heading adds more position error than it removes.
# Default 20 deg disables lever arm during tight-turn sections while leaving
# it active during straight/gentle-curve driving where it genuinely helps.
# Rule of thumb: lever_arm_m * sin(threshold_rad) should be < GPS noise sigma.
# ── Measurement timing ────────────────────────────────────────────────────
max_measurement_delay: 0.5
# How far behind the filter clock (seconds) a measurement can be and still be
# used. GPS and VSLAM within this window are retrodicted: the filter rewinds
# to the measurement's timestamp, fuses, and replays the buffered IMU forward.
# Other sensors arriving older than this are rejected as stale, because fusing
# them would require moving the filter clock backward.
# Raise it only for a sensor with a genuinely large, KNOWN latency. If the
# stale_reject counters in /fusion/debug/filter_health climb, the cause is
# almost always sensors on different clocks, not a too-small window: check
# that every sensor's header.stamp agrees (see the troubleshooting guide) and
# fix the drivers rather than widening this.
# ── Outlier rejection ─────────────────────────────────────────────────────
outlier_rejection: true
outlier_threshold_gnss: 16.27 # chi2(3, 0.999): 3D GPS position
outlier_threshold_imu: 15.09 # chi2(6, 0.999): 6D IMU (gyro + accel)
# If imu.has_magnetometer: false, IMU only fuses roll/pitch (DOF=2).
# Lower to 13.82 (chi2(2, 0.999)) to maintain 99.9% confidence.
outlier_threshold_enc: 11.34 # chi2(3, 0.999): 3D encoder
outlier_threshold_hdg: 10.83 # chi2(1, 0.999): 1D heading
outlier_threshold_vslam: 22.46 # chi2(6, 0.999): 6D VSLAM pose
# Keep vslam gate at chi2(6, 0.999). ORB-SLAM3 can jump on reinitialization:
# this gate rejects those jumps automatically when covariance is calibrated.
# Do NOT lower these below chi2 critical values. At 7.0 normal GPS noise
# trips the gate and every fix gets rejected.
gnss.max_speed: 0.0
# Physical-plausibility gate on GPS position. Rejects any fix farther from the
# filter's predicted position than the robot could have moved or drifted since
# the last accepted fix: max_speed * gap_seconds + max_speed_margin. This is a
# kinematic backstop that catches an impossible GPS jump the chi2 gate may
# admit when its covariance has been inflated during coast recovery. Set to the
# platform's maximum plausible speed in m/s (a few times cruise is safe); this
# is a per-robot spec like wheel radius, not per-run tuning. 0.0 = disabled.
gnss.max_speed_margin: 5.0
# Fixed slack (m) added to the bound, covering prediction error that the
# receiver's own noise does not explain. 3-5 m is typical.
gnss.max_speed_sigma_k: 5.0
# Multiples of the receiver's REPORTED horizontal sigma also added to the
# bound, so the gate adapts to the receiver instead of being a fixed distance.
# This matters more than it looks. With sigma_k at 0 the whole bound is
# absolute metres: at 1 Hz with max_speed 2.0 and a 5 m margin it is 7 m,
# and a standalone receiver whose own sigma is ~6 m then trips it constantly.
# Measured on a u-blox M9N: 157 of 500 good fixes rejected, loop closure
# 2.62 m -> 7.27 m. The full bound is:
# max_speed * gap + max_speed_margin + sigma_k * reported_sigma_xy
# Scaled by the RECEIVER's sigma deliberately, never by the filter's own
# covariance: chi2 is already the covariance-scaled test, and this gate exists
# precisely to catch what a coast-inflated chi2 lets through.
gnss.continuity_max_m: 0.0
gnss.continuity_auto: true
# When continuity_max_m is 0, measure the threshold from the receiver's own
# fix-to-fix scatter instead of leaving the gate off. The filter watches the
# first 200 admissible fixes, takes the largest prediction residual, and uses
# 1.5x that, clamped to [2, 25] m. It logs and publishes what it chose. An
# explicit continuity_max_m always wins and skips learning.
# Rejects a fix that disagrees with the two accepted fixes before it, by more
# than this many metres. 0.0 = disabled. This is the gate that can actually
# see a small GPS spike.
#
# The chi2 gate above cannot. It tests a fix against the FILTER, so its scale
# is S = H P H' + R, and on a consumer receiver that is tens of square metres.
# Measured on a u-blox at 1 Hz reporting 3.24 m sigma, by injecting a single
# displaced fix into a real log and replaying it:
#
# spike chi2 verdict position step it caused
# 5 m accepted 1.40 m
# 10 m accepted 3.55 m
# 15 m accepted 4.53 m
# 25 m accepted 6.21 m
# 30 m REJECTED
#
# A 15 m multipath spike, ordinary beside a building or under tree cover, is
# accepted and moves position 4.5 m. Analysis agrees with the experiment:
# sqrt(16.27 * (5.25^2 + 3.24^2)) = 24.9 m. Fixing heading does NOT help; a
# perfect absolute heading halved position sigma and made the gate LOOSER.
#
# Continuity asks a different question that never involves P: does this fix
# agree with the fixes either side of it? With continuity at 3.0 m the same
# spikes are rejected from 2 m upward.
#
# HOW TO SET IT. Measure, do not guess. tools/nis_from_bag.py reports your
# receiver's fix-to-fix second difference. Across 2361 fixes from seven field
# logs on one rover, a good fix had a median second difference of 0.09 to
# 0.30 m and a p99 under 2.1 m, so 3.0 sat 10 to 30 times above normal and
# rejected nothing that should have been kept. Dropping to 2.0 began rejecting
# good fixes. Err generous: rejecting good data is the more expensive mistake.
#
# It catches JUMPS, not sustained bias. Multipath that shifts the solution and
# holds it there breaks continuity once and then looks continuous at the new
# offset. chi2 and coast mode still own that case, which is why this is added
# alongside them rather than instead.
#
# The check is skipped unless three consecutive accepted fixes are evenly
# spaced (within a factor of two on the interval). Across a real outage the
# second difference is legitimately large, and rejecting the first fix after
# one is exactly the failure gnss_coast_min_gap_s exists to prevent.
gnss.outlier_sigma_xy: 0.0
# Short-term consistency of the receiver in metres, used ONLY by the chi2 gate
# above. 0.0 = gate on the same R the update uses, which is the old behaviour.
#
# A receiver's reported covariance describes ABSOLUTE accuracy: multipath and
# ionospheric error that moves slowly. Consecutive fixes are far more
# consistent than that figure implies. Measured on that same log, 3.24 m
# declared against a 0.171 m median second difference, a factor of 55.
#
# The update wants the absolute figure, or the filter believes GPS to
# centimetres it has not earned and tracks that slow bias rigidly. The gate
# wants the short-term figure, because an outlier IS a break in short-term
# consistency. Setting this moved the chi2 rejection threshold from 30 m to
# 15 m with no false rejections, but it cannot go further because the filter's
# own P remains in S. Prefer gnss.continuity_max_m for spike rejection.
#
# A value ABOVE the receiver's own sigma makes the gate LOOSER, not tighter:
# 5.0 on a 3.24 m receiver moved the threshold from 26 m out to 30 m.
# ── Adaptive noise ────────────────────────────────────────────────────────
adaptive.imu: true
adaptive.encoder: true
adaptive.gnss: true
adaptive.window: 50 # sliding window size (updates, not seconds)
adaptive.alpha: 0.01 # EMA learning rate. 0.01 = slow, stable.
# ── UKF process noise ─────────────────────────────────────────────────────
# These are per-predict-step noise values (not spectral densities).
# At 100Hz IMU, the filter predicts 100 times per second. Each step adds
# Q to P, so effective noise rate = q_* * 100 per second.
ukf.q_position: 0.01
ukf.q_orientation: 1.0e-9 # quaternion regularization ONLY: do not increase.
# Orientation uncertainty propagates from q_angular_vel
# through the kinematics. Large values here corrupt
# quaternion norm and cause yaw/Z drift.
ukf.q_velocity: 0.1
ukf.q_angular_vel: 0.1
ukf.q_acceleration: 1.0
ukf.q_gyro_bias: 1.0e-5 # biases change slowly (MEMS thermal drift)
ukf.q_accel_bias: 1.0e-5
ukf.q_encoder_wz_bias: 1.0e-7 # encoder WZ bias is mechanical: very stable.
# Smaller than gyro bias because it changes only
# with physical wear, not thermal effects.
# ── Startup ───────────────────────────────────────────────────────────────
init.stationary_window: 0.0
# Seconds of IMU data to collect before starting (robot must be stationary).
# Estimates accelerometer bias at startup → reduces 60s warmup transient
# from ~10cm to under 1cm. Set 2.0 if startup drift is a problem.
# Falls back to zero-bias automatically if robot moves during window.
init.wait_for_all_sensors: false
# When true: hold filter initialization until every configured sensor has
# published at least one message. Prevents the filter from drifting on IMU
# alone while GPS and wheel odometry are still coming online at startup.
# Replaces the sleep() workaround in launch files.
init.sensor_wait_timeout: 10.0
# Seconds to wait before starting anyway if a sensor never arrives.
# A WARN lists which sensors were missing. Set 0.0 to disable the timeout.
# ── Motion model ──────────────────────────────────────────────────────────
motion_model: "ConstantVelocityAcceleration"
# Controls how sigma points are propagated in the UKF predict step.
#
# "ConstantVelocityAcceleration" (default): no platform constraints.
# VY and AY grow freely between measurements. Correct for aerial vehicles.
# Good baseline for any platform.
#
# "DifferentialDrive": zeros VY and AY each predict step.
# The filter knows a diff-drive robot cannot slide sideways.
# Tighter lateral covariance, less position smear on straight runs.
# Use for: differential drive, skid-steer, tracked vehicles.
#
# "Ackermann": same lateral constraint as DifferentialDrive.
# wheelbase stored for future minimum-turning-radius extensions.
# Use for: car-like robots, forklifts, front-steered outdoor platforms.
motion_model_params.wheelbase: 0.55 # metres (only used by Ackermann)
# ── Deterministic replay ──────────────────────────────────────────────────
replay.checkpoint_path: "/tmp/fusioncore_checkpoint.txt"
# File used by ~/save_checkpoint and ~/load_checkpoint services.
# save_checkpoint: writes the full 23-state + 23x23 covariance to this file.
# load_checkpoint: restores that state (restarts filter from that point).
# Workflow: replay a bag to a known-good point → save → tweak params →
# load (instant, no re-replay) → observe the problem window.
Nominal IMU dt: not trusting timestamps you cannot trust¶
Anything downstream of an integrator amplifies timestamp error. On one recorded run, shifting every IMU stamp by a single microsecond and changing nothing else moved final yaw by 109 degrees, and across a 120 s GNSS outage moved final position by 50.6 m. Most of that is an unbounded quaternion covariance rather than the stamps themselves, but the sensitivity is real.
imu.fixed_rate_hz: 0.0 # 0 = derive dt from stamps (default). Above 0,
# propagate by exactly 1/rate instead.
Set to your IMU's real rate and the propagation step no longer depends on stamp jitter at all. This is what Martin Pecka's team does: they never compute dt in fusion from IMU timestamps, they assume the configured rate.
Only set a rate you have measured. A wrong rate is a systematic error, not a noisy one: the filter integrates the wrong amount of time on every single step, and it does not average out. A BNO085 nominally at 100 Hz has been logged running at 103 and 109.
FusionCore watches for this. It measures the real arrival rate from the raw
stamps and sets imu_fixed_rate_mismatch in the status when the two differ by
more than 2%. There is a second, more visible symptom: because the filter clock
advances at the nominal rate while stamps advance at the real one, the two
separate at exactly the rate error, and once that exceeds
max_measurement_delay the stale-skew guard starts rejecting your IMU. If
you enable this and your IMU begins getting rejected, the rate is wrong.
Stopping a parked robot chasing its GPS¶
When the wheels report zero velocity, FusionCore fuses a zero-velocity update (ZUPT). That pins velocity, and says nothing about position.
The consequence is easy to miss. Position process noise keeps growing between fixes even though the robot is not moving, so the covariance stays large, the Kalman gain stays high, and every incoming fix drags the estimate. A receiver that wanders while parked takes the estimate with it.
Measured on a u-blox M9N, parked for 57 seconds with wheel encoders confirming the robot was stationary: the receiver's reported position moved 9.76 m and the fused position followed it for 10.16 m. The robot did not move at all.
zupt.velocity_threshold: 0.05 # m/s below which the robot counts as still
zupt.angular_threshold: 0.05 # rad/s, same
zupt.noise_sigma: 0.01 # m/s: how tightly to believe "not moving"
zupt.position_noise_scale: 1.0 # scale on POSITION process noise while still
zupt.accel_std_threshold: 0.5 # m/s^2: block ZUPT if the IMU disagrees
zupt.accel_std_threshold exists because wheels reporting zero is not the same
thing as a stationary robot. An encoder that dies mid-run keeps publishing zero
while the robot drives, ZUPT then pins velocity to zero and the filter spends the
rest of the run refusing to let GNSS move it. On the run that found this, the
estimate recovered about 7 m of 20 m actually driven.
The accelerometer is the one sensor that cannot be fooled by a dead encoder, so the standard deviation of accelerometer magnitude over the last 100 samples is checked before ZUPT is allowed to fire. Measured on this project's rover: 1.69 to 2.25 m/s^2 while driving against 0.013 to 0.021 m/s^2 parked, which is two orders of magnitude of separation, so the 0.5 default sits nowhere near either population. Set to 0 to disable the guard.
A high-vibration platform running an isolated IMU mount may need this raised;
FusionCoreStatus::zupt_blocked_by_imu says when the guard is the reason ZUPT
is not firing, so check that before changing it.
zupt.position_noise_scale is the one that fixes the drift. At 1.0 nothing
changes. Below 1.0 the position covariance stops growing while the robot is
known to be still, so it decays as fixes arrive, the gain falls, and consecutive
fixes are averaged rather than followed. Measured at 0.001 across 11 parked
windows from six field runs, mean excursion falls from 3.49 m to 1.64 m, loop
closure over the whole run is unchanged within noise, and no additional fixes
are rejected. The effect saturates below about 0.001, and it goes no further on
its own: see the next section for why.
It is deliberately not applied while GNSS coast mode is active. Coast inflation exists so the filter can re-admit GNSS after a blackout, and silently cancelling it here would change a behaviour this setting has nothing to do with. The scale is handed back as soon as the encoders report motion again.
The default stays at 1.0 until this has been checked against the full NCLT regression suite. It is measured on one robot with one receiver, which is not enough to move a default.
Measuring the receiver while the robot is parked¶
Suppressing process noise stops the covariance growing, but it does not fix the other half of the problem. A Kalman filter assumes measurement errors are white, so sixty parked fixes of a fixed point shrink its position uncertainty by about sqrt(60). GNSS error is not white over a minute: multipath, ionosphere and satellite geometry drift slowly, so consecutive fixes are largely the same error repeated. The filter ends up far more confident than the geometry supports, each fix keeps dragging the estimate, and it carries that over-confidence into the next leg of the run.
Standing still is the one moment in a run where this is checkable, because every
fix is then sampling the same physical point. zupt.gnss_noise_scale turns that
into a correction:
zupt.gnss_noise_scale: 1.0 # UPPER BOUND on the inflation; 1.0 disables it
zupt.gnss_min_samples: 5 # parked fixes before the measurement is trusted
Two things are measured from the parked fixes, per axis:
- Magnitude. Their spread is the receiver's real short-term sigma. Divided by the sigma it declares and squared, a receiver that is as good as it claims scores 1 and is left alone.
- Correlation. The lag-1 autocorrelation
rof those fixes. The honest effective sample size isN(1-r)/(1+r), so the measurement noise has to carry a factor(1+r)/(1-r)for the filter's own posterior to mean what it says.
The applied inflation is the product of the two, capped by
zupt.gnss_noise_scale. So that number bounds how far one bad parked window can
push the filter and does not decide the amount. Nothing here is tuned per
environment or per receiver.
The correlation term fires for an honest receiver too, and it should. An RTK unit correctly reporting a 2 cm sigma still has errors correlated over minutes, so a filter that parks for a minute and averages sixty of them is wrong about how much it knows. That is a general defect, not a bad-receiver defect.
On the same 57-second parked window as above, with a receiver declaring 21 to 45 m of sigma while actually spreading 0.4 to 3.3 m, the magnitude term stayed at 1.0 throughout: the receiver was not over-confident. The lag-1 autocorrelation measured 0.63 rising to 0.985, which is an inflation of 5x rising to the 100x cap, and it took that window's idle drift from 0.53 m to 0.10 m. Across all 11 windows the mean falls from 1.64 m to 0.74 m, helping in 8 and hurting in none. Whole-run loop closure was unchanged within noise.
Watch what it actually measured on filter_health:
gnss_parked_sigma_observed and _declared report the magnitude side,
gnss_parked_correlation the correlation side, and gnss_parked_inflation what
was applied after the cap. All are -1 or 1.0 until a parked window has produced
zupt.gnss_min_samples fixes.
The cost, and it is real. A robot parked for a long time cannot re-acquire if it was genuinely lost before it stopped. For a stop of tens of seconds that does not matter. For one parked overnight it does. The evidence is dropped the moment the encoders report motion, so a robot that parks once does not stay over-confident for the rest of the run.
Secondary twist sources (encoder2)¶
encoder2.topic accepts a second nav_msgs/Odometry source and fuses it through
the same path as the wheel encoder. Typical uses are LiDAR odometry (KISS-ICP),
a tracking camera, or an optical flow sensor.
encoder2.topic: "/odom/flow"
encoder2.vel_noise: 0.05
encoder2.yaw_noise: 0.02
encoder2.channels: ["vx", "vy"] # default ["vx", "vy", "wz"]
encoder2.channels matters more than it looks. A Twist message always
carries all three of vx, vy and wz, so a source that measures only some of them
still publishes a number for the rest, and that number is 0.0. Once it reaches
the filter there is nothing to distinguish it from a measured zero.
The concrete case: the PMW3901 and PAA5100 optical flow drivers never assign
angular.z and leave twist.covariance at zero. Without listing channels, the
filter falls back to encoder2.yaw_noise and every sample arrives as a confident
"the robot is not rotating", competing with the gyro on every turn. The sensor
cannot measure yaw rate; it reported a zero because the field is a zero.
List only what the sensor genuinely measures. Anything omitted is not fused at all, rather than fused with a large noise value.
Optical flow specifically: the driver converts pixels to metres using a
z_height parameter and the relationship is linear, so a sensor mounted at 0.25 m
while the parameter is left at its 0.025 default reports every velocity 10x too
small. Measure the real mount height.
GNSS Doppler velocity bridge (ublox F9P / M8U)¶
FusionCore itself has no dependency on any specific GPS driver. It accepts velocity from any receiver via gnss.velocity_topic, which expects nav_msgs/Odometry with ENU velocity (linear.x=east, linear.y=north).
If your receiver is a u-blox module (F9P, M8U, NEO-M9N, etc.), the fusioncore_ublox companion package provides a ready-made bridge. It is a separate package with its own dependency on ublox_msgs so the FusionCore core remains clean.
# Build the companion package alongside FusionCore
colcon build --packages-select fusioncore_ros fusioncore_ublox
Launch the bridge alongside FusionCore:
# Terminal 1: FusionCore
ros2 launch fusioncore_ros fusioncore.launch.py fusioncore_config:=your_robot.yaml
# Terminal 2: ublox bridge
ros2 launch fusioncore_ublox gnss_doppler_bridge.launch.py \
navpvt_topic:=/ublox/navpvt \
output_topic:=/gnss/doppler_vel
Matching FusionCore config:
What the bridge does:
| NavPVT field | Unit | ENU output |
|---|---|---|
vel_e (east) |
mm/s | twist.linear.x (m/s) |
vel_n (north) |
mm/s | twist.linear.y (m/s) |
vel_d (down) |
mm/s | twist.linear.z = -vel_d/1000 (m/s) |
s_acc |
mm/s | covariance[0,7] = (s_acc/1000)^2 |
Fixes with fix_type < 3 (no 3D lock) or gnssFixOK flag unset are silently dropped. Speeds below 0.05 m/s are also dropped to avoid heading corruption at standstill.
Other receivers: publish nav_msgs/Odometry with ENU velocity on any topic and point gnss.velocity_topic at it. FusionCore doesn't care which driver produced it.
Choosing a motion model¶
Start with the default (ConstantVelocityAcceleration) unless you have a specific reason to change it. It works well for all platforms and matches what robot_localization users are used to.
Switch to DifferentialDrive if:
- Your robot is a differential drive, skid-steer, or tracked vehicle
- You see small lateral position drift on straight runs
- Your VY state doesn't stay near zero between encoder updates
Switch to Ackermann if:
- Your robot has front-wheel steering (car-like, forklift, outdoor field robot)
- The lateral constraint is the same as DifferentialDrive; wheelbase is stored for future minimum-turning-radius enforcement
Do not use DifferentialDrive or Ackermann for:
- Aerial vehicles (no lateral constraint applies)
- Holonomic (mecanum) platforms (those can move sideways intentionally)
Wait for all sensors: replacing sleep() in launch files¶
A common pattern in ROS launch files is sleep(3) before starting the navigation stack to give sensors time to come online. This is fragile: on a slow machine the sensors might need 5 seconds, and on a fast one you waste 3 seconds on every launch.
init.wait_for_all_sensors: true replaces this entirely. FusionCore holds initialization until it has seen at least one message from every sensor you configured. Then it starts. No sleep needed.
The timeout is a safety net: if a sensor fails to start, the filter does not hang forever. It logs which sensors were missing and starts anyway:
This is especially useful at competition startup, on-site robot power-on, or any deployment where sensor initialization order is not guaranteed.
Deterministic replay: debugging without hardware¶
See How It Works: Deterministic replay for the full workflow. Quick reference:
# Save filter state at any point during bag replay
ros2 service call /fusioncore/save_checkpoint std_srvs/srv/Trigger
# Restore that state instantly (no need to replay from the beginning)
ros2 service call /fusioncore/load_checkpoint std_srvs/srv/Trigger
The checkpoint file path is set by replay.checkpoint_path (default /tmp/fusioncore_checkpoint.txt).
GPS without a fix (indoor / no GPS)¶
Set reference.use_first_fix: false and leave reference coords at 0.0. The filter starts at the origin and runs on IMU + wheel odometry alone. No GPS topics needed.
Agricultural RTK (UTM output)¶
Some RTK receivers output easting/northing directly:
input.gnss_crs: "EPSG:32617" # UTM zone 17N (adjust for your zone)
output.crs: "EPSG:32617"
output.convert_to_enu_at_reference: false
reference.use_first_fix: true
GPS receiver setup: NavSatFix vs GPSFix¶
FusionCore supports two GPS message types on gnss.fix_topic (default /gnss/fix). The default is sensor_msgs/NavSatFix because every ROS GPS driver publishes it. Set gnss.use_gps_fix: true to switch to gps_msgs/GPSFix if your driver supports it.
nmea_navsat_driver does NOT publish gps_msgs/GPSFix
An earlier version of this table listed it as a GPSFix source. That was
wrong, reported by a user on issue #73. Checked against the driver source:
it publishes sensor_msgs/NavSatFix on fix, geometry_msgs/TwistStamped
on vel, geometry_msgs/QuaternionStamped on heading, and
sensor_msgs/TimeReference. If you are on nmea_navsat_driver, leave
gnss.use_gps_fix at false. To get GPSFix from an NMEA receiver, run
fix_translator from gps_umd, which converts NavSatFix to GPSFix.
sensor_msgs/NavSatFix |
gps_msgs/GPSFix |
|
|---|---|---|
| Driver support | Universal | gpsd_client (gps_umd), septentrio_gnss_driver, swiftnav-ros2, KumarRobotics/ublox |
| RTK_FLOAT status | Not expressible | Yes (status 20) |
| Separate HDOP / VDOP | No | Yes |
| Satellites used | No | Yes |
| 95% CI error bounds | No | err_horz / err_vert |
| Covariance matrix | Yes | Yes |
When to use NavSatFix (default)¶
NavSatFix works with all receivers. For most setups, leave gnss.use_gps_fix: false.
The only thing you cannot get via NavSatFix is RTK_FLOAT. If you are using autonomous GPS (CEP 1-3m) or RTK fixed, NavSatFix is all you need.
When to use GPSFix¶
Switch to gnss.use_gps_fix: true when:
- Your receiver can output RTK_FLOAT and you want to fuse those fixes (better than autonomous, worse than RTK fixed). Set
gnss.min_fix_type: 3to require it or allow it. - Your driver publishes receiver-native HDOP/VDOP rather than a covariance matrix, and you want those values used directly in the noise model.
- Your driver sets
err_horz/err_vert(95% CI bounds) and you prefer that over a synthetic covariance.
fusioncore:
ros__parameters:
gnss.use_gps_fix: true
gnss.min_fix_type: 3 # require RTK_FLOAT or better (3=FLOAT, 4=FIXED)
gnss.base_noise_xy: 0.5 # metres: baseline at HDOP=1 for RTK_FLOAT
gnss.base_noise_z: 1.0
Covariance priority (GPSFix)¶
When gnss.use_gps_fix: true, FusionCore picks the best available covariance source in this order:
position_covariance_type == 3(full 3x3): used directly, including off-diagonal terms.position_covariance_type >= 1(diagonal): diagonal elements used, hdop/vdop derived from them.err_horz > 0anderr_vert > 0: 95% CI bounds converted to 1-sigma variance (divide by 1.96), used as a diagonal covariance.hdop > 0andvdop > 0: receiver-native DOP values used directly in the noise model (sigma_xy = base_noise_xy * hdop).- Defaults (
hdop=1.5, vdop=2.0).
Upgrading from an older config
If your YAML has ukf.q_orientation: 0.01, change it to 1.0e-9 or delete the line. The old value corrupts quaternion math at typical IMU rates and causes yaw drift and Z-axis rise in simulation.