FusionCore

https://manankharwar.github.io/fusioncore
Admin
ROS 2 UKF sensor fusion for robots that run in the real world. IMU + wheel encoders + GPS at 100 Hz....
Tokens:34,566
Snippets:392
Trust Score:9.7
Update:4 months ago
Show doc for...
Tokens:
Raw
### Clearpath Husky Example with Topic Remapping

Source: https://manankharwar.github.io/fusioncore/migration_from_robot_localization

This example demonstrates how to launch FusionCore for a Clearpath Husky, specifying custom topics for wheel odometry and GPS fix.

```bash
ros2 launch fusioncore_ros fusioncore_nav2.launch.py \
  fusioncore_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/clearpath_husky.yaml \
  --ros-args \
  -r /odom/wheels:=/husky_velocity_controller/odom \
  -r /gnss/fix:=/fix

```

--------------------------------

### Launch FusionCore Simulation

Source: https://manankharwar.github.io/fusioncore/simulation

Build the workspace and launch the FusionCore simulation. Ensure you source the correct setup files.

```bash
cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash
colcon build
source install/setup.bash
ros2 launch fusioncore_gazebo fusioncore_gazebo.launch.py

```

--------------------------------

### Install FusionCore on ROS 2 Jazzy

Source: https://manankharwar.github.io/fusioncore/getting-started

Installs FusionCore by cloning the repository, setting up the workspace, and building the packages. Ensure you are in your ROS 2 workspace root.

```bash
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
git clone https://github.com/manankharwar/fusioncore.git
cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash
rosdep install --from-paths src --ignore-src -r -y
colcon build --packages-up-to fusioncore_ros
source install/setup.bash
```

--------------------------------

### Launch slam_toolbox for Indoor Navigation

Source: https://manankharwar.github.io/fusioncore/nav2

Starts slam_toolbox for map to odom transformation, typically used alongside FusionCore for indoor navigation with a static map.

```bash
# slam_toolbox for map → odom
ros2 launch slam_toolbox online_async_launch.py

```

--------------------------------

### Install ROS-Gazebo Bridge

Source: https://manankharwar.github.io/fusioncore/simulation

Install the necessary ROS-Gazebo bridge package. This is a prerequisite for running the simulation.

```bash
sudo apt install ros-jazzy-ros-gz

```

--------------------------------

### Install FusionCore for ROS 2

Source: https://manankharwar.github.io/fusioncore

Installs FusionCore for ROS 2 Jazzy or Humble. Ensure you have the correct ROS 2 distribution sourced before running.

```bash
mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
git clone https://github.com/manankharwar/fusioncore.git
cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash  # or /opt/ros/humble/setup.bash
rosdep install --from-paths src --ignore-src -r -y
colcon build && source install/setup.bash

```

--------------------------------

### Quick Demo with FusionCore

Source: https://manankharwar.github.io/fusioncore

Clones the repository, installs dependencies, and runs a quick demo script to visualize sensor fusion. This demo showcases GPS spike rejection using sample data.

```bash
git clone https://github.com/manankharwar/fusioncore && cd fusioncore
pip install numpy matplotlib
python3 tools/demo_quick.py --open

```

--------------------------------

### Skip Gazebo Package Installation

Source: https://manankharwar.github.io/fusioncore

Use this command to skip the Gazebo package during the build process, useful for headless or Raspberry Pi installations.

```bash
touch ~/ros2_ws/src/fusioncore/fusioncore_gazebo/COLCON_IGNORE

```

--------------------------------

### Launch FusionCore and Nav2 for Outdoor GPS Navigation

Source: https://manankharwar.github.io/fusioncore/nav2

Starts the entire stack including FusionCore and Nav2 with automatic lifecycle management. Assumes default Nav2 parameters for outdoor GPS navigation.

```bash
ros2 launch fusioncore_ros fusioncore_nav2.launch.py \
  fusioncore_config:=/path/to/your_robot.yaml

```

--------------------------------

### Build and Install FusionCore ROS 2 Package

Source: https://manankharwar.github.io/fusioncore/blog/fusioncore-ros2-sensor-fusion-launch

This snippet outlines the steps to clone the FusionCore repository, install dependencies, and build the package within a ROS 2 workspace. Ensure you have ROS 2 Jazzy sourced before running these commands.

```bash
mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
git clone https://github.com/manankharwar/fusioncore.git
cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash
rosdep install --from-paths src --ignore-src -r -y
colcon build --packages-up-to fusioncore_ros
source install/setup.bash
```

--------------------------------

### Running the TurtleBot3 Simulation with FusionCore and ICP

Source: https://manankharwar.github.io/fusioncore/blog/rtabmap-fusioncore-icp-loop

Commands to install necessary ROS 2 packages and launch the TurtleBot3 simulation demonstrating the FusionCore and icp_odometry feedback loop.

```bash
sudo apt install ros-jazzy-fusioncore-ros ros-jazzy-rtabmap-ros \
                 ros-jazzy-turtlebot3-gazebo ros-jazzy-nav2-bringup
export TURTLEBOT3_MODEL=waffle
ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py

```

--------------------------------

### F1/10 Indoor ROS 2 Launch Configuration

Source: https://manankharwar.github.io/fusioncore/hardware/ackermann

Launches FusionCore for an F1/10 indoor setup using VESC wheel encoder and RealSense D435i IMU. Remaps topics for odometry and IMU data.

