⚡ electrical engineering

FusionCore: A 23-State Unscented Kalman Filter for IMU, Wheel Encoder, GPS, and Visual SLAM Fusion in ROS 2

FusionCore is een open-source ROS 2-pakket dat een 23-toestands Unscented Kalman-filter toepast om IMU-, wielencoder-, GPS- en Visual SLAM-gegevens robuust te fuseren tot een hoogfrequente odometrie-stroom, waarmee aanzienlijk lagere trajectfouten en een grotere numerieke stabiliteit worden bereikt dan het standaard robot_localization-pakket over diverse NCLT-datasetsequenties.

Oorspronkelijke auteurs: Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

Gepubliceerd 2026-05-26

📖 6 min leestijd🧠 Diepgaand

CC BY 4.0

Oorspronkelijke auteurs: Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

Oorspronkelijk artikel gelicentieerd onder CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ Dit is een AI-gegenereerde uitleg van het onderstaande artikel. Het is niet geschreven of goedgekeurd door de auteurs. Raadpleeg het oorspronkelijke artikel voor technische nauwkeurigheid. Lees de volledige disclaimer

Stel je voor dat je probeert een robot door een stad te navigeren, maar je bent geblinddoekt, je oren zijn bedekt en je kunt alleen raden waar je bent door te voelen hoe de wielen draaien. Dat is in wezen wat robots moeten doen wanneer ze proberen hun locatie te bepalen. Ze moeten gegevens uit verschillende "zintuigen" combineren om een helder beeld te krijgen van waar ze zich bevinden.

Dit artikel introduceert FusionCore, een nieuwe softwaretool die is ontworpen als het "brein" dat deze zintuigen combineert voor robots die het ROS 2-systeem gebruiken. Hieronder wordt uitgelegd hoe het werkt, met behulp van eenvoudige analogieën.

Het Probleem: Het "Ruizige" Team

Robots hebben meestal drie hoofdmanieren om hun locatie te raden:

Wielenencoders (De Stappenteller): Telt hoe vaak de wielen ronddraaien. Het is geweldig voor korte afstanden, maar raakt in de war als de wielen slippen op ijs of modder.
GPS (De Satellietkaart): Vertelt je precies waar je je op aarde bevindt. Maar het is traag, verdwijnt soms (zoals wanneer je onder een brug door rijdt) en kan af en toe een "glitch" geven die zegt dat je je in een andere stad bevindt.
IMU (Het Oor): Een sensor die versnelling en draaiing voelt. Het is zeer snel, maar na verloop van tijd begint het te "driften" (zoals een tol die langzaam kantelt), waardoor de robot denkt dat hij beweegt terwijl hij stil staat.

Bestaande software (genaamd robot_localization) probeert deze signalen te mengen, maar heeft een paar gebreken:

Het behandelt de "drift" in de sensoren als een vaste fout, niet als iets dat in de tijd verandert.
Het raakt in de war wanneer GPS-signalen over kaartgrenzen heen springen.
Het heeft moeite om het verschil te zien tussen een echte GPS-update en een gekke glitch.

De Oplossing: FusionCore's "Slimme Brein"

FusionCore is een nieuw "brein" (een wiskundig filter) dat al deze signalen samenvoegt tot één enkele, vloeiende, hoogwaardige stroom van locatiegegevens (100 keer per seconde). Het gebruikt een 23-dimensionale "toestand" om alles bij te houden, inclusief sommige dingen die andere systemen negeren.

Hier zijn de belangrijkste trucs die FusionCore gebruikt, uitgelegd met metaforen:

1. De "Drift-Detective" (Online Bias Estimation)

Stel je voor dat je loopt met een vriend die een lichte hink heeft. Als je de hink niet opmerkt, wordt je kaart van waar je bent langzaam onjuist.

Oude manier: De software gaat ervan uit dat de hink vaststaat en negeert deze.
FusionCore-methode: Het heeft een "Drift-Detective" in zich. Het houdt de sensoren voortdurend in de gaten en vraagt: "Glijden de wielen? Wordt de gyroscoop moe?" Het berekent het exacte bedrag aan "hink" (bias) in real-time en trekt dit af. Dit houdt de robot uit de war, zelfs als GPS een tijdje weg is.

2. Het "Globale Adres" (ECEF-Native GPS)

GPS-coördinaten worden meestal omgezet in lokale kaarten (zoals UTM-zones). Als je over een zonegrens rijdt, kan de kaart glitchen en crashen.

