Fondazioni matematiche di Filtri Kalman Estesi nella localizzazione robot

Il filtro Kalman esteso (EKF) è un algoritmo ampiamente utilizzato nella localizzazione dei robot, che stima la posizione e l'orientamento del robot combinando i dati dei sensori con un modello matematico del movimento del robot.

Rappresentanza e Predizione dello Stato

Il nucleo di EKF consiste nel rappresentare lo stato del robot come vettore, tipicamente compreso posizione e orientamento. Il passo di previsione utilizza un modello di movimento non lineare per proiettare lo stato attuale in avanti nel tempo. Ciò comporta il calcolo della matrice giacobica del modello di movimento per linearizzare le equazioni non lineari intorno alla stima attuale.

Aggiornamento di misurazione e linearizzazione

Le misurazioni dei sensori sono incorporate attraverso la fase di aggiornamento, poiché le misurazioni sono spesso funzioni non lineari dello stato, EKF linearizza queste funzioni utilizzando i loro giacobini.

Equazioni matematiche

Le equazioni di previsione sono:

Previsione di stato:[]

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

Previsione della frequenza:

Pk+1− = Fk Pk Fkt + Qk

dove f è il modello di movimento non lineare, Fk è il suo Jacobian, P è la matrice di covarianza, e Q è la covarianza di processo rumore.

Le equazioni di aggiornamento sono:

Credi di Kalman:

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

Aggiornamento di stato:[

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

Aggiornamento di frequenza:[

Pk = (I - Kk Hk) Pk−

Qui, h è il modello di misura non lineare, Hk è il suo Jacobian, Rk è la covarianza di rumore di misura, e zk è la misurazione del sensore reale.