```bash
ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/f1tenth_indoor.yaml \
  --ros-args \
  -r /odom/wheels:=/vesc/odom \
  -r /imu/data:=/camera/imu
```

--------------------------------

### Example Integration Test Output

Source: https://manankharwar.github.io/fusioncore/simulation

Expected output from the automated integration tests, showing the results of each test case.

```text
══════════════════════════════════════════════════
  FUSIONCORE INTEGRATION TEST SCORECARD
══════════════════════════════════════════════════
  [PASS] ✓ IMU dead reckoning
           0.041m drift in 10s stationary
  [PASS] ✓ Outlier rejection
           0.312m jump on 500m GPS outlier
  [PASS] ✓ GPS correction
           0.018m drift after stop with GPS active
  [PASS] ✓ Circle return
           0.247m from start after full circle
══════════════════════════════════════════════════
  Overall: ALL TESTS PASSED ✓
══════════════════════════════════════════════════


```

--------------------------------

### Launch FusionCore for Indoor Navigation

Source: https://manankharwar.github.io/fusioncore/nav2

Starts FusionCore for odometry without GPS. This is used for indoor navigation scenarios where GPS is unavailable.

```bash
# FusionCore for odometry
ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=your_robot.yaml

```

--------------------------------

### Primary IMU Remapping Example

Source: https://manankharwar.github.io/fusioncore/hardware/ackermann

Remaps the primary IMU data topic to the D435i camera's IMU topic, with the VESC IMU configured as the secondary.

```bash
-r /imu/data:=/camera/imu   # D435i as primary
# then in config: imu2.topic: "/vesc/imu"
```

--------------------------------

### Run FusionCore Quick Test Script

Source: https://manankharwar.github.io/fusioncore/getting-started

Executes a comprehensive test script that starts FusionCore with fake sensors, runs through the lifecycle, and verifies key outputs. This is a single command to quickly confirm functionality.

```bash
bash tools/quick_test.sh
```

--------------------------------

### Configure FusionCore Startup Behavior

Source: https://manankharwar.github.io/fusioncore/troubleshooting

Ensure FusionCore waits for VSLAM initialization before starting to prevent the first VSLAM message from being dropped. Use `init.wait_for_all_sensors` and `init.sensor_wait_timeout` to manage this.

```yaml
init.wait_for_all_sensors: true
init.sensor_wait_timeout: 10.0
```

--------------------------------

### Robot Localization Node Wiring

Source: https://manankharwar.github.io/fusioncore/migration_from_robot_localization

Illustrates the typical node connections in a robot_localization setup, involving ekf_node and navsat_transform_node.

```text
/imu/data ──────────────────────────────────────────────────┐
/odom/wheels ──────→ ekf_node → /odometry/filtered          │
/odometry/filtered ─→ navsat_transform_node → GPS in ENU ───┘
/fix ──────────────→ navsat_transform_node

```

--------------------------------

### Updating Nav2 Configuration for FusionCore

Source: https://manankharwar.github.io/fusioncore/migration_from_robot_localization

These are examples of how to update the 'odom_topic' in Nav2's configuration files to point to FusionCore's output topic '/fusion/odom'. This is necessary if you are not using the bundled nav2_params.yaml.

```yaml
bt_navigator:
  ros__parameters:
    odom_topic: /fusion/odom        # was /odometry/filtered

velocity_smoother:
  ros__parameters:
    odom_topic: /fusion/odom        # was /odometry/filtered

```

--------------------------------

### Configure Sensor Synchronization

Source: https://manankharwar.github.io/fusioncore/how-it-works

Set 'init.wait_for_all_sensors' to true to ensure the filter waits for all configured sensors before starting. This prevents initial drift. A timeout can be configured with 'init.sensor_wait_timeout'.

```yaml
init.wait_for_all_sensors: true
init.sensor_wait_timeout: 10.0   # give up and start anyway after this many seconds

```

--------------------------------

### Add FusionCore as a Lifecycle Node

Source: https://manankharwar.github.io/fusioncore/migration_from_robot_localization

Example of how to add FusionCore as a lifecycle node in a custom ROS 2 launch file using Python. This requires configuring and activating the node after launch.

```python
from launch_ros.actions import LifecycleNode

fusioncore_node = LifecycleNode(
    package="fusioncore_ros",
    executable="fusioncore_node",
    name="fusioncore",
    namespace="",
    output="screen",
    parameters=["/path/to/fusioncore.yaml"],
)

```

--------------------------------

### GNSS Fix Rejection Log Example

Source: https://manankharwar.github.io/fusioncore/how-it-works

An example log message indicating that a GNSS fix was rejected due to quality checks or Mahalanobis gating, showing the rejected fix type and minimum required type.

```log
[WARN] GNSS fix rejected (fix_type=1, min=4, hdop=1.20, quality check or Mahalanobis gate)
```

--------------------------------

### Enable Sensor Wait for Initialization

Source: https://manankharwar.github.io/fusioncore/configuration

Replace `sleep()` in launch files with `init.wait_for_all_sensors: true`. FusionCore waits for at least one message from each configured sensor before starting, ensuring all sensors are online. A timeout prevents indefinite hangs.

```yaml
init.wait_for_all_sensors: true
init.sensor_wait_timeout: 10.0
```

```log
[WARN] Sensor wait timed out after 10.0s. Missing: [GNSS]. Starting anyway.
```

--------------------------------

### Van/Large Vehicle Outdoor ROS 2 Launch Configuration

