Laajennettu Kalman Filter (EKF) on laajalti käytetty algoritmi robotti lokalisointi. Se arvioi robotin sijainti ja suunta yhdistämällä sensorin tiedot matemaattisen mallin robotti liike. Ymmärtäminen matemaattisia säätiöitä EKF on välttämätöntä tehokkaan täytäntöönpanon ja virityksen.

Valtion edustus ja ennuste

ECF:n ydin liittyy robotin tilan kuvaamiseen vektorina, tyypillisesti mukaan lukien sijainti ja suuntautuminen. Ennustusvaiheessa käytetään epälineaarista liikemallia projisoidakseen nykytilan eteenpäin ajassa. Tämä edellyttää motion mallin jakobianmatriisin laskemista lineaarisesti epälineaaristen yhtälöiden selvittämiseksi nykyisen arvion ympärille.

Mittauspäivitys ja lineaarisuus

Koska mittaukset ovat usein epälineaarisia toimintoja tilassa, EKF lineaarisoi nämä toiminnot käyttäen jakobianeja. Tämä prosessi säätää ennustetun tilan odotetun ja todellisen anturilukeman eron perusteella.

Matemaattiset yhtälöt

Ennusteyhtälöt ovat seuraavat:

Valtionennus:

x. k. 1− = f. x. k. uk.

Kovarianssien ennustus:

Pk+1− = Fk Pk Fkt + Qk

jossa f on ei-lineaarinen liikemalli, Fk on sen jakobialainen, P on kovavarianssimatriisi ja Q on prosessimelun munasarja.

Päivitysyhtälöt ovat:

[[LLT:0]]Kalman gain: [[LLT:1]]

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

Tilan päivitys:

x.k = x.k. +.k. (zk - h(x.k−))

[[LLT:0]]Korapin päivitys: [[LLT:1]]

Pk = (I - Kk Hk) Pk−

Tässä on epälineaarinen mittausmalli, Hk on sen jakobialainen, Rk on mittausmelun mittakoira ja zk on todellinen sensorin mittaus.