DOCSEARCHbyAlgolia
Submit Docs

FusionCore Unscented Kalman Filter

repository·main·Indexed 42 days ago

https://github.com/manankharwar/fusioncore

A 23-state Unscented Kalman Filter (UKF) for outdoor robots that fuses IMU, wheel encoders, GPS, and visual SLAM at 100 Hz. It features adaptive GPS noise estimation, chi-squared outlier rejection, and robust error reporting to prevent silent drift. Designed for ROS 2 Jazzy, it includes benchmarks against the robot_localization package using the NCLT dataset.

Tokens
74.7K
Snippets
191
Records
332
Agent score
56%

What's inside FusionCore

  1. Understand FusionCore NCLT Benchmark Results

    main

    FusionCore was benchmarked against robot_localization EKF using the NCLT dataset (University of Michigan North Campus Long-Term). In a comparison of 12 sequences using a single configuration file with no per-sequence tuning, FusionCore outperformed robot_localization in 10 out of 12 sequences.

    Key Performance Metrics

    • ATE (Absolute Trajectory Error): SE3-aligned to RTK GPS ground truth.
    • Drift Rate: Measured in meters per kilometer (m/km).
    • GPS Fixes: Mode-3 (3D) fixes only.
    • Max Blackout: The longest duration without GPS signal.

    Performance Tiers

    • Excellent (< 20m ATE): High GPS density, blackouts < 300s, clean data.
    • Good (20-35m ATE): Moderate GPS density, blackouts < 260s.
    • Moderate (35-65m ATE): Long blackouts (> 240s) or low GPS density.
    • Poor (> 65m ATE): Caused by adversarial GPS clusters at blackout boundaries.

  2. Benchmark results: FusionCore vs robot_localization

    main

    FusionCore was benchmarked against robot_localization EKF using the NCLT dataset (University of Michigan North Campus Long-Term). The benchmark evaluated twelve sequences across different seasons using a single configuration file without per-sequence tuning.

    Key Findings:

    • FusionCore outperformed robot_localization EKF in 10 out of 12 sequences.
    • FusionCore's performance is highly correlated with the duration of the longest GPS blackout.
    • robot_localization EKF tends to fail (high drift) when the actual GPS noise exceeds the noise reported by the sensor driver, as its gating mechanism is fixed and does not adapt to real-world noise levels.

  3. Choose a motion model for FusionCore

    main

    Select a motion model based on your robot's physical constraints to improve localization accuracy.

    • ConstantVelocityAcceleration (Default): Recommended for most platforms. Matches robot_localization behavior.
    • DifferentialDrive: Use for differential drive, skid-steer, or tracked vehicles. Switch to this if you observe lateral position drift on straight runs or if the VY state is non-zero between encoder updates.
    • Ackermann: Use for car-like, forklift, or outdoor field robots with front-wheel steering. This model stores wheelbase for future minimum-turning-radius enforcement.

    Avoid using DifferentialDrive or Ackermann for:

    • Aerial vehicles (no lateral constraints apply).
    • Holonomic/Mecanum platforms (they can move sideways intentionally).

  4. How FusionCore handles GPS spikes via Mahalanobis gating

    main

    FusionCore uses Mahalanobis distance gating (based on a $\chi^2$ test at the 99.9th percentile) to detect and reject outlier measurements. When a measurement (such as a corrupted GPS fix) is detected as a $3\sigma+$ outlier, the filter blocks the update in a single step, preventing the state from jumping significantly.

    In benchmark testing, FusionCore successfully rejected a corrupted GPS fix that was $\approx 707\text{m}$ away from the true position, resulting in a position jump of only $\approx 1\text{m}$, whereas standard EKF implementations without outlier rejection may jump by tens or hundreds of meters.

  5. Understand FusionCore's core estimation features

    main

    FusionCore achieves higher stability and accuracy compared to standard EKF implementations through two primary mechanisms:

    1. Bias Estimation: Unlike some filters that only track pose, FusionCore tracks gyro bias and accelerometer bias as live filter states. It continuously estimates and corrects these biases using GPS cross-covariance, preventing heading drift from accumulating into quadratic position error.

    2. Adaptive Noise Modeling: FusionCore does not just rely on sensor-reported covariance. It adapts the noise model using a 50-sample sliding window of the live innovation sequence. It specifically:

      • Scales GPS noise by the HDOP (Horizontal Dilution of Precision) from the sensor message.
      • Applies a chi-squared Mahalanobis gate to reject statistical outliers (e.g., GPS multipath in urban canyons).

    3. Numerical Stability: To prevent the numerical divergence (NaN errors) common in Unscented Kalman Filters (UKF) when using high-frequency IMU data, FusionCore:

      • Normalizes the quaternion component of the state vector at every predict step.
      • Regularizes the orientation covariance to maintain positive semi-definiteness.

  6. Comparison: FusionCore vs robot_localization

    main

    FusionCore differs from robot_localization in several key architectural ways:

    Featurerobot_localizationFusionCore
    GPS FusionRequires navsat_transform_node; uses UTM projectionNative ECEF filter state; no UTM projection
    IMU BiasNot in state vectorGyro + accel bias are filter states
    Outlier RejectionManual Mahalanobis thresholdPre-calibrated Mahalanobis chi-squared gate
    Noise EstimationUses sensor-reported covarianceAdaptive estimation from 50-sample innovation window
    ZUPTNot built-inAutomatic when stationary
    Delay Compensationsmooth_lagged_data + history_lengthIMU ring buffer replay
    GPS Quality GatingNot built-inGating based on HDOP, satellite count, and fix type
    VSLAM/Dual IMUNot built-inNative support via vslam.topic and imu2.topic

  7. Understanding GPS Outage and Dead Reckoning Benchmarks

    main

    When evaluating FusionCore (FC) against other filters like robot_localization EKF (RL-EKF) during GPS outages, be aware of common pitfalls in benchmark methodology:

    • Avoid Raw Coordinate Comparison: Comparing raw coordinates directly is invalid if the Ground Truth (GT) frame and the filter's odometry frames differ in origin or rotation. The resulting error will be dominated by frame offsets rather than actual drift.
    • Avoid SE(3)-aligned ATE on short segments: Using SE(3) alignment on a short outage segment can mask errors. If a filter has a large constant global offset before the outage, SE(3) alignment will compensate for that offset, making the dead-reckoning error appear artificially low.

    Correct Dead-Reckoning Methodology: To perform a valid comparison of dead-reckoning quality, both filters must start the outage from the same global error state. A proper test should:

    1. Start both filters with GPS disabled from $t=0$ to prevent accumulated global error differences.
    2. Run 60–120s of pure dead-reckoning.
    3. Compare Absolute Trajectory Error (ATE) at the end using the same initial position.

  8. Handle VSLAM reinitialization and outliers

    main

    FusionCore manages VSLAM tracking jumps (e.g., in ORB-SLAM3) using two mechanisms:

    1. Chi-squared gate (Immediate): Rejects large jumps based on the outlier_threshold_vslam parameter (default 22.46).
    2. Map re-anchoring (Recovery): If vslam.reinit_n (default 10) consecutive updates are rejected, FusionCore assumes the VSLAM map has reinitialized to a new origin. It computes a new offset between the VSLAM frame and the filter's odom frame and resumes fusion.

    Tuning Parameters

    ParameterPurposeTuning Guide
    outlier_threshold_vslamMahalanobis distance thresholdIncrease if normal motion is rejected; decrease if jumps are accepted as valid.
    vslam.reinit_nConsecutive rejections before re-anchoringIncrease if re-anchoring fires too easily during fast motion; decrease for faster recovery.

    Warning Log: When re-anchoring occurs, you will see: [WARN] VSLAM: 10 consecutive rejections: reinitialization detected. Re-anchoring map origin.

  9. Understand the FusionCore UKF state model

    main

    FusionCore uses a 23-state Unscented Kalman Filter (UKF) to provide a 3D position estimate at 100 Hz. Unlike standard EKFs, it includes live estimation for sensor biases.

    The 23 states consist of:

    • Position: 3 states
    • Orientation (Quaternion): 4 states
    • Body-frame velocity: 3 states
    • Angular velocity: 3 states
    • Body-frame acceleration: 3 states
    • Gyro bias: 3 states (estimated and corrected continuously)
    • Accelerometer bias: 3 states (estimated and corrected continuously)
    • Encoder WZ bias: 1 state

  10. Understand the architectural differences between FusionCore and robot_localization

    main

    When comparing FusionCore to robot_localization, note that the following differences are inherent to the FusionCore architecture rather than tuning choices:

    • Bias Estimation: FusionCore includes gyro and accel bias states in the filter; robot_localization does not.
    • GPS Noise Adaptation: FusionCore adapts GPS noise based on the innovation sequence, whereas robot_localization uses the sensor-reported covariance as-is.
    • Outlier Rejection: FusionCore uses chi-squared gates calibrated per sensor DOF (Degrees of Freedom). robot_localization uses unsquared Mahalanobis scalars.

    Note on Statistical Confidence: In the rl_ekf_outdoor.yaml configuration, Mahalanobis rejection thresholds are set to chi-squared critical values at 99.9% confidence to match FusionCore's default statistical confidence level.

  11. Understand the 23-dimensional state vector

    main

    FusionCore maintains a 23-dimensional state vector representing the robot's motion and sensor biases.

    IndexStateUnitsNotes
    0–2Position x, y, zm (ENU frame)Local ENU, origin at first GPS fix
    3–6Orientation qw, qx, qy, qzunit quaternionBody-to-world rotation
    7–9Linear velocity vx, vy, vzm/s (body frame)
    10–12Angular velocity wx, wy, wzrad/s (body frame)
    13–15Linear acceleration ax, ay, azm/s² (body frame)
    16–18Gyroscope bias bωx, bωy, bωzrad/sSlow MEMS drift
    19–21Accelerometer bias bax, bay, bazm/s²Slow MEMS drift
    22Encoder WZ bias b_ewzrad/sSystematic encoder heading rate offset

    Encoder WZ Bias Note: The filter estimates the b_ewz (encoder WZ bias) online. This accounts for mechanical asymmetries or wheel diameter mismatches that cause a constant heading rate offset. The filter converges on this estimate within a few minutes of driving by using the cross-covariance between encoder WZ and GPS position.

  12. Access ROS 2 topics from the host machine

    main

    To allow the FusionCore container to communicate with ROS 2 nodes running on your host machine (or other containers), you must share the host's network namespace using the --net=host flag. This is necessary because ROS 2 uses DDS for discovery, which relies on UDP multicast; without host networking, the container remains isolated on a Docker bridge network and cannot see host topics.

    docker run --rm -it \
  --net=host \
  -v ~/my_robot.yaml:/config/robot.yaml:ro \
  ghcr.io/manankharwar/fusioncore:latest \
  ros2 launch fusioncore_ros fusioncore.launch.py \
    fusioncore_config:=/config/robot.yaml