Source: https://manankharwar.github.io/fusioncore/hardware/ackermann

Launches FusionCore for a van or large vehicle outdoor setup using ZED 2i, wheel encoder, and GPS. Remaps topics for odometry, IMU, and GPS fix.

```bash
ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/van_outdoor_gps.yaml \
  --ros-args \
  -r /odom/wheels:=/your/wheel/odom \
  -r /imu/data:=/zed/zed_node/imu/data \
  -r /gnss/fix:=/your/gps/fix
```

--------------------------------

### Configure GPS without Initial Fix

Source: https://manankharwar.github.io/fusioncore/configuration

Set `reference.use_first_fix: false` to allow FusionCore to start using IMU and wheel odometry even without a GPS fix. This is useful for indoor environments or when GPS is unreliable. No GPS topics are strictly required in this mode.

```yaml
reference.use_first_fix: false
```

--------------------------------

### VSLAM Reinitialization Warning Log

Source: https://manankharwar.github.io/fusioncore/hardware/vslam-imu

Example log message indicating that FusionCore has detected a VSLAM reinitialization event. This occurs after a configurable number of consecutive outlier rejections.

```text
[WARN] VSLAM: 10 consecutive rejections: reinitialization detected. Re-anchoring map origin.

```

--------------------------------

### Launch FusionCore for Indoor Wheels

Source: https://manankharwar.github.io/fusioncore/hardware/wheels-indoor

Launches FusionCore with a configuration file for indoor wheel odometry. Remap the wheel odometry topic if it differs from the default.

```bash
ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/wheels_indoor.yaml \
  --ros-args \
  -r /odom/wheels:=/your/wheel/odom/topic

```

--------------------------------

### Launch FusionCore and Nav2 with Custom Nav2 Parameters

Source: https://manankharwar.github.io/fusioncore/nav2

Launches FusionCore and Nav2, allowing you to specify a custom Nav2 parameter file.

```bash
ros2 launch fusioncore_ros fusioncore_nav2.launch.py \
  fusioncore_config:=your_robot.yaml \
  nav2_params:=/path/to/your_nav2_params.yaml

```

--------------------------------

### Launch FusionCore with a specific configuration file

Source: https://manankharwar.github.io/fusioncore/hardware

Launches the FusionCore ROS 2 node using a specified hardware configuration file. Ensure the path to the config file is correct.

```bash
ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/wheels_indoor.yaml
```

--------------------------------

### Build FusionCore Packages

Source: https://manankharwar.github.io/fusioncore/reference/benchmark

Builds the necessary FusionCore packages for benchmarking. Run this command before executing benchmarks.

```bash
colcon build --packages-select fusioncore_core fusioncore_ros fusioncore_datasets
```

--------------------------------

### Launching FusionCore with Bundled Nav2 Parameters

Source: https://manankharwar.github.io/fusioncore/migration_from_robot_localization

This command launches FusionCore and Nav2 using the default bundled nav2_params.yaml, which is pre-configured for outdoor GPS navigation and uses '/fusion/odom' as the odometry topic.

```bash
# This already uses the bundled nav2_params.yaml: nothing else needed
ros2 launch fusioncore_ros fusioncore_nav2.launch.py \
  fusioncore_config:=your_robot.yaml

```

--------------------------------

### Launch FusionCore and SLAM

Source: https://manankharwar.github.io/fusioncore/hardware/wheels-indoor

Launches FusionCore for odometry and a SLAM system (e.g., slam_toolbox) for localization. Ensure topics are correctly remapped.

```bash
# FusionCore (odometry)
ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=wheels_indoor.yaml \
  --ros-args -r /odom/wheels:=/diff_controller/odom

# SLAM
ros2 launch slam_toolbox online_async_launch.py

```

--------------------------------

### Launch FusionCore with topic remappings

Source: https://manankharwar.github.io/fusioncore/hardware

Launches FusionCore with a specific configuration and remaps default ROS 2 topics to match your sensor driver outputs. Use this when your sensor topics differ from FusionCore's expected topics.

```bash
ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/wheels_indoor.yaml \
  --ros-args \
  -r /odom/wheels:=/diff_controller/odom \
  -r /imu/data:=/imu
```

--------------------------------

### Launch FusionCore for Manual Verification

Source: https://manankharwar.github.io/fusioncore/getting-started

Launches FusionCore using its default launch file. This is the first step for manual inspection of the filter's behavior.

```bash
source /opt/ros/jazzy/setup.bash && source ~/ros2_ws/install/setup.bash
ros2 launch fusioncore_ros fusioncore.launch.py
```

--------------------------------

### Launch FusionCore with an Environment Preset

Source: https://manankharwar.github.io/fusioncore/hardware/environment-presets

Launches the FusionCore navigation stack using a specified robot configuration and an environment preset. The preset overrides GPS noise parameters while other hardware configurations remain unchanged. Use this to adapt GPS performance to environments like urban streets with multipath.

```bash
ros2 launch fusioncore_ros fusioncore_nav2.launch.py \
  fusioncore_config:=your_robot.yaml \
  env_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/env_urban.yaml

```

--------------------------------

### Launch FusionCore with Nav2

Source: https://manankharwar.github.io/fusioncore/getting-started

Launches FusionCore integrated with Nav2, requiring a robot configuration file.

```bash
ros2 launch fusioncore_ros fusioncore_nav2.launch.py \
  fusioncore_config:=/path/to/your_robot.yaml
```

--------------------------------

