Es5: LOCALIZZAZIONE CON FILTRO DI KALMAN ESTESO
CORRISPONDENZE NOTE
Sfortunatamente nella realtà le funzioni che descrivono la probabilità di transizione dello stato x
dall'istante t-1 all'istante t p(x
t
| u
t
, x
t-1
), dove u
t
è il controllo all'istante t, e la probabilità della
misura z all'istante t p(z
t
| x
t
), non sono quasi mai lineari, ma sono del tipo:
ε
t
è il rumore gaussiano di processo con media nulla e varianza R
t
; δ
t
è il rumore gaussiano di
misura con media nulla e varianza Q
t
.
Il filtro di Kalman esteso EKF (Extended Kalman Filter),
rispetto al filtro di Kalman, permette di gestire anche questo tipo di funzioni, linearizzandole con
una retta tangente nel punto corrispondente alla media della gaussiana da trasformare, in modo tale
che la funzione risultante dalla trasformazione sia ancora una gaussiana. La tecnica utilizzata per la
linearizzazione è l'espansione di Taylor:
G
t
e H
t
sono le matrici jacobiane rispettivamente delle funzioni g ed h. L'algoritmo implementato
per la localizzazione con il filtro di Kalman esteso, di cui sotto si riporta il diagramma UML delle
classi, a partire dalla conoscenza dello stato all'istante precedente, caratterizzato da media μ
t-1
e
varianza Σ
t-1
, dal controllo e dalla misura all'istante t, dalla mappa m e dal vettore delle
corrispondenze c
t
, che permette di stabilire appunto una corrispondenza univoca tra un landmark
rilevato ed un landmark nella mappa, calcola la stima dello stato all'istante t. L'algoritmo si
scompone essenzialmente in due fasi principali. Nella prima fase, dopo aver scomposto linearmente
il modello del moto in una componente senza rumore ed in una componente con rumore, calcola lo
stato predetto in base alla sola informazione del controllo. Nella seconda fase, nota come fase di
aggiornamento o passo di correzione, per ogni landmark rilevato all'istante t (nota la sua
corrispondenza nella mappa grazie al vettore c
t
) calcola la misura predetta e la matrice jacobiana
della misura ed aggiorna di conseguenza lo stato stimato.
