fusioncore

manankharwarApache-2.0活跃维护

面向户外机器人的 ROS 2 定位模块,用 23 状态 UKF 以 100 Hz 融合 IMU、轮速计和 GPS;定位出错时能指出是哪个传感器、为什么出错,而不是悄悄漂移。

ROS 2 localization for outdoor robots: IMU, wheel encoders and GPS fused in a 23-state UKF at 100 Hz. When the estimate goes wrong it reports which sensor and why, instead of drifting silently. Apache 2.0.

v0.5.0发布于 2026-10-27 03:14 UTC384star +1 / 7 天69fork同步于 2026-11-10 18:01 UTC

怎么上手

由 README 提炼,执行前请确认
mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
git clone https://github.com/manankharwar/fusioncore.git
cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y
colcon build --packages-up-to fusioncore_ros
source install/setup.bash
硬件要求按 README 说明

README 说明可在 Raspberry Pi 4 上运行,Release 构建下每周期远低于 1 ms,同一份源码用于 ARM 与 x86;未给出显存或具体 GPU 型号要求。

有 Docker 镜像
下载:源码 zip·GitHub Releases(安装包)本站不镜像安装包

项目说明

由 README 自动翻译提炼

面向会丢 GPS 的户外轮式机器人,用 23 状态 UKF 融合 IMU、轮速与 GPS,出错时指出是哪个传感器

面向户外机器人的 ROS 2 定位模块,用 23 状态 UKF 以 100 Hz 融合 IMU、轮速计和 GPS;定位出错时能指出是哪个传感器、为什么出错,而不是悄悄漂移。

核心能力

  • 23 状态 UKF 以 100 Hz 融合 IMU、轮速、GPS 与视觉 SLAM
  • 按平台 profile 冻结不可观测状态,减少无谓估计
  • 逐条发布 GPS 拒收原因与马氏距离阈值
  • filter_health 报告各传感器新息范数与航向不确定度
  • 提供 bag_report.py 无需真值即可检查传感器配置问题

容易踩的坑

必须设置 CMAKE_BUILD_TYPE 为 Release,未优化构建会显著变慢;仅用 6 轴 IMU、轮速和 GPS 位置时航向不可观测,需加磁力计或双天线 GNSS;平滑型 GNSS 接收机的卡方门限实际灵敏度低于标称值,建议先用 nis_from_bag.py 实测。

内容由模型根据 README 生成,可能不完整或不准确,以官方文档为准。看原文 README

最近发版

