Voetgangersnavigatie met één bewegingssensor
Kun je bijhouden waar iemand loopt, zonder GPS, alleen met een kleine bewegingssensor op de voet? Dat was de vraag in onze eindopdracht. In deze notitie leg ik uit hoe het werkt, wat we bouwden en, eerlijk, wat er misging en wat ik daarvan heb geleerd.
Het idee
We gebruikten een Raspberry Pi met een Sense HAT. Dat is een insteekbordje met onder andere een versnellingsmeter en een gyroscoop: een IMU. Met die sensor op de voet liepen we ruim twaalf minuten rond en sloegen we elke meting op, zo'n 24 keer per seconde.
In theorie is het simpel. Tel je de versnelling op, dan krijg je de snelheid. Tel je de snelheid op, dan krijg je de afgelegde weg. De gyroscoop vertelt welke kant je op draait. Zo zou je de hele route moeten kunnen terugrekenen.
Het probleem: drift
In de praktijk loopt dat binnen een paar seconden volledig uit de hand. Elke sensor heeft een kleine fout, en door twee keer op te tellen groeit die fout razendsnel. In mijn notitie over hoe een INS werkt liet ik al zien dat zelfs een dure sensor zonder hulp snel wegloopt. Met een goedkope sensor als deze gaat het nog veel sneller.
Hieronder twee stukjes van tien seconden uit onze echte data. Bovenaan de snelheid als je gewoon optelt: na tien seconden denkt de berekening dat de voet met bijna 70 m/s beweegt, ruim 240 km/u. Onderaan dezelfde tien seconden met de truc die ik hieronder uitleg.
De truc: elke stap staat je voet even stil
Bij het lopen staat je voet bij elke stap even plat op de grond. Op dat moment weet je zeker dat de snelheid nul is. Dat is gratis informatie, en het heet een zero velocity update, kortweg ZUPT.
Het filter herkent die stilstand aan de gyroscoop: als de voet nauwelijks draait en de versnellingsmeter alleen de zwaartekracht voelt, staat de voet stil. Op dat moment zet het de berekende snelheid terug naar nul. Omdat het filter ook bijhoudt hoe fouten samenhangen, corrigeert het meteen ook een deel van de fout in de positie en de stand.
Je ziet het ritme van het lopen: een piek bij elke stap, en daartussen een rustig stuk waar de voet op de grond staat.
Het filter: Bayes in een notendop
Het rekenwerk doet een Kalmanfilter (hoe dat werkt, leg ik uit in het Kalmanfilter, simpel uitgelegd). Dat houdt negen dingen bij: de fout in de stand (3), in de positie (3) en in de snelheid (3). Bij elke meting voorspelt het filter hoe die fouten groeien. Bij elke stilstand stuurt het bij.
Een Kalmanfilter is eigenlijk de regel van Bayes, toegepast op normale verdelingen. Je hebt een voorspelling met een bepaalde onzekerheid, de prior. Je hebt een meting met een eigen onzekerheid, de likelihood. Vermenigvuldig je die, dan krijg je een nieuwe schatting die beter is dan allebei: de posterior. Speel er hieronder zelf mee.
Maak de meting onzekerder en de nieuwe schatting schuift naar de voorspelling. Maak de voorspelling onzekerder en hij schuift naar de meting. Dat is precies wat de docent ons vroeg te laten zien: hoe de keuze van prior en likelihood het resultaat bepaalt.
| Instelling in ons filter | Waarde | Wat het betekent |
|---|---|---|
| Toestanden | 9 | Fout in stand, positie en snelheid, elk in drie richtingen |
| Ruis gyroscoop en versnellingsmeter | 0,01 | Hoeveel we de sensoren per stap wantrouwen |
| Ruis van de nul-snelheidsmeting | 0,01 m/s | Hoe zeker we zijn dat de voet echt stilstaat |
| Drempel stilstand | draaisnelheid onder een vaste grens | Wanneer het filter aanneemt dat de voet op de grond staat |
Het tweede deel: data als een stroom
In het tweede deel van de opdracht ging het om de architectuur: hoe stuur je sensordata live door, zodat een ander systeem er meteen mee kan rekenen? We kozen Apache Kafka. Dat is software die berichten in een soort wachtrij zet, waar andere programma's ze weer uit kunnen halen.
Het voordeel van zo'n opzet: de sensor hoeft niet te weten wie de data gebruikt. Je kunt later een kaart, een alarm of een opslag aansluiten zonder iets aan de sensor te veranderen. Eerlijk is eerlijk: onze versie was nog niet werkend af. De onderdelen stonden er, maar ze werkten nog niet goed samen.
Terugblik: wat er misging
Dit was het resultaat dat we inleverden. Het moest een wandeling voorstellen, maar de route beslaat ruim twee kilometer in twaalf minuten, met lange rechte lijnen. Dat klopt natuurlijk niet.
Bij het schrijven van deze notitie ben ik de data opnieuw gaan bekijken. Toen vielen drie dingen op die we destijds hadden moeten controleren:
- De tijd sprong elke minuut terug. De tijdkolom telde alleen de seconden binnen een minuut, dus na 59,99 kwam weer 0,01. Voor het filter betekende dat elke minuut een tijdstap van min zestig seconden. Daar komen die lange rechte lijnen vandaan.
- De versnelling stond in g, niet in m/s². De sensor geeft 1 voor de zwaartekracht, terwijl onze code 9,8 aftrok. Alles was dus bijna tien keer te klein, en de zwaartekracht werd verkeerd weggehaald.
- De gyro-bias was veel groter dan we dachten. Bij stilstand gaf de gyroscoop op één as nog ongeveer 0,4 aan, terwijl wij met een bias rond 0,01 rekenden.
Met alleen die drie dingen aangepast, en verder hetzelfde filter, ziet de route er zo uit:
Dat lijkt al veel meer op lopen: een doorlopend pad van ongeveer 1,3 kilometer in twaalf en een halve minuut, in de orde van grootte van gewoon lopen. Maar ik weet niet of dit ook de echte route is. Daarvoor ontbreekt het belangrijkste: een referentie.
Wat ik ervan heb geleerd
- Controleer eerst je eenheden en tijdstempels. Voordat je een filter bouwt, kijk je of de tijd oploopt en of de zwaartekracht 9,81 is. Dat kost vijf minuten en had ons alles gescheeld.
- Meet altijd een referentie. Loop een route die je kent, bijvoorbeeld een rechthoek met bekende maten of een rondje terug naar het startpunt. Als landmeter had ik dit moeten zien aankomen: zonder controlepunten weet je niet of je meting klopt.
- Een plausibel plaatje is geen bewijs. De nieuwe route ziet er geloofwaardig uit, maar het filter is gevoelig voor de instellingen. Met een andere drempel voor stilstand komt het eindpunt tientallen meters anders uit.
- Meet sneller. 24 metingen per seconde is weinig voor een voet die bij elke stap een fractie van een seconde stilstaat. Systemen voor voetnavigatie meten meestal minstens honderd keer per seconde.
Bronnen: Roger Labbe, Kalman and Bayesian Filters in Python; Zrenner e.a., Does the Position of Foot-Mounted IMU Sensors Influence the Accuracy of Spatio-Temporal Parameters in Endurance Running?, Sensors, 2020; M.T. Koroglu, Multiple Hypothesis Testing Approach to Pedestrian Inertial Navigation. De opnieuw berekende route is van oktober 2026.