### Launch FusionCore with ICP Indoor Configuration

Source: https://manankharwar.github.io/fusioncore/hardware/icp-indoor

Launches FusionCore using the ICP indoor configuration and remaps wheel odometry to the ICP pipeline's output topic. Replace `/kiss/odometry` with your specific ICP topic.

```bash
ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/icp_indoor.yaml \
  --ros-args \
  -r /odom/wheels:=/kiss/odometry

```

--------------------------------

### Manually Configure and Activate FusionCore Node

Source: https://manankharwar.github.io/fusioncore/getting-started

Manually sets the FusionCore node to the 'configure' state and then to the 'activate' state. This is an alternative to using the launch file for lifecycle management.

```bash
ros2 lifecycle set /fusioncore configure
ros2 lifecycle set /fusioncore activate
```

--------------------------------

### SLAM Integration: FusionCore and slam_toolbox

Source: https://manankharwar.github.io/fusioncore/hardware/icp-indoor

Launches FusionCore for odometry and slam_toolbox for SLAM in separate terminals. FusionCore provides the base_link to odom TF, while slam_toolbox handles the map to odom TF.

```bash
# Terminal 1: FusionCore
ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=icp_indoor.yaml \
  --ros-args -r /odom/wheels:=/kiss/odometry

# Terminal 2: slam_toolbox
ros2 launch slam_toolbox online_async_launch.py

```

--------------------------------

### Build and Test FusionCore Core Package

Source: https://manankharwar.github.io/fusioncore/getting-started

Builds only the fusioncore_core package with testing enabled and then runs the tests. Finally, it displays the test results verbosely.

```bash
cd ~/ros2_ws
colcon build --packages-select fusioncore_core --cmake-args -DBUILD_TESTING=ON
colcon test --packages-select fusioncore_core
colcon test-result --verbose
```

--------------------------------

### Configure IMU Startup Bias Initialization

Source: https://manankharwar.github.io/fusioncore/hardware/wheels-indoor

Configuration snippet for initializing IMU bias by keeping the robot stationary for a specified window at startup. This reduces initial drift.

```yaml
init.stationary_window: 2.0

```

--------------------------------

### Launch FusionCore without Nav2

Source: https://manankharwar.github.io/fusioncore/getting-started

Launches FusionCore as a standalone node, requiring a robot configuration file.

```bash
ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=/path/to/your_robot.yaml
```

--------------------------------

### Launch FusionCore with VSLAM + IMU Configuration

Source: https://manankharwar.github.io/fusioncore/hardware/vslam-imu

Launches the FusionCore ROS 2 node using a specific configuration file for VSLAM and IMU integration. Ensure the `vslam.topic` is correctly set to your VSLAM's odometry output.

```bash
ros2 launch fusioncore_ros fusioncore.launch.py \
  fusioncore_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/vslam_imu.yaml

```

--------------------------------

### Run FusionCore Quick Test Script in Docker

Source: https://manankharwar.github.io/fusioncore/getting-started

Executes the FusionCore quick test script within a Docker container. This is useful for testing in an isolated environment.

```bash
docker run --rm -it ghcr.io/manankharwar/fusioncore:latest bash tools/quick_test.sh
```

--------------------------------

### FusionCore Package Structure

Source: https://manankharwar.github.io/fusioncore/reference/architecture

Illustrates the directory layout and primary components of the FusionCore project, highlighting the separation of the core C++ library, ROS 2 wrapper, and Gazebo simulation.

```tree
fusioncore/
├── fusioncore_core/              # Pure C++17 math library. Zero ROS dependency.
│   ├── include/fusioncore/
│   │   ├── ukf.hpp               # Unscented Kalman Filter: 47 sigma points
│   │   ├── state.hpp             # 23-dimensional state vector (quaternion orientation)
│   │   ├── fusioncore.hpp        # Public API: FusionCore, FusionCoreConfig
│   │   └── sensors/
│   │       ├── imu.hpp           # Raw IMU + orientation measurement models
│   │       ├── encoder.hpp       # Wheel encoder measurement model
│   │       └── gnss.hpp          # GPS: ECEF, lever arm, covariance, quality gating
│   └── src/
│       ├── ukf.cpp               # UKF: sigma points, predict, update
│       └── fusioncore.cpp        # Manager: outlier rejection, adaptive noise,
│                                 #          snapshots, observability, delay compensation
├── fusioncore_ros/               # ROS 2 Jazzy wrapper
│   ├── src/fusion_node.cpp       # Lifecycle node: sensor callbacks, TF validation,
│   │                             #   ZUPT, diagnostics, /fusion/pose, reset service
│   ├── config/fusioncore.yaml    # Default configuration
│   └── launch/
│       ├── fusioncore.launch.py
│       └── fusioncore_nav2.launch.py
└── fusioncore_gazebo/            # Simulation world
    ├── worlds/fusioncore_test.sdf
    └── launch/
        ├── fusioncore_gazebo.launch.py
        └── integration_test.py

```

--------------------------------

### Run All Benchmark Sequences

Source: https://manankharwar.github.io/fusioncore/reference/benchmark

Sequentially runs all 12 benchmark sequences. This process is expected to take 6-8 hours to complete.

```bash
bash benchmarks/run_all.sh
```

--------------------------------

### Minimal FusionCore Robot Configuration Template

Source: https://manankharwar.github.io/fusioncore/migration_from_robot_localization

