必须设置 CMAKE_BUILD_TYPE 为 Release,未优化构建会显著变慢;仅用 6 轴 IMU、轮速和 GPS 位置时航向不可观测,需加磁力计或双天线 GNSS;平滑型 GNSS 接收机的卡方门限实际灵敏度低于标称值,建议先用 nis_from_bag.py 实测。
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.
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.bashREADME 说明可在 Raspberry Pi 4 上运行,Release 构建下每周期远低于 1 ms,同一份源码用于 ARM 与 x86;未给出显存或具体 GPU 型号要求。
面向会丢 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 无需真值即可检查传感器配置问题
容易踩的坑
- 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_robotorground_robot_flat, those profiles now turn on GNSS re-anchoring, which can make the published position jump after a GPS outage. Setgnss.reanchor_enabled: falseto keep 0.4.x behaviour; an explicit false wins over the profile. Nothing changes forplatform.profile: general, the default.vslam.frame_idwas 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 mScored 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.pyreads 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_loston/diagnosticsand 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
- v0.4.00.4.02026-09-15 · 2026-09-15 03:26 UTC
查看更新说明
First release with breaking changes. Two public
FusionCoreConfigfields were removed, so code built directly againstfusioncore_corewill need a change, and four defaults now alter behaviour for anyone upgrading without touching their config. Nothing errors and every YAML still loads.Breaking
setting was now effect gnss.recovery_rejection_n0, off 15 post-blackout P inflation fires gnss.continuity_autonew truefix-to-fix gate arms itself after 100 fixes zupt.accel_std_thresholdnew 0.5 the IMU can veto a ZUPT gnss.gps_track_heading_cross_check_degnew 15.0 a disagreeing GPS track heading is refused Removed from
FusionCoreConfig:gnss_recovery_timeout_s(declared, documented, set in a shipped config, never read by anything) andencoder_nhc_vy_sigma(written by the node into a field nothing consumed, while the working assignment reachedencoder.vel_noise_y). The ROS parametergnss.recovery_timeout_sis still declared so old configs load, and the node now warns once if it is set.Rejection reason codes are unchanged.
NOT_FINITEandQUALITY_OTHERwere appended at 12 and 13, andMIN_SATSkept 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 mThe 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_satellitescan no longer silently reject everyNavSatFix. 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
- 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_healthcarries 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.
ACCEPTEDis counted alongside the rejections, which is what makes an empty result readable: ifgnss:ACCEPTEDis 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_xyandgnss.min_sigma_zset a floor, in metres, on the sigma a receiver reports before it becomesR.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_xyof 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
Ris built from that number and the chi2 gate judges every fix against the sameR. 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,0produced 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_managercannot own FusionCore. It requires abondheartbeat that FusionCore does not implement, so the manager reports a bond timeout on every boot regardless ofbond_timeout. FusionCore being a lifecycle node makes putting it under the manager look correct, andgetting-started.mdwas actively suggesting it. Both that page andnav2.mdnow say not to, and name the two approaches that work.- Fixes the receiver marked
NO_FIXwere dropped without a trace, counted nowhere, so a receiver that had lost fix looked identical to one working normally. filter_healthpublished 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_distancewas 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.pynow 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 - An unmeasured antenna lever arm was the one setting FusionCore said nothing about. The startup log printed it only when non-zero, so
- 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 documentedcolcon buildwas 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 existingbuild/keeps the old binary:rm -rf build install && colcon buildFor 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.shhad 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'sautostartin 0.3.1, the launch files'autoconfigurein 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:=falsenow also clears the node'sautostart. If you drive the lifecycle externally, from anav2_lifecycle_manageror by hand, you now getunconfigured -> inactive -> activein 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.cppmeasures NIS and NEES in simulation;tools/nis_from_bag.pymeasures 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_cppall 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.
- 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.alphawas 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), withW[0] = lambda/(n+lambda)andlambda = alpha^2*(n+kappa) - n. FusionCore has 23 states and useskappa = 0, so withalpha = 0.1that works out tolambda = -22.77and 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.00Yaw 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.0giveslambda = 0and all 47 weights non-negative. That is the standard unscaled UKF. Worth stating clearly, because it is counterintuitive: withkappa = 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 mThe 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
c8b8b1ffrom 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.jsonpredates 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
用户点评
登录用户的真实使用记录,可按版本和硬件筛选还没有人点评。第一个说出真实体验的人最有价值。
FusionCore
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 problem | How FusionCore handles it |
|---|---|
| IMU calibration is approximate | Gyro and accel bias are filter states, estimated continuously. init.stationary_window: 2.0 estimates startup bias before motion begins. |
| Extrinsic calibration is never exact | Reads 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 is | Stamps 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 slipping | Adaptive 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 tuning | Two 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 Pi | Well 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 platform | Set imu2.topic to fuse a second IMU as an independent measurement. No pre-merging needed. |
| GPS drops out under canopy | Inertial 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 minutes | ZUPT 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:
| Tuning | Two 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 drifted | Every 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 it | The 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 typo | ROS 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 all | tools/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:
Sequence Set Raw GPS Default, ROS node Default, replay Re-anchoring on, replay Re-anchoring on, ratio to raw GPS 2012-01-08 held out 9.883 m 56.957 m 50.065 m 10.899 m 1.103x 2012-02-04 dev 57.564 m 163.830 m 145.629 m 59.340 m 1.031x 2012-03-31 held out 13.825 m 16.818 m 16.937 m 10.787 m 0.780x 2012-05-11 held out 9.187 m 9.186 m 9.187 m 9.187 m 1.000x 2012-06-15 dev 9.293 m 24.363 m 12.041 m 12.041 m 1.296x 2012-08-20 dev 34.250 m 160.359 m 172.961 m 39.790 m 1.162x 2012-09-28 dev 8.960 m 18.883 m 22.236 m 10.176 m 1.136x 2012-10-28 held out 13.923 m 15.999 m 16.763 m 13.150 m 0.944x 2012-11-04 held out 20.267 m 23.626 m 24.512 m 20.414 m 1.007x 2012-12-01 held out 12.681 m 16.160 m 16.466 m 12.741 m 1.005x 2013-02-23 held out 15.724 m 17.294 m 16.951 m 15.793 m 1.004x 2013-04-05 dev 8.624 m 9.994 m 10.007 m 8.798 m 1.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(version803c158e99ff, printed on every row) throughhardware/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 intools/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.yamlas 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):
Sequence Set Points scored Raw GPS robot_localization, node FusionCore default, node FusionCore default, node repeat FusionCore default, replay FusionCore re-anchoring, replay 2012-01-08 held out 10167 9.878 / 6.768 31.647 / 6.176 56.969 / 13.800 57.666 / 13.837 50.075 / 12.352 10.895 / 6.816 2012-02-04 dev 8905 57.564 / 13.600 145.976 / 36.984 163.830 / 42.020 150.519 / 36.416 145.629 / 35.612 59.340 / 13.519 2012-03-31 held out 10046 13.824 / 7.469 160.874 / 42.830 16.816 / 7.357 15.886 / 7.336 16.934 / 7.376 10.785 / 7.413 2012-05-11 held out 10384 9.187 / 5.210 8.387 / 4.425 9.186 / 5.267 9.184 / 5.251 9.187 / 5.217 9.187 / 5.217 2012-06-15 dev 6526 9.293 / 4.891 45.941 / 16.375 24.363 / 7.846 23.366 / 7.931 12.041 / 4.900 12.041 / 4.900 2012-08-20 dev 9799 34.250 / 7.319 10.400 / 4.451 160.359 / 35.210 159.581 / 35.923 172.961 / 38.082 39.790 / 7.495 2012-09-28 dev 8404 8.960 / 6.430 95.140 / 26.585 18.883 / 6.859 18.875 / 6.863 22.236 / 7.140 10.176 / 6.554 2012-10-28 held out 8901 13.923 / 8.251 25.929 / 6.679 15.999 / 8.399 15.997 / 8.386 16.763 / 8.436 13.150 / 8.335 2012-11-04 held out 8200 20.267 / 9.657 24.170 / 7.834 23.626 / 10.220 24.733 / 10.268 24.512 / 10.301 20.414 / 9.806 2012-12-01 held out 7838 12.681 / 7.842 12.528 / 5.421 16.160 / 7.910 16.135 / 7.884 16.466 / 7.859 12.741 / 7.948 2013-02-23 held out 8569 15.724 / 9.451 77.986 / 24.301 17.294 / 9.650 17.300 / 9.633 16.951 / 9.630 15.793 / 9.441 2013-04-05 dev 7458 8.624 / 6.319 8.329 / 4.449 9.994 / 6.457 10.020 / 6.462 10.007 / 6.447 8.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
| Problem | How FusionCore handles it |
|---|---|
| A GPS outlier corrupts the state | Chi-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 area | GPS is fused directly in ECEF. No UTM projection and no zone boundary case. |
| A wheeled robot drifts sideways without GPS | Non-holonomic constraint zeros lateral and vertical velocity as a virtual measurement on every encoder update. |
| GPS fixes arrive 50 to 200 ms late | IMU ring buffer with retrodiction reconstructs the exact filter state at the GPS timestamp. |
| Bag replay gives different results each run | Message 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 name | TF lookup on every message, with an imu.frame_id override. |
| navsat_transform startup ordering and CPU cost | No 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
- Getting Started
- Configuration reference
- Hardware configs
- Nav2 integration
- Migrating from robot_localization
- Is your filter's covariance honest?
- Known limitations
- How it works
- Troubleshooting
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.