Wiskundige basis van uitgebreide Kalmanfilters in robotlocalization

Het Extended Kalman Filter (EKF) is een veelgebruikt algoritme in robotlokalisatie. Het schat de positie en oriëntatie van de robot door sensorgegevens te combineren met een wiskundig model van de beweging van de robot. Het begrijpen van de wiskundige grondslagen van EKF is essentieel voor een effectieve implementatie en tuning.

Vertegenwoordiging en voorspelbaarheid van de staat

De kern van EKF bestaat uit het representeren van de toestand van de robot als vector, meestal inclusief positie en oriëntatie. De voorspellingstap gebruikt een niet-lineair bewegingsmodel om de huidige toestand vooruit te projecteren in de tijd. Dit houdt in dat de Jacobiaanse matrix van het bewegingsmodel wordt berekend om de niet-lineaire vergelijkingen rond de huidige schatting te lineariseren.

Meetupdate en -linearisatie

Sensormetingen worden opgenomen door middel van de updatestap. Aangezien metingen vaak niet-lineaire functies van de staat zijn, EKF lineariseert deze functies met behulp van hun Jacobianen. Dit proces past de voorspelde toestand aan op basis van het verschil tussen verwachte en werkelijke sensormetingen.

Wiskundige vergelijkingen

De voorspellingsvergelijkingen zijn:

State prediction:

x

Covariumvoorspelling:

Pk+1− = Fk Pk Fkt + Qk

waar f het niet-lineaire bewegingsmodel is, is Fk de Jacobiaanse, P de covariummatrix en Q is het procesgeluids-covarium.

De updatevergelijkingen zijn:

Kalman winst:

Kk = Pk− Hkt (Hk Pk− Hkt + Rk) -1

State update:

x

Covariumupdate:

Pk = (I - Kk Hk) Pk−

Hier is h het niet-lineaire meetmodel, Hk is zijn Jacobiaanse, Rk is de meetgeluids-coovarium, en zk is de werkelijke sensormeting.