This is a basic YAML configuration file for FusionCore. It defines essential parameters for robot odometry, IMU, GNSS, and UKF settings. Ensure 'base_frame' matches your robot's TF frame.

```yaml
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   # set true for ground robots

    imu.has_magnetometer: false
    imu.gyro_noise: 0.005    # rad/s: check your IMU datasheet
    imu.accel_noise: 0.1     # m/s²
    imu.remove_gravitational_acceleration: false  # see gravity note below

    encoder.vel_noise: 0.05  # m/s
    encoder.yaw_noise: 0.02  # rad/s

    gnss.base_noise_xy: 2.5  # m: standard GPS. Use 0.5 for RTK float, 0.015 for RTK fixed
    gnss.base_noise_z: 5.0
    gnss.max_hdop: 4.0
    gnss.min_satellites: 4
    gnss.min_fix_type: 1     # 1=GPS, 2=DGPS/RTK_FIXED, 4=RTK_FIXED only
                             # WARNING: sensor_msgs/NavSatFix has no RTK_FLOAT status.
                             # Setting min_fix_type: 3 will silently starve the filter. Use 2 or 4.

    outlier_rejection: true
    outlier_threshold_gnss: 16.27
    outlier_threshold_imu: 15.09
    outlier_threshold_enc: 11.34
    outlier_threshold_hdg: 10.83

    adaptive.imu: true
    adaptive.encoder: true
    adaptive.gnss: true
    adaptive.window: 50
    adaptive.alpha: 0.01

    ukf.q_position: 0.01
    ukf.q_orientation: 1.0e-9
    ukf.q_velocity: 0.1
    ukf.q_angular_vel: 0.1
    ukf.q_acceleration: 1.0
    ukf.q_gyro_bias: 1.0e-5
    ukf.q_accel_bias: 1.0e-5

    input.gnss_crs: "EPSG:4326"
    output.crs: "EPSG:4978"
    output.convert_to_enu_at_reference: true
    reference.use_first_fix: true   # map origin = first GPS fix

```

--------------------------------

### FusionCore VSLAM-IMU Configuration

Source: https://manankharwar.github.io/fusioncore/hardware/vslam-imu

This YAML snippet shows the complete configuration for FusionCore, including parameters for base frame, odometry, IMU, VSLAM, outlier rejection, adaptive filtering, UKF settings, initialization, motion model, and coordinate reference systems.

```yaml
fusioncore:
  ros__parameters:
    base_frame: base_link
    odom_frame: odom
    publish_rate: 100.0
    publish.force_2d: true        # set false for aerial platforms

    imu.has_magnetometer: true    # VectorNav, Xsens, BNO08x
    imu.gyro_noise: 0.005         # rad/s
    imu.accel_noise: 0.1          # m/s²
    imu.remove_gravitational_acceleration: false

    vslam.topic: "/vslam/odometry"
    vslam.position_noise: 0.1
    vslam.orientation_noise: 0.02

    outlier_rejection: true
    outlier_threshold_vslam: 22.46

    adaptive.imu: true
    adaptive.encoder: false
    adaptive.gnss: false
    adaptive.window: 50
    adaptive.alpha: 0.01

    ukf.q_position: 0.01
    ukf.q_orientation: 1.0e-9
    ukf.q_velocity: 0.1
    ukf.q_angular_vel: 0.1
    ukf.q_acceleration: 1.0
    ukf.q_gyro_bias: 1.0e-5
    ukf.q_accel_bias: 1.0e-5

    init.stationary_window: 2.0
    init.wait_for_all_sensors: true
    init.sensor_wait_timeout: 10.0

    motion_model: "ConstantVelocityAcceleration"

    reference.use_first_fix: false
    reference.x: 0.0
    reference.y: 0.0
    reference.z: 0.0
    input.gnss_crs: "EPSG:4326"
    output.crs: "EPSG:4978"
    output.convert_to_enu_at_reference: true

```

--------------------------------

### Check Camera Topic Heartbeat

Source: https://manankharwar.github.io/fusioncore/troubleshooting

Use these commands to verify that your camera topics are publishing data. If a topic is silent, the 'rgbd_sync' node may not be synchronizing correctly.

```bash
ros2 topic hz /right/image_rect
ros2 topic hz /stereo/depth
ros2 topic hz /rgbd_image

```

--------------------------------

### FusionCore Base Configuration

Source: https://manankharwar.github.io/fusioncore/configuration

Configure the base frame, odometry frame, publish rate, and options for publishing 2D data and TF transforms.

```yaml
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.
```

--------------------------------

### Run a Single Benchmark Sequence

Source: https://manankharwar.github.io/fusioncore/reference/benchmark

Executes a single benchmark sequence. The process stops automatically upon completion. Playback speed can be adjusted.

```bash
bash benchmarks/run_one.sh 2012-01-08
```

--------------------------------

### FusionCore and SLAM TF Tree Structure

Source: https://manankharwar.github.io/fusioncore/hardware/wheels-indoor

Illustrates the typical TF tree structure when using FusionCore for odometry and a SLAM system for global localization.

```text
map → odom        ← slam_toolbox or RTABMAP
odom → base_link  ← FusionCore

```

--------------------------------

### FusionCore Adaptive Noise Modeling

Source: https://manankharwar.github.io/fusioncore/vs-robot-localization

FusionCore adapts its noise model in real-time using an innovation window. A floor prevents collapse, and the chi-squared gate remains calibrated to actual error levels.