5 个版本v0.5.0
  1. v0.5.00.5.0最新正式版2026-10-27 · 2026-10-27 03:14 UTC

    One behaviour change, and it only affects the ground-robot profiles. If you set platform.profile: ground_robot or ground_robot_flat, those profiles now turn on GNSS re-anchoring, which can make the published position jump after a GPS outage. Set gnss.reanchor_enabled: false to keep 0.4.x behaviour; an explicit false wins over the profile. Nothing changes for platform.profile: general, the default. vslam.frame_id was removed (it was declared and never read).

    Behaviour change: re-anchoring is on in the ground-robot profiles

    This is a product decision with stated costs, not a pass. Re-anchoring failed the rule a library default change has to pass (tools/ship_rule.md, written before the test) on two secondary clauses: on the seven held-out NCLT sequences it raised median error by 3.3% and 4.4% on two, and output-rate jitter by 0.126 on one. So the library default stays off.

    What it does, on all twelve NCLT sequences with the corrected dataset player (offline replay, full-trajectory ATE RMSE, tools/eval_20261025_results.md):

    RMSE           lower on 10, higher on none, identical on the 2 where it never fires
    median         higher on 4 (+0.3% to +3.0%)
    jitter         higher on 3 (up to +0.124 output-rate path ratio)
    largest move   2012-08-20, 165.364 m -> 45.669 m
    

    Scored at GPS fix times it brings FusionCore below raw GPS on 2 of 12 sequences, both held out (2012-03-31 by 22.0%, 2012-10-28 by 5.6%). The shipped default does not beat raw GPS on any (raw GPS lower on 11, one tie). Through the ROS node on 2012-01-08 re-anchoring read 10.882 m at fix times against 10.898 m in replay on the same points, inside a pass band fixed before that run. One sequence, one run.

    Each re-anchor is a deliberate position jump, reported on /fusion/debug/gnss_status (reanchored, reanchor_jump_m, reanchor_count) and in /diagnostics. Anything that differentiates the pose will see it.

    Why it exists

    On NCLT, 74.8% to 96.8% of squared error on the five development sequences sat in GPS blackouts or the re-convergence after them, and in that recovery window the outlier gate rejected 81% of the fixes arriving on two sequences: the filter had drifted far enough that good fixes looked like outliers. Re-anchoring watches for that state (the gate refusing most of what arrives while the recent fixes agree with each other and with wheel odometry) and moves position, never heading, onto them.

    Also in this release

    • Our NCLT dataset player declared the IMU's accelerometer-derived tilt as a 0.57 degree attitude. That was our choice, not a dataset flaw. It made the node fuse the accelerometer twice and lock onto a wrong position for about 800 s on 2012-01-08 (178.808 m to 57.314 m once fixed). Every NCLT number measured before 2026-10-25 used the old player.
    • The node warns once when it fuses IMU roll/pitch declared tighter than 1 degree, the mistake above.
    • tools/bag_report.py reads any rosbag2 and reports why an estimator is drifting, with no ground truth and no FusionCore build: IMU and wheel yaw rates that disagree, sensors on different clocks, a receiver whose declared accuracy does not match its fixes, a log with no stop, long GNSS gaps, and an IMU orientation that is only accelerometer tilt.
    • platform.profile (general, ground_robot, ground_robot_flat) declares what the platform can observe. It zeroes the bias process noises and priors on a robot with encoders. It slows the biases rather than holding them fixed; see the CHANGELOG.
    • messages_lost on /diagnostics and a per-minute count of consumed sensor callbacks.
    • Declination sign fixed on the raw-field magnetometer path, and applied on the 9-axis IMU path, which ignored it.

    What is not claimed

    The comparison against robot_localization on NCLT is withheld: our player fix changed its input and broke its config, and it is being re-run. There is no hardware result in this release. Full detail, with every number's pipeline and scorin

  2. v0.4.00.4.02026-09-15 · 2026-09-15 03:26 UTC
    查看更新说明

    First release with breaking changes. Two public FusionCoreConfig fields were removed, so code built directly against fusioncore_core will need a change, and four defaults now alter behaviour for anyone upgrading without touching their config. Nothing errors and every YAML still loads.

    Breaking

    settingwasnoweffect
    gnss.recovery_rejection_n0, off15post-blackout P inflation fires
    gnss.continuity_autonewtruefix-to-fix gate arms itself after 100 fixes
    zupt.accel_std_thresholdnew0.5the IMU can veto a ZUPT
    gnss.gps_track_heading_cross_check_degnew15.0a disagreeing GPS track heading is refused

    Removed from FusionCoreConfig: gnss_recovery_timeout_s (declared, documented, set in a shipped config, never read by anything) and encoder_nhc_vy_sigma (written by the node into a field nothing consumed, while the working assignment reached encoder.vel_noise_y). The ROS parameter gnss.recovery_timeout_s is still declared so old configs load, and the node now warns once if it is set.

    Rejection reason codes are unchanged. NOT_FINITE and QUALITY_OTHER were appended at 12 and 13, and MIN_SATS kept its 5.

    A filter that dead-reckoned through a GNSS outage now comes back

    This is the reason for the version bump. It took six separate defects.

    NCLT 2012-06-15, error 300 s after fixes returned     277 m  ->  13 m
    a log with 275/129/65/55 s gaps, 300 s after the 129s 746 m  ->  78 m
    NCLT 2013-04-05 ATE                                 277.6 m  -> 189.7 m
    

    The inflation had no constant that could work, so it is now sized from the rejected innovation itself. Only the chi2 path could arm it, so cascades starting at any other gate never did. A 3-DOF gate cannot be opened by inflating two of its axes. The gap test was in absolute seconds, which is meaningless at 1 Hz where the healthy spacing between fixes is the threshold, so it is now relative to the receiver's own cadence.

    The fifth hid the other four: the continuity history was allowed to span the gap. That history is also where fix cadence is derived, so a buffer spanning an outage reported a mean spacing of 115 s instead of 0.2, and "have we just had an outage" silently became "was the gap longer than 231 seconds". The gate was healthy by every measure you could take from outside. What was broken was a statistic it exported to something else.

    The sixth: the code clearing the post-outage latch sat inside reset() and never ran on an accepted fix, so any blackout disabled spike rejection for the rest of the run. Measured with a sustained 300 m offset after a clean recovery: 586 of 601 spike fixes accepted and 299.97 m of error before, 0 of 601 and 25.39 m after.

    Also new

    A fix-to-fix continuity gate that measures its own threshold from the receiver over the first 100 fixes, because chi2 judges a fix against the filter and cannot see a metre-scale spike. An IMU check before ZUPT, since wheels reporting zero is not the same as a stationary robot and a dead encoder previously convinced the filter it was parked while driving. A GPS track heading cross-check. gnss.min_satellites can no longer silently reject every NavSatFix. Encoder rejections now say why. Radar and GNSS velocity inputs substitute a predicted yaw rate instead of a fabricated zero.

    CI now fails on a ROS parameter that never reaches the filter, contributed by @Rayan-and-beyond.

    Benchmarks

    FusionCore beats robot_localization on 10 of 12 NCLT sequences and on spike rejection. The two losses are both multi-minute blackouts where robot_localization's 2D model structurally cannot leak roll and pitch into yaw. That remains open and the honest path through it is an absolute heading source, not tuning.

    Note for anyone reproducing: 2012-06-15 repeated to 27.6% between two runs of identical code in this release's validation, so a single number from that sequence is not usable in either direction. `tools

  3. v0.3.90.3.92026-09-08 · 2026-09-08 17:41 UTC
    查看更新说明

    This release is about FusionCore explaining itself. Every fix in it came from the same failure: a gate rejected data, or a setting was wrong, and nothing said so.

    The filter now tells you what it did with every measurement

    /fusion/debug/filter_health carries four parallel arrays: outcome_names, outcome_counts, outcome_first_seen, outcome_last_seen. Every gate outcome since init, with the measurement stamps of its first and last occurrence.

    A name appears only once it has happened, so a gate that never fired is one you can rule out by its absence. ACCEPTED is counted alongside the rejections, which is what makes an empty result readable: if gnss:ACCEPTED is missing too, no fix ever reached the filter and the gates are not the problem. Reading a rejection count against the accepted count also gives you a rate, and a rate is what separates a gate catching spikes from a gate fighting your receiver.

    Establishing that the chi2 gate had never fired on one field run previously cost a day of replaying a bag. It is now a line on a topic.

    A floor on receivers that flatter themselves

    gnss.min_sigma_xy and gnss.min_sigma_z set a floor, in metres, on the sigma a receiver reports before it becomes R.

    There was already a floor, hardcoded at 2 cm, added so an RTK-fixed receiver reporting 3 mm could not fail its own chi2 gate. That assumes a receiver is honest when it claims to be that good. A u-blox M9N was measured declaring sigma_xy of 0.076 m while sitting completely still, scattering 1.03 m over 70 fixes and drifting 1.97 m end to end: 13.6x over-confident. Earlier the same day, same spot, the same receiver declared a reasonable 3.16 m.

    This matters more than a wrong covariance, because R is built from that number and the chi2 gate judges every fix against the same R. The floor is in metres rather than a multiplier on purpose: a multiplier tuned for the over-confident mode would be badly wrong the moment the receiver reverts to reporting honestly. Defaults are unchanged, so nothing shifts unless you set them.

    Things that were silently wrong

    • An unmeasured antenna lever arm was the one setting FusionCore said nothing about. The startup log printed it only when non-zero, so 0,0,0 produced no output at all, while zeros mean no correction is applied. The resulting error is the true offset rotated by heading, which on flat ground is a cross-track bias that flips sign when the robot turns around. It is now warned about at the moment heading validates well enough for the correction to go live. Reported from the Sowbot agricultural stack.
    • nav2_lifecycle_manager cannot own FusionCore. It requires a bond heartbeat that FusionCore does not implement, so the manager reports a bond timeout on every boot regardless of bond_timeout. FusionCore being a lifecycle node makes putting it under the manager look correct, and getting-started.md was actively suggesting it. Both that page and nav2.md now say not to, and name the two approaches that work.
    • Fixes the receiver marked NO_FIX were dropped without a trace, counted nowhere, so a receiver that had lost fix looked identical to one working normally.
    • filter_health published an empty rejection reason for three of the twelve reasons, because a second copy of the reason table had gone stale. Empty reads as "nothing was rejected", which is worse than a wrong name.
    • gnss.heading_observable_distance was hardcoded at 5 m and unreachable from any config.
    • The lever-arm TF warning repeated every 5 seconds for the life of the run, roughly 700 times an hour. A missing TF does not fix itself, so it now reports three times and says when it is the last.

    Config checking beyond misspelled names

    tools/check_config_params.py now flags wrong values, not just names the node never declared. Outlier thresholds below their chi-squared floor are errors and fail CI, because that is a mathematical limit. Hardware-dependent settings w

  4. v0.3.80.3.82026-09-01 · 2026-09-01 17:37 UTC
    查看更新说明

    No estimation code changed in this release. The filter behaves exactly as it did in 0.3.7. What changed is the layer around it.

    If you already have a workspace, rebuild it. No package set CMAKE_BUILD_TYPE, so the documented colcon build was producing no optimisation flags at all. The three C++ packages now default to Release. The default only applies when CMake configures a build directory, so an existing build/ keeps the old binary:

    rm -rf build install && colcon build
    

    For scale: the consistency suite runs in 298 s unoptimised and 4.3 s at -O3, and the test suites together went from 5 min 19 s to 42 s. On a Raspberry Pi at 100 Hz that is the difference between keeping up and starving.

    tools/quick_test.sh had been broken since 0.3.3, which is the whole time the README has told new users to run it first. Reported by @PaulBouchier in #75. Two auto-activation paths had grown up around the script (the node's autostart in 0.3.1, the launch files' autoconfigure in 0.3.3) and it was still driving the lifecycle by hand, so whichever transition arrived second was invalid. It failed differently depending on which side won the race, which is why it went unnoticed.

    Behaviour change: autoconfigure:=false now also clears the node's autostart. If you drive the lifecycle externally, from a nav2_lifecycle_manager or by hand, you now get unconfigured -> inactive -> active in the order you ask for it. That documented path could not previously have worked for anyone.

    New: measuring whether the reported covariance is honest. test_consistency.cpp measures NIS and NEES in simulation; tools/nis_from_bag.py measures NIS on any recorded run, with no ground truth needed. Both should average 3.0 for a GNSS position measurement.

    Running it on real field bags turned up something that is not a FusionCore issue and affects anyone fusing a smoothing GNSS receiver, robot_localization included: the receiver declared 4.84 m horizontal 1-sigma while consecutive fixes agreed to 0.087 m. The absolute error really is metres; the fix-to-fix consistency is centimetres. A Kalman filter assumes white noise, so handing it the declared figure as R makes S dwarf every innovation. One consequence is that the chi2 outlier gate is far less sensitive than its nominal 99.9% threshold suggests. Details and the caveats in docs/guides/filter-consistency.md.

    Also: middleware compatibility now verified rather than assumed (Fast DDS, rmw_zenoh_cpp, rmw_cyclonedds_cpp all pass the full quick test on Jazzy). README leads with the debug topics, which it previously did not mention at all.

    Full detail in CHANGELOG.md.

  5. v0.3.72026-08-14 · 2026-08-14 05:48 UTC
    查看更新说明

    This release changes one default, ukf.alpha, from 0.1 to 1.0. That one line is worth a release on its own, because the old value was wrong in a way that affected every user, and nobody could have overridden it: ukf.alpha was never exposed as a ROS parameter, so it only ever had its compile-time value.

    What was wrong

    The UKF propagates its mean as a weighted sum over sigma points, sum(W_i * sigma_i), with W[0] = lambda/(n+lambda) and lambda = alpha^2*(n+kappa) - n. FusionCore has 23 states and uses kappa = 0, so with alpha = 0.1 that works out to lambda = -22.77 and a centre weight of -99.0, against 46 outer weights of +2.17.

    Those sum to 1, so the filter is formally correct. It is only correct in practice when the sigma points sit in a tight cluster. Yaw is structurally unobservable without an absolute heading source, and during a GPS gap the quaternion sigma points spread out. Their forward displacements then cancel against each other, and what is left is the centre point, the one still pointing the right way, multiplied by -99.

    The result is that position moves backwards while velocity and heading both read correctly. Here is a 120 second GPS blackout, truth is 1 m/s dead straight, reproduced by tools/repro/blackout.cpp:

       t=  30 s   x =   27.24    truth   30.00
       t=  60 s   x =  -17.10    truth   60.00
       t=  90 s   x = -180.55    truth   90.00
       t= 120 s   x = -411.85    truth  120.00
    

    Yaw reads 0.00 degrees the whole way down, which is correct, and the encoder is perfect. The filter is driving backwards at speed while every state it reports looks fine.

    That is why this took a long time to find. It presented as "velocity is right, position is wrong", it survived every process-noise and covariance explanation we tried, and it was not a regression: the behaviour is bit-identical on code from May.

    What changed

    alpha = 1.0 gives lambda = 0 and all 47 weights non-negative. That is the standard unscaled UKF. Worth stating clearly, because it is counterintuitive: with kappa = 0, any alpha below 1 produces a negative centre weight at this state count. Lowering alpha to tighten the spread does the opposite of what it does in a low-dimensional filter.

    Measured on NCLT 2013-04-05, full sequence at 1x playback, with robot_localization in the same run as a control:

                            FusionCore     RL-EKF
    before                   5268.80 m    230.8 m
    after                     131.85 m    229.7 m
    

    The control moved by 1.1 m across those runs, so the harness was stable and the change is FusionCore's.

    Path length ratio went from badly wrong to 1.0092, meaning the estimated trajectory is now within 0.9% of the true distance travelled. For comparison, robot_localization draws the same path 18% short.

    Two things this release does not do

    It does not fully close the gap. Commit c8b8b1f from 19 May measures 22.96 m on this same sequence and the same machine, so a second regression of roughly 5x is still unidentified. The sigma weight was the dominant cause, not the only one.

    It does not fix heading. With the weighting corrected, a 120 second blackout still drifts yaw to about -134 degrees from zero rotation input, with the wheel encoder reporting zero yaw rate throughout. Position now follows velocity correctly, so the path is smooth, it just curves away. That is a separate defect and it is the prime suspect for the remaining gap.

    Only 2013-04-05 has been re-measured. Every other entry in tools/benchmark_baseline.json predates this fix and should be treated as stale.

    Should you upgrade

    Yes, and particularly if your robot loses GPS for extended periods, runs without an absolute heading source such as a magnetometer or dual-antenna GNSS, or operates GPS-denied on wheel odometry and a 6-axis IMU. Continuous GPS largely masks the problem because track heading bounds the yaw covariance, so if you have never seen bad behaviour that is consistent with this and does

