### 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"],
)

```

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

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

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

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

```

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

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

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

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

```

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

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