```python
R \u2190 (1 - \u03b1)R + \u03b1\u00b7 \u0108

where \u0108 is the empirical innovation covariance and \u03b1 = 0.01. A floor prevents collapse. The chi-squared gate stays calibrated to actual error levels regardless of what the driver reports.
```

--------------------------------

### Manually Publish TF Transforms and Control Lifecycle

Source: https://manankharwar.github.io/fusioncore/getting-started

Manually publishes static transforms for 'base_link' to 'imu_link' and 'odom' to 'base_link', then configures and activates the FusionCore node. This is part of the manual verification process.

```bash
source /opt/ros/jazzy/setup.bash && source ~/ros2_ws/install/setup.bash
ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id imu_link &
ros2 run tf2_ros static_transform_publisher --frame-id odom --child-frame-id base_link &
sleep 1 && ros2 lifecycle set /fusioncore configure
sleep 1 && ros2 lifecycle set /fusioncore activate
```

--------------------------------

### Remap Topics for FusionCore Launch

Source: https://manankharwar.github.io/fusioncore/migration_from_robot_localization

Add these remaps to your ros2 launch command if your wheel odometry or GPS topics differ from FusionCore's defaults. This ensures FusionCore subscribes to the correct topics.

```bash
ros2 launch fusioncore_ros fusioncore_nav2.launch.py \
  fusioncore_config:=your_robot.yaml \
  --ros-args \
  -r /odom/wheels:=/your/wheel/odom/topic \
  -r /gnss/fix:=/fix

```

--------------------------------

### Optional Visual Odometry Topic Configuration

Source: https://manankharwar.github.io/fusioncore/hardware/ackermann

Configure an optional second visual odometry source, such as KISS-ICP or RealSense T265. Requires a nav_msgs/Odometry topic with velocity in the twist field.

```yaml
encoder2.topic: "/kiss/odometry"         # KISS-ICP on LiDAR
# or
encoder2.topic: "/camera/odom/sample"    # RealSense T265 visual odometry
```

--------------------------------

### Add Startup Delay for RTABMAP SLAM Node

Source: https://manankharwar.github.io/fusioncore/hardware/icp-indoor

Introduce a startup delay for the RTABMAP SLAM node when TF frames are published from a separate machine to prevent TF tree connection errors.

```python
TimerAction(
    period=10.0,
    actions=[Node(
        package='rtabmap_slam',
        executable='rtabmap',
        output='screen',
        parameters=[...],
        remappings=[...],
    )]
),
```

--------------------------------

### Run Automated Integration Tests

Source: https://manankharwar.github.io/fusioncore/simulation

Execute the automated integration tests for the FusionCore simulation. These tests verify various aspects of the fusion pipeline.

```python
python3 ~/ros2_ws/src/fusioncore/fusioncore_gazebo/launch/integration_test.py

```

--------------------------------

### Check Wheel Odometry at Rest

Source: https://manankharwar.github.io/fusioncore/troubleshooting

Verify the encoder's reported velocity at standstill. Non-zero values can cause the 'init.stationary_window' to abort.

```bash
ros2 topic echo /odom/wheels --field twist.twist.linear --once

```

--------------------------------

### Configure Checkpoint File Path

Source: https://manankharwar.github.io/fusioncore/how-it-works

This configuration parameter allows you to specify the file path where FusionCore's state checkpoints will be saved or loaded from. The default path is '/tmp/fusioncore_checkpoint.txt'.

```yaml
replay.checkpoint_path: "/tmp/fusioncore_checkpoint.txt"   # default
```

--------------------------------

### Configure LiDAR ICP as Secondary Odometry Source

Source: https://manankharwar.github.io/fusioncore/hardware/wheels-indoor

Configuration snippet for enabling LiDAR ICP as a secondary odometry source in FusionCore. Uncomment the desired topic.

```yaml
encoder2.topic: "/kiss/odometry"    # KISS-ICP
# encoder2.topic: "/icp_odom"       # rtabmap icp_odometry

```

--------------------------------

### Ignore Gazebo package for Headless Machines

Source: https://manankharwar.github.io/fusioncore/getting-started

Creates a COLCON_IGNORE file to prevent the Gazebo package from building on headless machines.

```bash
touch ~/ros2_ws/src/fusioncore/fusioncore_gazebo/COLCON_IGNORE
```

--------------------------------

### Verify FusionCore Odometry Publishing Rate

Source: https://manankharwar.github.io/fusioncore/getting-started

Checks the publishing rate of the /fusion/odom topic to ensure FusionCore is publishing odometry data as expected.

```bash
ros2 topic hz /fusion/odom
```

--------------------------------

### Configure FusionCore for Secondary ICP Odometry

Source: https://manankharwar.github.io/fusioncore/hardware/icp-indoor

Enables FusionCore to use ICP odometry as a secondary source by specifying its topic in the configuration file. FusionCore will then subscribe to both primary wheel odometry and this secondary ICP source.

```yaml
# in icp_indoor.yaml
encoder2.topic: "/kiss/odometry"

```

--------------------------------

### FusionCore Wheel Encoder Configuration

Source: https://manankharwar.github.io/fusioncore/configuration

Configure noise parameters for wheel encoders and an optional second velocity source like lidar or visual odometry. FusionCore requires nav_msgs/Odometry messages for the second source.

```yaml
    # ── Wheel encoders ────────────────────────────────────────────────────────
    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
```

--------------------------------

### Update odom_topic in Nav2 Parameters