用户点评

登录用户的真实使用记录,可按版本和硬件筛选
共 0 条

    还没有人点评。第一个说出真实体验的人最有价值。

    README 原文

    未翻译

    FusionCore

    CI arXiv DOI Docs Newsletter

    Sensor fusion for outdoor wheeled ground robots that lose GPS. IMU, wheel encoders, GPS and visual SLAM at 100 Hz in a 23-state UKF. Two numbers from your IMU datasheet instead of days of tuning, and when the estimate goes wrong it names the sensor and the reason instead of drifting silently. Apache 2.0, ROS 2 Jazzy and Humble, and the filter itself is a plain C++ library with no ROS dependency.



    What this is for

    A wheeled robot, outdoors, with wheel odometry, that loses GPS. Differential drive, skid steer, tracked or Ackermann. That is the platform every number on this page was measured on: NCLT is a wheeled Segway and the hardware runs are a four-wheeled rover.

    You tell the filter which platform it is, in one line, and that decides which states it bothers to estimate:

    fusioncore:
      ros__parameters:
        platform.profile: ground_robot_flat   # general | ground_robot | ground_robot_flat
    

    A 23-state filter can estimate more than a wheeled robot can observe. The gyro measures WZ + B_GZ and the accelerometer measures AX + B_AX + gravity, so those pairs are not separately observable from those sensors alone. On a robot with encoders, platform.profile: ground_robot sets the bias process noises and priors to zero and treats the encoder's yaw rate as bias-free, so the encoder carries the constraint. From 0.5.0 it also turns on gnss.reanchor_enabled (recovery after a GPS outage), which failed its ship rule on median and jitter and is enabled there as a stated product decision; set gnss.reanchor_enabled: false to opt out. It slows the biases rather than holding them: the UKF's covariance regularisation still lets them drift (a median sigma of 0.053 rad/s on the gyro bias on NCLT 2013-04-05). In replay on five NCLT sequences that lowered ATE RMSE on all five, by 0.3% to 17.7% (table in the CHANGELOG). Declaring the platform is what lets the filter stop guessing. See Declaring the platform.

    Not built for drones, boats, legged robots or mecanum platforms. The library will run on them: the core is full 3D, there is no planar assumption, and platform.profile: general exists precisely so nothing is frozen that those platforms need. But nothing here is tuned or benchmarked for them, the profiles do not apply, and the non-holonomic constraint is wrong for anything that can move sideways on purpose.

    For those platforms, and for fusing an arbitrary set of sensors, use robot_localization. It is mature, it is general by design, and it is the right answer where this one has no evidence.

    Start without installing anything

    If you have a rosbag, you can get something useful out of this before you build it. Point bag_report.py at the bag and it tells you what is wrong with your estimation setup, in terms of your robot rather than a benchmark dataset:

    python3 tools/bag_report.py /path/to/your/bag
    

    No config, no ROS distro to match, and every check works with no ground truth, which is the point: almost nobody has a surveyed reference for their own robot. It reports things like whether your IMU and your wheels disagree about which way the robot turned, whether two sensors are on different clocks, and whether the bag even contains a manoeuvre that makes heading observable. Details in docs/bag-report.md.

    The first bag it was ever run on had an IMU and wheel encoders reporting opposite yaw-rate signs on 100% of turns, over 127,419 samples. That bag was from this project's own benchmark harness, and the bug had been there for every number it had ever published.


    Answers to the things that stop people trying it

    Yes, you can run it beside robot_localization rather than instead of it. Same bag, same inputs, both filters, compare the two trajectories. That is how the numbers here were produced, and fusioncore_ros/tests/test_rl_interface_conformance.py locks the interface so it stays a drop-in comparison rather than a rewrite.

    Yes, your existing robot_localization config converts. tools/rl_to_fusioncore.py reads it and emits the FusionCore equivalent, because that config already encodes everything you learned about your robot. It refuses to guess: anything that does not map cleanly comes out as a commented TODO with the reason, repeated on stderr so it is visible when you redirect stdout to a file. A config that looks complete but quietly invented a number is worse than one that admits what it does not know.

    No, you do not need ROS 2. The filter is a plain C++ library with Eigen and no ROS dependency. The ROS 2 wrapper is a separate package you can ignore.

    No, you do not need ground truth to evaluate it. tools/bag_report.py works entirely on raw sensor topics, because almost nobody has a surveyed reference for their own robot. It will not give you an accuracy number, and it says so rather than inventing one.

    Yes, it runs on a Raspberry Pi 4. Well under 1 ms per cycle in a Release build, same source on ARM and x86. Build unoptimised and it is drastically slower, so do not skip CMAKE_BUILD_TYPE.

    No, it does not beat raw GPS or robot_localization on NCLT. All twelve NCLT sequences were measured under a rule committed before the run: the shipped default has higher error than raw GPS on 11 of 12 (the twelfth a 0.001 m tie), and with opt-in re-anchoring it is lower than raw GPS on 2. Against robot_localization the result depends on the sequence, in both directions and by large factors, and the table shows each one rather than a win count. The old claim that FusionCore beat robot_localization on ten of twelve was measured through a broken dataset player and is withdrawn.

    What is defensible today. FusionCore rejects GPS fixes that are kinematically impossible, and that one is provable from physics without any reference: a 60 m jump in one second cannot happen to a 1.5 m/s robot. The large NCLT errors on both sides mostly start at the same moment, the minutes after a GPS blackout, when a filter has to decide which returning fixes to trust: robot_localization tends to accept a bad one and then lock out good ones, FusionCore's default tends to reject good ones for too long.


    Quick start

    sudo apt install ros-jazzy-fusioncore     # or ros-humble-fusioncore
    

    Or from source:

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

    Check it works before wiring it to a robot. This starts the filter with fake sensors and verifies every output, in about 15 seconds:

    bash tools/quick_test.sh
    

    Then point it at your robot:

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

    The launch file brings the lifecycle node all the way up to active on its own. Pass autoconfigure:=false if a nav2_lifecycle_manager should own it instead.

    Docker, if you would rather not install ROS 2: docs/docker.md

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

    When it goes wrong, it tells you why

    Most localization debugging is not a mathematics problem. The filter drifts, and the hard part is working out which of six sensors caused it. FusionCore publishes what it is thinking while it runs, on real hardware:

    ros2 topic echo /fusion/debug/gnss_status     # one message per GPS fix
    ros2 topic echo /fusion/debug/filter_health   # filter state at 1 Hz
    

    gnss_status answers "why was that fix dropped?" for every fix. A rejection_reason (CHI2_FAILED, SIGMA_XY_HIGH, IMPLAUSIBLE_JUMP, DELAY_TOO_LARGE and the rest), the Mahalanobis distance printed next to the threshold it was actually tested against, and the filter's own position sigma at that moment.

    filter_health answers "does this filter even know which way it is pointing?" Per-sensor innovation norms, heading uncertainty in degrees, which source the heading came from (GPS_TRACK, MAGNETOMETER, DUAL_ANTENNA, NONE), and a separate count of measurements dropped because two drivers disagree about the clock rather than because the data was bad.

    That last distinction matters more than it sounds. A sensor whose timestamps run behind the filter clock is not being fused at all, and from the outside that looks exactly like a badly tuned filter.

    You can also ask, after the fact, whether the covariance the filter reported was honest. This needs no ground truth and works on any recorded bag:

    python3 tools/nis_from_bag.py /path/to/your_bag
    

    Details: Is your filter's covariance honest?


    What FusionCore does not do

    Every project has these. Most do not write them down.

    Yaw is not observable from a 6-axis IMU, wheel encoders and GPS position alone. The gyro measures wz + gyro_bias and the encoder measures wz + encoder_bias, which is two equations for three unknowns. GPS track heading only helps while the robot moves in a straight line fast enough for the displacement bearing to beat the position noise. Add a magnetometer or dual-antenna GNSS heading and the problem goes away. Without one, expect heading uncertainty to grow during slow or twisty driving, and read heading_sigma_deg in filter_health rather than assuming.

    The chi-squared gate is less sensitive than its nominal threshold on a smoothing receiver. Many GNSS receivers report their absolute accuracy, several metres dominated by multipath, while emitting fixes that agree with each other to centimetres because they filter internally. A Kalman filter assumes white measurement noise, so it gets handed a covariance far larger than any innovation it will see, and the gate then sits much further above typical than its 99.9% design point suggests. Measure yours with nis_from_bag.py before relying on the gate.

    Long GPS blackouts still accumulate heading error. Beyond roughly five to seven minutes of dead reckoning, residual bias drift dominates. See known limitations.


    Built around the problems real robots have

    The problemHow FusionCore handles it
    IMU calibration is approximateGyro and accel bias are filter states, estimated continuously. init.stationary_window: 2.0 estimates startup bias before motion begins.
    Extrinsic calibration is never exactReads frame_id from every IMU message and looks up the TF rotation to base_link automatically. Set imu.frame_id to override broken frame names from drivers. No manual rotation matrices.
    Sensors disagree about what time it isStamps more than 1 s from the node clock warn at startup. A sensor lagging the filter clock is rejected as stale rather than being allowed to corrupt it, and the count is published so you can see it happening.
    GPS arrives late (50 to 200 ms)An IMU ring buffer replays the buffered updates when a delayed fix arrives, reconstructing the state at the GPS timestamp rather than approximating it. The buffer holds 100 IMU samples, one second at 100 Hz, but the usable window is max_measurement_delay (0.5 s by default): a fix older than that is dropped rather than replayed.
    Wheel odometry is noisy or slippingAdaptive noise covariance updates from the innovation sequence. Optional GPS velocity fusion compares GPS speed against wheel speed every cycle, so the innovation reveals slip and the gain down-weights it.
    Noise parameters need days of tuningTwo numbers from your IMU datasheet, imu.gyro_noise and imu.accel_noise. The adaptive estimators handle the rest, though a bad frame or a bad timestamp will still need fixing by hand.
    The robot runs on a Raspberry PiWell under 1 ms per cycle on a Pi 4 in a Release build. Same source on ARM and x86 via Eigen. Build unoptimised and it is drastically slower, so do not skip CMAKE_BUILD_TYPE (0.3.8 and later default to Release).
    Two IMUs on the platformSet imu2.topic to fuse a second IMU as an independent measurement. No pre-merging needed.
    GPS drops out under canopyInertial coast mode holds position through sustained dropout, and the gate relaxes on re-acquisition after a genuine gap rather than after a sustained outlier.
    The robot sits still for minutesZUPT fuses a zero-velocity pseudo-measurement when encoder speed and angular rate are both below threshold, so IMU noise does not integrate into drift while idle.

    What it costs you to run

    Accuracy is the easy claim to make and the hard one to verify, especially on a robot nobody has surveyed. These are the costs, which you can check against your own setup in an afternoon:

    TuningTwo numbers, imu.gyro_noise and imu.accel_noise, both off your IMU datasheet. The adaptive estimators handle the rest. A bad frame or a bad timestamp still needs fixing by hand, and the tools below find those.
    Finding out why it driftedEvery sensor has a named rejection reason, and the gated ones carry their chi-squared value too, published on a debug topic while it runs. So "it drifts" becomes a count per reason rather than a guess about which of six sensors did it.
    Knowing whether to trust itThe filter reports which quantities are actually observable and what to drive to fix the rest, rather than leaving you to infer it. See Observability.
    Catching a config typoROS 2 silently ignores an override for a parameter a node never declared, so a typo tunes nothing and warns nobody. tools/check_config_params.py fails on it.
    Evaluating it at alltools/bag_report.py on a recording, before you build anything.

    Three of those five exist because they were the thing that cost days here first. The observability report exists because three separate issues in this project turned out to be "the robot was never driven in a way that made this observable", and nothing said so for weeks.


    Benchmark

    FusionCore against robot_localization on the NCLT dataset: same IMU, wheel odometry and GPS, no manual tuning, twelve full-length sequences across all seasons. RL-EKF run with chi-squared-equivalent thresholds at 99.9% confidence.

    Status, 2026-10-26, and this supersedes every dated note below it.

    All twelve NCLT sequences were re-run under a reporting rule committed before the run started (tools/report_rule_20261025.md). The dataset player was corrected first: it had been declaring the IMU's accelerometer-derived tilt as a 0.57 degree attitude measurement. That declaration was ours, not the dataset's. It counted the accelerometer twice and made the ROS node lock onto a wrong position for about 800 s on 2012-01-08 (178.808 m full trajectory RMSE before the correction, 57.314 m in this run).

    Scored at GPS fix times, the shipped default does not beat raw GPS: raw GPS has the lower RMSE on 11 of 12, and the twelfth, 2012-05-11, is a tie at 9.186 m against 9.187 m. With opt-in re-anchoring (gnss.reanchor_enabled), FusionCore is lower than raw GPS on 2 of 12, 2012-03-31 by 22.0% and 2012-10-28 by 5.6%, both held-out sequences never used for tuning. Against the default on the same pipeline and scored over the full trajectory, re-anchoring lowers RMSE on 10 of 12 and raises it on none. It also raises the median on 4 (by 0.3% to 3.0%) and the output-rate jitter on 3 (by up to 0.124), which is why it is off by default.

    ATE RMSE (3D) at GPS fix times, every column on identical timestamps:

    SequenceSetRaw GPSDefault, ROS nodeDefault, replayRe-anchoring on, replayRe-anchoring on, ratio to raw GPS
    2012-01-08held out9.883 m56.957 m50.065 m10.899 m1.103x
    2012-02-04dev57.564 m163.830 m145.629 m59.340 m1.031x
    2012-03-31held out13.825 m16.818 m16.937 m10.787 m0.780x
    2012-05-11held out9.187 m9.186 m9.187 m9.187 m1.000x
    2012-06-15dev9.293 m24.363 m12.041 m12.041 m1.296x
    2012-08-20dev34.250 m160.359 m172.961 m39.790 m1.162x
    2012-09-28dev8.960 m18.883 m22.236 m10.176 m1.136x
    2012-10-28held out13.923 m15.999 m16.763 m13.150 m0.944x
    2012-11-04held out20.267 m23.626 m24.512 m20.414 m1.007x
    2012-12-01held out12.681 m16.160 m16.466 m12.741 m1.005x
    2013-02-23held out15.724 m17.294 m16.951 m15.793 m1.004x
    2013-04-05dev8.624 m9.994 m10.007 m8.798 m1.020x

    How these were produced: the ROS node at 1x playback (achieved filter rate 99.8 to 103.3 Hz on every sequence) and the deterministic offline replay of the same shipped config on the same input, scored by tools/score_nclt_suite.py (version 803c158e99ff, printed on every row) through hardware/ate_tum.py. Re-anchoring was run through replay only, so compare it with the replay default column. The node and the replay still disagree: node over replay is 0.70x to 1.14x over the full trajectory, and at fix times up to 2.0x on 2012-06-15 (24.363 m against 12.041 m). That gap is not yet explained. No sequence was excluded. Medians, path ratios, jitter and coverage for every arm and sequence are in tools/eval_20261025_results.md. "Held out" sequences were never used to develop or tune anything (tools/ship_rule.md).

    robot_localization, re-run on all twelve sequences 2026-10-27 to 11-07. Config fusioncore_datasets/config/rl_ekf.yaml as amended 2026-10-26, following robot_localization's own planar wheeled-robot example: wheel odometry fuses vx, vy and vyaw, the IMU fuses yaw rate only, GPS arrives through navsat_transform. Its GPS gate (odom1_pose_rejection_threshold: 3.72, chi-squared 2 DOF at 99.9%) has been in that file since April and was not changed. Same harness, player and scorer as above, node at 1x, one sequence at a time; that playback also ran FusionCore a second time, reported as a repeat. Filter rate 100.0 to 103.3 Hz on every sequence; robot_localization logged 9 update-rate misses on 2012-01-08 and none on the other eleven. Coverage 99.9% or above everywhere. No sequence excluded, none re-run.

    ATE RMSE / median (metres) at GPS fix times. The points are the timestamps common to every arm including robot_localization, so the raw GPS and FusionCore figures can differ from the table above in the third decimal (2012-01-08 raw GPS 9.878 m here, 9.883 m above):

    SequenceSetPoints scoredRaw GPSrobot_localization, nodeFusionCore default, nodeFusionCore default, node repeatFusionCore default, replayFusionCore re-anchoring, replay
    2012-01-08held out101679.878 / 6.76831.647 / 6.17656.969 / 13.80057.666 / 13.83750.075 / 12.35210.895 / 6.816
    2012-02-04dev890557.564 / 13.600145.976 / 36.984163.830 / 42.020150.519 / 36.416145.629 / 35.61259.340 / 13.519
    2012-03-31held out1004613.824 / 7.469160.874 / 42.83016.816 / 7.35715.886 / 7.33616.934 / 7.37610.785 / 7.413
    2012-05-11held out103849.187 / 5.2108.387 / 4.4259.186 / 5.2679.184 / 5.2519.187 / 5.2179.187 / 5.217
    2012-06-15dev65269.293 / 4.89145.941 / 16.37524.363 / 7.84623.366 / 7.93112.041 / 4.90012.041 / 4.900
    2012-08-20dev979934.250 / 7.31910.400 / 4.451160.359 / 35.210159.581 / 35.923172.961 / 38.08239.790 / 7.495
    2012-09-28dev84048.960 / 6.43095.140 / 26.58518.883 / 6.85918.875 / 6.86322.236 / 7.14010.176 / 6.554
    2012-10-28held out890113.923 / 8.25125.929 / 6.67915.999 / 8.39915.997 / 8.38616.763 / 8.43613.150 / 8.335
    2012-11-04held out820020.267 / 9.65724.170 / 7.83423.626 / 10.22024.733 / 10.26824.512 / 10.30120.414 / 9.806
    2012-12-01held out783812.681 / 7.84212.528 / 5.42116.160 / 7.91016.135 / 7.88416.466 / 7.85912.741 / 7.948
    2013-02-23held out856915.724 / 9.45177.986 / 24.30117.294 / 9.65017.300 / 9.63316.951 / 9.63015.793 / 9.441
    2013-04-05dev74588.624 / 6.3198.329 / 4.4499.994 / 6.45710.020 / 6.46210.007 / 6.4478.798 / 6.443

    No aggregate and no winner, per the rule committed before the run: the spread is the result. Where robot_localization does badly, the one case examined in full (2012-03-31) is not the harness. A 262 s GPS blackout, then the first three fixes after it are 535 to 537 m wrong in the dataset. Its own covariance is so large after dead reckoning that the gate admits the first one, the covariance collapses onto it, and every good fix after that fails the gate for about 290 s while it drives about 600 m off. That window is 7% of the points and 99.7% of its squared error. 2012-09-28 and 2013-02-23 show the same pattern in a partial check. Where FusionCore does badly, 2012-08-20 is the clearest: robot_localization reads 10.400 m, below raw GPS, and FusionCore's default 160.359 m. That sequence has a cluster of fixes up to 727 m wrong at the end of a blackout; FusionCore's loss there was not re-diagnosed on this run. Full-trajectory RMSE, median, path ratio and jitter for both node arms are in tools/eval_20261025_results.md.

    Older dated notes, the May 2026 comparison table and every withdrawn figure are in benchmarks/nclt_history.md, unchanged.


    Running on real hardware

    "The system was stable on real robot data and was relatively easy to configure. I was able to get reasonable behavior without spending excessive time on parameter tuning. The overall experience felt more deployment-oriented than research-demo-oriented."

    Michał Bednarek (@mbed92), Robotics PhD Factory differential-drive robot, ROS 2 Humble: Cartographer (point-cloud localization, no preloaded map) + wheel odometry + IMU

    Sam (@samuk), Agroecology Lab Outdoor agricultural robot, integration in progress

    Russ Hall, Andino robot (Raspberry Pi) OAK-D (stereo depth + IMU) + Velodyne VLP-16 + rtabmap: indoor SLAM mapping

    Running FusionCore on your robot? Add yourself in Discussions #22 and I will list you in ADOPTERS.md.


    Coming from robot_localization

    FusionCore replaces the robot_localization + navsat_transform pair with one lifecycle node. The migration guide covers the YAML and launch changes: migration guide

    ProblemHow FusionCore handles it
    A GPS outlier corrupts the stateChi-squared gate per sensor DOF rejects bad fixes before they reach the filter, and reports which gate fired. Covariance is bounded at every step, so no NaN divergence.
    UTM zone boundary near the operating areaGPS is fused directly in ECEF. No UTM projection and no zone boundary case.
    A wheeled robot drifts sideways without GPSNon-holonomic constraint zeros lateral and vertical velocity as a virtual measurement on every encoder update.
    GPS fixes arrive 50 to 200 ms lateIMU ring buffer with retrodiction reconstructs the exact filter state at the GPS timestamp.
    Bag replay gives different results each runMessage timestamps drive all updates under use_sim_time: true. Same bag, same config, same output.
    An IMU mounted off-axis, or a driver with a wrong frame nameTF lookup on every message, with an imu.frame_id override.
    navsat_transform startup ordering and CPU costNo navsat_transform node. ECEF conversion is one matrix multiply per fix inside the filter.

    Using the filter without ROS 2

    fusioncore_core is a plain C++17 library. Its only dependency is Eigen, and it contains no ROS headers, so it compiles and runs on its own:

    #include "fusioncore/fusioncore.hpp"
    
    fusioncore::FusionCoreConfig cfg;
    fusioncore::FusionCore fc(cfg);
    fusioncore::State s;
    fc.init(s, 0.0);
    fc.update_imu(0.01, wx, wy, wz, ax, ay, az);
    fc.update_encoder(0.02, vx, 0.0, wz);
    
    fusioncore::sensors::GnssFix fix;      // ENU metres, plus hdop/vdop/satellites
    fix.x = 12.0; fix.y = 3.0; fix.z = 0.0;
    fc.update_gnss(0.20, fix);
    
    const auto& out = fc.get_state();      // out.x is the 23-state vector, out.P its covariance
    
    g++ -std=c++17 -O2 your_main.cpp fusioncore_core/src/*.cpp \
        -I fusioncore_core/include -I /usr/include/eigen3 -o your_app
    

    Useful if you are on PX4, a custom middleware, or an embedded loop where ROS 2 is not the right fit. fusioncore_ros is a thin wrapper over exactly this API.


    In the ecosystem

    rtabmap_ros (merged): included as a named demo in the official rtabmap_ros repository. The demo runs FusionCore and icp_odometry in a feedback loop: FusionCore's stable odom frame seeds scan matching via guess_frame_id, and the ICP result returns as a second velocity source. View the demo

    Stereolabs community: a FusionCore + ZED integration guide is posted on the Stereolabs developer forum. @privvyledge set out to compare FusionCore against Wolf, TIER IV EagleEye and robot_localization in May 2026 on an F1/10 scale car and a full-size van; no results have been reported back.

    OpenMowerNext: PR #45 integrates FusionCore as the localization stack for a community ROS 2 mowing system, replacing robot_localization with a single lifecycle node fusing RTK GPS (u-blox F9P), IMU and wheel odometry.


    Documentation

    manankharwar.github.io/fusioncore


    Contributing

    Bug reports are genuinely useful here and several have changed the library. If something misbehaves, the fastest route to an answer is usually the filter's own diagnostics, which the bug report template asks for:

    ros2 topic echo /fusion/debug/filter_health --once
    ros2 topic echo /fusion/debug/gnss_status --once
    

    If you would rather write code, the good first issues are scoped so you do not need to understand the estimator to pick one up. They point at the file and the line, and each one comes from a real failure rather than a wishlist. CONTRIBUTING.md has the build and test steps.


    License

    Apache 2.0, and it stays that way.


    Citation

    @article{kharwar2026fusioncore,
      author  = {Kharwar, Manan},
      title   = {FusionCore: A 23-State Unscented Kalman Filter for
                 IMU, Wheel Encoder, GPS, and Visual SLAM Fusion in ROS 2},
      journal = {arXiv preprint arXiv:2605.25239},
      year    = {2026},
      url     = {https://arxiv.org/abs/2605.25239}
    }
    

    To cite the software release itself:

    @software{kharwar2026fusioncore_software,
      author    = {Kharwar, Manan},
      title     = {FusionCore: ROS 2 UKF Sensor Fusion},
      year      = {2026},
      publisher = {Zenodo},
      doi       = {10.5281/zenodo.20091053},
      url       = {https://doi.org/10.5281/zenodo.20091053}
    }
    

    Bug reports with a log and a config get answered, usually the same day. If something behaves strangely, /fusion/debug/filter_health and tools/nis_from_bag.py will often say why before I can.

    Need it working on your hardware with someone accountable for the result? Commercial support and fixed-price integration.