FusionCore-methode: In plaats van lokale straatadressen te gebruiken, spreekt het de taal van de hele aarde (ECEF). Het behandelt de planeet als één grote bol, zodat het nooit in de war raakt door kaartgrenzen. Het is alsof je breedte- en lengtegraden gebruikt voor de hele wereld in plaats van elke 10 mijl over te schakelen naar een nieuwe stadskaart.

3. De "Leugenopsporing" (Adaptive Outlier Gating)

Soms stuurt GPS een bericht met de tekst: "Ik ben nu in de oceaan!" terwijl de robot duidelijk op een weg staat.

Oude manier: De software gebruikt een starre regel: "Als de fout groter is dan 5 meter, negeer het." Maar als de GPS eigenlijk gewoon een beetje ruis heeft, kan het goede data negeren.
FusionCore-methode: Het gebruikt een "Leugenopsporing" die leert. Het kijkt naar de geschiedenis van fouten. Als de GPS meestal nauwkeurig is maar plotseling een slechte dag heeft, verzwakt de detector zijn greep. Als de GPS zich gek gedraagt, slaat hij de deur dicht. Het past zijn eigen gevoeligheid automatisch aan, zodat het niet bedrogen wordt door glitches of goede data afwijst.

4. De "Tijdsreiziger" (Delay Compensation)

GPS-gegevens komen vaak een fractie van een seconde te laat aan, omdat de satelliet eerst wiskunde moet doen voordat het signaal wordt verzonden.

FusionCore-methode: Het houdt een "geheugenbuffer" (een ringbuffer) van de laatste seconde aan IMU-gegevens bij. Wanneer het late GPS-bericht eindelijk arriveert, past de software dit niet alleen toe op het huidige moment. Het "spoelt terug" naar het exacte moment waarop het GPS-signaal werd genomen, past de correctie toe en "spoelt dan voor" terug naar nu. Dit zorgt ervoor dat de positie van de robot perfect gesynchroniseerd is, zelfs met trage sensoren.

5. Het "Visuele Geheugen" (VSLAM Fusion)

Als de robot GPS verliest (zoals het rijden in een tunnel), kan het een camera (Visual SLAM) gebruiken om kenmerken in de kamer te volgen.

FusionCore-methode: Het kan naadloos overschakelen naar het beeld van de camera. Als de camera zijn weg kwijtraakt en zijn kaart moet "herstarten", weet FusionCore hoe het de positie van de robot terug kan brengen naar de realiteit zonder dat de robot in paniek raakt of crasht.

De Resultaten: De Grote Test

De auteur testte FusionCore tegen de standaardsoftware (robot_localization) met behulp van 12 lange rijsequenties (tot wel 92 minuten elk) uit een publieke dataset.

De Winnaar: FusionCore won in 10 van de 12 races.
De Marge: In sommige gevallen was het 22 keer nauwkeuriger dan de standaardsoftware.
De Verliezers: De geavanceerde versie van de standaardsoftware (UKF) crashte (divergeerde) bij alle 12 sequenties, wat betekent dat het de positie van de robot volledig uit het oog verloor.
Waarom FusionCore 2 Races Verloor:
1. De Lange Blackout: In één race was het GPS-signaal bijna 8 minuten weg. Zelfs de beste detective kan niet perfect raden zonder kaart voor zo'n lange tijd, en de robot hinkte iets.
2. De "Adversariële" Glitch: In een andere race stuurde de GPS een cluster leugens dat zo overtuigend was dat het filter bedrogen werd. De standaardsoftware slaagde er per ongeluk in deze af te wijzen omdat zijn regels te streng waren, terwijl FusionCore probeerde te behulpzaam te zijn en bedrogen werd. De auteur merkt op dat dit specifieke, oplosbare problemen zijn.

Samenvatting

FusionCore is een slimmere, aanpasbaardere manier om robots te helpen weten waar ze zijn. Het behandelt sensorfouten als levende dingen die in de tijd veranderen, leert leugens te negeren en onthoudt het verleden om het heden te corrigeren. Hoewel het niet perfect is, presteert het in de meeste realistische scenario's aanzienlijk beter dan de huidige industriestandaard.

Technische Samenvatting: FusionCore

Probleemstelling

Nauwkeurige pose-schatting is fundamenteel voor autonome robotnavigatie, maar de praktische implementatie van sensorfusiesystemen blijft uitdagend. Bestaande oplossingen kampen met verschillende kritieke beperkingen:

