Calcolo passo-passo delle matrici di covarianza in Ekf-slam

Il filtro Kalman esteso (EKF) Simultaneous Localization and Mapping (SLAM) è una tecnica utilizzata nella robotica per costruire una mappa di un ambiente sconosciuto, determinando simultaneamente la posizione del robot all'interno di esso.

Inizializzazione della Matrice di Covariance

Il processo inizia con inizializzazione della matrice di covarianza, tipicamente denotata come P. Questa matrice combina le incertezze della posa del robot e le caratteristiche della mappa. La covarianza iniziale riflette la fiducia iniziale nella posizione iniziale del robot e le caratteristiche note.

Passo di pre-

Durante la fase di previsione, il modello di movimento del robot viene utilizzato per stimare il nuovo stato. La matrice di covarianza viene aggiornata utilizzando il Jacobian del modello di movimento, indicato come F, e la covarianza di rumore di processo, Q. L'aggiornamento segue:

P[_{]] [}] = F P F^]T[ + Q

Aggiornamento Passo con Misure

Quando vengono ricevute nuove misurazioni dei sensori, la matrice di covarianza viene aggiornata per incorporare queste informazioni. Il modello di misura Jacobian, H e la covarianza di rumore di misura, R, sono utilizzati per calcolare il guadagno Kalman, K:

K = P_] ^{]] [} [[_]] ] ^]]] + R]^{-1[F][FLT][F][F]][F]][F[F[FLT]]]]]]][}