Source: https://manankharwar.github.io/fusioncore/nav2

Demonstrates how to update the 'odom_topic' parameter in Nav2 configuration files (e.g., bt_navigator, velocity_smoother) to use FusionCore's output.

```yaml
# before
bt_navigator:
  ros__parameters:
    odom_topic: /odometry/filtered

velocity_smoother:
  ros__parameters:
    odom_topic: /odometry/filtered

# after
bt_navigator:
  ros__parameters:
    odom_topic: /fusion/odom

velocity_smoother:
  ros__parameters:
    odom_topic: /fusion/odom

```

--------------------------------

### Fix Missing Base Link to Odom TF Transform

Source: https://manankharwar.github.io/fusioncore/how-it-works

During startup, FusionCore validates TF transforms. If the base_link to odom transform is missing, this command provides the fix.

```bash
--- TF Validation ---
  [OK]      imu_link -> base_link
  [MISSING] base_link -> odom
  Fix: ros2 run tf2_ros static_transform_publisher --frame-id odom --child-frame-id base_link
---------------------

```

--------------------------------

### Enable Ackermann Motion Model

Source: https://manankharwar.github.io/fusioncore/how-it-works

Configure the 'motion_model' parameter to 'Ackermann' for car-like platforms. Optional 'motion_model_params' can specify parameters like wheelbase.

```yaml
motion_model: "Ackermann"
motion_model_params:
  wheelbase: 0.55    # meters

```

--------------------------------

### Configure Secondary IMU Topic

Source: https://manankharwar.github.io/fusioncore/hardware/ackermann

Set the topic for a secondary IMU sensor. FusionCore will treat it as an independent measurement of the same state as the primary IMU.

```yaml
imu2.topic: "/vesc/imu"
imu2.frame_id: ""                            # or override if driver uses a non-standard frame
imu2.remove_gravitational_acceleration: false
```

--------------------------------

### Monitor TF Publisher for Odom to BaseLink

Source: https://manankharwar.github.io/fusioncore/troubleshooting

Use tf2_monitor to identify all broadcasters publishing the `odom` to `base_link` transform. Ensure only FusionCore is publishing this transform to avoid conflicts.

```bash
ros2 run tf2_ros tf2_monitor odom base_link
```

--------------------------------

### Call fromLL Service for GPS Waypoint Navigation

Source: https://manankharwar.github.io/fusioncore/nav2

Converts latitude and longitude to the local map frame for Nav2 using the fromLL service. Ensure FusionCore has a GPS fix before calling.

```bash
ros2 service call /fromLL fusioncore_ros/srv/FromLL \
  "{ll_point: {latitude: 43.2557, longitude: -79.8711, altitude: 0.0}}"

```

--------------------------------

### Gravity Removal Parameter Comparison

Source: https://manankharwar.github.io/fusioncore/migration_from_robot_localization

This snippet highlights the difference in the 'remove_gravitational_acceleration' parameter between robot_localization and FusionCore. Setting both to 'true' describes the same physical situation: the driver has already removed gravity.

```yaml
# robot_localization:
imu0_remove_gravitational_acceleration: true
# means: "remove gravity before I fuse it"

# FusionCore:
imu.remove_gravitational_acceleration: true
# means: "my driver already removed gravity, add it back"

```

--------------------------------

### Heading Input Configuration

Source: https://manankharwar.github.io/fusioncore/configuration

Specify the topics for GPS heading (sensor_msgs/Imu) or azimuth (compass_msgs/Azimuth) measurements.

```yaml
# 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)
```

--------------------------------

### FusionCore GPS Configuration

Source: https://manankharwar.github.io/fusioncore/configuration

Configure baseline noise for XY and Z positions, heading noise, and set thresholds for HDOP, minimum satellites, and minimum fix type for GPS measurements.

```yaml
    # ── GPS ───────────────────────────────────────────────────────────────────
    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

    gnss.max_hdop: 4.0          # reject fixes with HDOP worse than this
    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.
```

--------------------------------

### Check FusionCore Lifecycle Status

Source: https://manankharwar.github.io/fusioncore/troubleshooting

Verify if the FusionCore lifecycle node is active. If it's unconfigured or inactive, manually transition it to the configured and active states.

```bash
ros2 lifecycle get /fusioncore
```

```bash
ros2 lifecycle set /fusioncore configure
ros2 lifecycle set /fusioncore activate
```

--------------------------------

### Configure VSLAM Topic for FusionCore

Source: https://manankharwar.github.io/fusioncore/hardware/vslam-imu

Sets the ROS 2 topic where FusionCore should subscribe to VSLAM odometry messages. Adjust this path to match your specific VSLAM node's output topic.

```yaml
vslam.topic: "/orbslam3/camera/odometry"  # adjust to your node's topic

```

--------------------------------

### Save and Load Filter State for Debugging

Source: https://manankharwar.github.io/fusioncore/configuration

Use `save_checkpoint` and `load_checkpoint` services during bag replay for deterministic debugging. This allows restoring the filter state instantly without replaying from the beginning. The checkpoint file path is configurable.

```bash
# Save filter state at any point during bag replay
ros2 service call /fusioncore/save_checkpoint std_srvs/srv/Trigger
```

```bash
# Restore that state instantly (no need to replay from the beginning)
ros2 service call /fusioncore/load_checkpoint std_srvs/srv/Trigger
```

--------------------------------

### Configure RTABMAP to Use ICP Odometry Directly

