Extended Kalman Filter (EKF) er en mye brukt algoritme i robotlokalisering. Det anslår robotens posisjon og orientering ved å kombinere sensordata med en matematisk modell av robotens bevegelse. Å forstå matematiske grunnlag for EKF er avgjørende for effektiv implementering og tuning.

Statens representasjon og forutsigelse

Kjernen i EKF innebærer å representere robotens tilstand som vektor, typisk inkludert posisjon og orientering. Forutsigelsestrinnet bruker en ikke-lineær bevegelsesmodell for å projisere den aktuelle tilstanden fremover i tiden. Dette innebærer å beregne Jacobian matrisen til bevegelsesmodellen for å linearisere de ikke-lineære ligningene rundt det aktuelle estimatet.

Måleoppdatering og linearisering

Sensormålinger er inkorporert gjennom oppdateringstrinnet. Siden målinger ofte er ikke-lineære funksjoner av tilstanden, EKF lineariserer disse funksjonene ved hjelp av sine Jacobians. Denne prosessen justerer den forutspådde tilstanden basert på forskjellen mellom forventet og faktiske sensoravlesninger.

Matematiske likheter

Forutsigelseslikningene er:

Statsprediktasjon:

x ⁇ k+1 ⁇ = f(x ⁇ k, uk)

Kovariansprediktasjon:

Pk+1 = Fk Pk Fkt + Qk

hvor f er den ikke-lineære bevegelsesmodellen, er Fk dens Jacobian, P er kovariansmatrisen, og Q er prosessstøykovariansen.

Oppdateringslikningene er:

Kalman gevinst:

Kk = Pk ⁇ Hkt (Hk Pk ⁇ Hkt + Rk) ⁇ 1

Statsoppdatering:]

x ⁇ k = x ⁇ k ⁇ + Kk (zk - h(x ⁇ k ⁇ ))

Kovariansoppdatering:

Pk = (I - Kk Hk) Pk ⁇

Her er h den ikke-lineære målemodellen, Hk er dens Jacobian, Rk er målestøykovariansen, og zk er den faktiske sensormålingen.