Bias-drift: IMU-gyroscopen en versnellingsmeters vertonen continue bias-drift, en yaw-rate-biasen van wielencoders (systematische afwijkingen bij differentieelaandrijving) worden zelden als filtertoestanden behandeld, wat leidt tot niet-observeerbare accumulatie van koersfouten tijdens GPS-uitval.
Architecturale fragiliteit: Dominante ROS-pakketten zoals robot_localization vertrouwen op aparte nodes voor GPS-coördinaattransformatie (UTM), wat fouten bij zonegrenzen introduceert. Ze maken ook gebruik van vaste ruiscovarianties en scalare uitschieterrejectiondrempels die geen rekening houden met de dimensie van de meting.
Operationele gaten: Huidige systemen hebben moeite met opereren zonder GPS, herstel na herinitialisatie van de kaart, en de integratie van diverse sensoren zoals Visual SLAM (VSLAM) en radar zonder complexe coördinatie.

Methodologie

FusionCore is een open-source ROS 2-pakket dat een 23-toestands Unscented Kalman Filter (UKF) implementeert dat IMU, wielencoder-odometrie, GPS en Visual SLAM-pose fuseert tot één enkele odometrie-stroom van 100 Hz.

Kernarchitectuur

Toestandsvector: Het filter onderhoudt een 23-dimensionale toestandsvector die positie ( $p$ ), oriëntatie ( $q$ ), lineaire/hoeksnelheid ( $v, \omega$ ), lineaire versnelling ( $a$ ), gyroscopenbias ( $b_g$ ), versnellingsmeterbias ( $b_a$ ) en een kritieke yaw-rate-bias van de wielencoder ( $b_{ewz}$ ) bevat.
Bias-schatting: In tegenstelling tot eerdere benaderingen zijn gyroscopen-, versnellingsmeter- en encoder-yaw-biasen volwaardige filtertoestanden met dedicated procesruis. Dit stelt het filter in staat om online te convergeren naar sensor-specifieke bias-waarden zonder een aparte kalibratiefase.
GPS-fusie: Het systeem fuseert GPS-gegevens natiu in het Earth-Centered, Earth-Fixed (ECEF)-stelsel, en converteert pas naar ENU voor de filtertoestand. Dit elimineert de behoefte aan een aparte navsat_transform-node en voorkomt fouten bij UTM-zonegrenzen.
Meetmodellen:
- IMU: Verwerkt 6-DOF ruwe data en 2/3-DOF oriëntatie-updates, rekening houdend met zwaartekracht en bias.
- Wielencoders: Trekt de geschatte $b_{ewz}$ af van de gerapporteerde hoeksnelheid voordat integratie plaatsvindt. Handhaaft niet-holonome beperkingen (nul verticale snelheid en versnelling) met adaptieve ruis.
- VSLAM: Accepteert 6-DOF pose-metingen van elk visueel odometrie/SLAM-systeem, inclusief automatisch herstel na herinitialisatie van de kaart door de transformatie opnieuw te verankeren na opeenvolgende uitschieterrejecties.
- Snelheid: Ondersteunt GPS-Dopplersnelheid en Radar-Doppler eigen-snelheid voor slipdetectie.

Innovaties in Filterontwerp

Per-sensor Chi-Kwadraat-gating: Implementeert Mahalanobis-afstand-uitschieterrejectie met drempels die specifiek zijn gekalibreerd voor de vrijheidsgraden (DOF) van elk sensorpad (bijv. 3-DOF GPS versus 1-DOF koers), in plaats van één enkele scalare drempel te gebruiken.
Adaptieve ruiscovariantie: Past de sensorruiscovariantie ( $R$ ) automatisch in real-time aan met behulp van een schuivend venster van innovatiereeksen en een exponentieel bewegend gemiddelde, waardoor handmatig opnieuw afstemmen door omgevingsveranderingen overbodig wordt.
Vertragingcompensatie: Gebruikt een ringbuffer van 1 seconde voor de IMU om retrodictie uit te voeren, waarbij vertraagde GPS- of VSLAM-metingen worden toegepast op de juiste historische toestand voordat opnieuw vooruit wordt gepropageerd.
Zero-Velocity Updates (ZUPT): Detecteert stationaire periodes om bias-drift te onderdrukken.
Coast-modus: Tijdens GPS-blackouts wordt de geschatte encoder-yaw-bias afgetrokken van de encoder-data om koersdrift te verminderen.