Source: https://manankharwar.github.io/fusioncore/troubleshooting

Wire RTABMAP's SLAM node to subscribe to '/icp_odom' instead of FusionCore's '/fusion/odom'. This prevents corrupted maps when ICP odometry fails temporarily.

```python
Node(
    package='rtabmap_slam',
    executable='rtabmap',
    output='screen',
    parameters=[{
        'frame_id': 'base_link',
        'subscribe_scan': True,
        'approx_sync': True,
    }],
    remappings=[
        ('odom', '/icp_odom'),
        ('imu', '/imu/data'),
    ]
),

```

--------------------------------

### FusionCore IMU Configuration

Source: https://manankharwar.github.io/fusioncore/configuration

Set IMU sensor noise parameters, specify if a magnetometer is present, and configure gravitational acceleration removal. Also includes options for overriding the IMU frame ID and using a second IMU.

```yaml
    # ── 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
```

--------------------------------

### Remap Wheel Odometry to ZED Visual Odometry

Source: https://manankharwar.github.io/fusioncore/hardware/ackermann

If wheel encoders are not available, remap the wheel odometry topic to the ZED visual odometry topic. This makes the ZED odometry the sole velocity source.

```bash
-r /odom/wheels:=/zed/zed_node/odom
```

--------------------------------

### Check VSLAM Topic Configuration

Source: https://manankharwar.github.io/fusioncore/troubleshooting

Verify that the VSLAM topic is correctly set in FusionCore's parameters. If the topic is empty, FusionCore is not subscribed to VSLAM data. Set the topic in your YAML configuration.

```bash
ros2 param get /fusioncore vslam.topic
```

```bash
ros2 topic hz /vslam/odometry   # replace with your actual topic
```

```yaml
vslam.topic: "/vslam/odometry"
```

--------------------------------

### Save and Load FusionCore State Checkpoints

Source: https://manankharwar.github.io/fusioncore/how-it-works

Use these ROS 2 service calls to save the current state of the FusionCore filter to a file or load a previously saved state. This allows resuming the filter from a specific point in time.

```bash
ros2 service call /fusioncore/save_checkpoint std_srvs/srv/Trigger
ros2 service call /fusioncore/load_checkpoint std_srvs/srv/Trigger
```

--------------------------------

### Enable Differential Drive Motion Model

Source: https://manankharwar.github.io/fusioncore/how-it-works

Configure the 'motion_model' parameter to 'DifferentialDrive' to enforce non-holonomic constraints for differential drive robots during the prediction step.

```yaml
motion_model: "DifferentialDrive"

```

--------------------------------

### Check TF Transform Publication

Source: https://manankharwar.github.io/fusioncore/troubleshooting

Use this command to verify if FusionCore is publishing the 'odom' to 'base_link' transform. This is crucial for downstream nodes like Nav2.

```bash
ros2 run tf2_ros tf2_echo odom base_link

```

--------------------------------

### Configure Ackermann Motion Model

Source: https://manankharwar.github.io/fusioncore/hardware/ackermann

Set the motion model to Ackermann and specify the wheelbase in meters. This is crucial for accurate motion predictions.

```yaml
motion_model: "Ackermann"
motion_model_params.wheelbase: 0.32   # metres (see table below)
```

--------------------------------

### Drive Robot and Monitor Position

Source: https://manankharwar.github.io/fusioncore/simulation

Commands to drive the robot in a circle and monitor its fused odometry position. Use these in separate terminals.

```bash
# Terminal 2: drive in a circle
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist 
  "{linear: {x: 0.5}, angular: {z: 0.3}}" --rate 10

# Terminal 3: watch position
ros2 topic echo /fusion/odom --field pose.pose.position

```

--------------------------------

### Configure Radar Ego-Velocity Fusion

Source: https://manankharwar.github.io/fusioncore/how-it-works

Specify the topic for radar ego-velocity messages and the noise level when covariance is not provided. FusionCore expects `nav_msgs/Odometry` in the robot body frame.

```yaml
radar.velocity_topic: "/radar/ego_velocity"
radar.vel_noise: 0.1    # m/s, used when message has no covariance

```

--------------------------------

### Check for Zero Timestamps in IMU Data

Source: https://manankharwar.github.io/fusioncore/troubleshooting

Inspect the header timestamps of IMU messages to detect issues with drivers publishing zero timestamps, which can affect initialization on older FusionCore versions.

```bash
ros2 topic echo /imu/data --field header.stamp --once
```

--------------------------------

### Second GPS Receiver Configuration

Source: https://manankharwar.github.io/fusioncore/configuration

Configure the topic for a secondary GPS receiver and its lever arm offsets.

```yaml
# 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
```

--------------------------------

### Echo Raw IMU Linear Acceleration

Source: https://manankharwar.github.io/fusioncore/hardware/icp-indoor

Use this command to check the raw linear acceleration output from the IMU. If X is near 9.81 and Z is near zero when the robot is still, the IMU's orientation might be misaligned.

```bash
ros2 topic echo /imu --field linear_acceleration --once

```

--------------------------------

### Use Default Constant Velocity Acceleration Motion Model

Source: https://manankharwar.github.io/fusioncore/how-it-works

To retain the original behavior or for aerial vehicles, set 'motion_model' to 'ConstantVelocityAcceleration' or omit the line entirely.

```yaml
motion_model: "ConstantVelocityAcceleration"   # or just leave the line out

```
Tokens:
Raw