Belangrijkste Bijdragen

Het artikel presenteert tien specifieke bijdragen:

Een 23-toestands UKF-formulering met continue online schatting van gyroscopen-, versnellingsmeter- en wielencoder-yaw-rate-biasen.
ECEF-native GPS-fusie, waardoor afhankelijkheden van UTM-projectie worden verwijderd.
Per-sensor Mahalanobis chi-kwadraat uitschietergating met dimensie-gekalibreerde drempels.
Automatische aanpassing van ruiscovariantie vanuit innovatiereeksen.
Automatische Zero-Velocity Updates (ZUPT) voor stationaire periodes.
IMU-ringbuffer-retrodictie voor vertragingcompensatie.
Visual SLAM-pose-fusie met automatisch herstel na herinitialisatie van de kaart.
GPS-snelheidsfusie voor cross-sensor slipdetectie.
Radar-Doppler eigen-snelheidsfusie voor 4D-imaging radarplatforms.
Een kwantitatieve evaluatie tegen robot_localization op twaalf volledige NCLT-sequenties.

Evaluatie en Resultaten

Het systeem is geëvalueerd op twaalf volledige sequenties (55–92 minuten elk) uit de NCLT-public dataset, die diverse seizoensomstandigheden bestrijken. De evaluatie vergeleek FusionCore met de robot_localization EKF en UKF met behulp van de RMSE van de Absolute Trajectfout (ATE).

Prestaties: FusionCore behaalde een lagere ATE dan de robot_localization EKF op 10 van de 12 sequenties. De verbeteringen op de winnende sequenties varieerden van 1,2× tot 22,2×.
Stabiliteit: De robot_localization UKF divergeerde numeriek (NaN) op alle twaalf sequenties binnen de eerste 30 seconden, wat bekende stabiliteitsproblemen met hoge frequentie-invoer bevestigt.
Foutanalyse:
- 2012-06-15 (FC-verlies): FusionCore verloor van de robot_localization EKF (49,2m vs 18,2m) door een GPS-blackout van 7,7 minuten. Het artikel schrijft dit toe aan de beperkingen van encoder-gebaseerde dode loop-rekening over zulke lange duur zonder een absolute koersreferentie (bijv. magnetometer).
- 2012-08-20 (FC-verlies): FusionCore verloor significant (98,3m vs 10,6m) door een "adversariele GPS-cluster" van 105 fixes die 720–840m van de ground truth verschenen bij een blackout-grens. Het artikel merkt op dat de verkeerd gekalibreerde gate van robot_localization deze uitschieters toevallig verwierp, terwijl de versoepelde gate van FusionCore (noodzakelijk voor herstel) ze accepteerde. De auteurs identificeren een snelheidsconsistentie-voorcontrole als oplossing.
Covariantie-mismatch: Het artikel benadrukt dat de afhankelijkheid van robot_localization van vaste, vaak onnauwkeurige GPS-covariantierapporten leidt tot afwijzing van geldige data en een instorting naar dode loop-rekening. Het adaptieve ruismodel van FusionCore mitigeert dit door het ruismodel op te blazen wanneer innovatiefouten de gerapporteerde covarianties overschrijden.

Betekenis en Claims

Het artikel beweert dat FusionCore de praktische beperkingen van bestaande ROS 2 sensorfusiestacks aanpakt door een enkele lifecycle-node te bieden die geen externe coördinatoren vereist. De betekenis ligt in:

Robuustheid: Het behalen van superieure trajectnauwkeurigheid in langdurige, real-world scenario's waar bestaande UKF-implementaties numeriek falen.
Aanpasbaarheid: Omgaan met dynamische sensorkwaliteit en GPS-covariantie-mismatches door adaptieve ruis en dimensie-bewuste gating.
Operationele continuïteit: Het mogelijk maken van opereren zonder GPS en naadloos herstel na herinitialisatie van VSLAM-kaarten.
Reproduceerbaarheid: Het bieden van een volledig open-source implementatie met benchmark-scripts en configuraties voor standaardplatforms (TurtleBot3, Husky) en integratie met rtabmap_ros.

De auteurs concluderen dat hoewel specifieke faalmodi (lange blackouts, adversariele GPS-clusters) bestaan, deze diagnoseerbaar zijn en dat ze architecturale oplossingen (integratie van magnetometer, snelheidsvoorcontroles) hebben geïdentificeerd voor toekomstig werk.