IEKF e ISPKF

Una delle principali limitazioni del filtro di Kalman Esteso consiste nel fatto che la linearizzazione della funzione di osservazione viene effettuata una sola volta, attorno alla stima a priori $\hat{\mathbf{x}}^{-}$. Se la funzione di osservazione risulta fortemente non lineare oppure se l'errore iniziale della stima è elevato, una singola linearizzazione può risultare insufficiente e introdurre errori significativi.

Dal punto di vista dell'aggiornamento della misura, il filtro di Kalman esteso può essere interpretato come una singola iterazione del metodo di Gauss–Newton applicata al problema di stima dello stato. È quindi naturale estendere il procedimento ripetendo più volte la fase di osservazione, mantenendo invariata la fase di predizione. Ad ogni iterazione viene costruita una nuova approssimazione locale del problema attorno alla stima corrente, ottenendo la classe dei filtri di Kalman iterativi.

L'unica differenza rispetto ai rispettivi filtri non iterativi è nella fase di osservazione (cfr. equazione (3.32)), sostituita da una procedura iterativa della forma

$$\mathbf{x}_{i+1} = \hat{\mathbf{x}} + \mathbf{K} \Bigl(... ...}_i) - \mathbf{H}_i (\hat{\mathbf{x}}-\mathbf{x}_i) \Bigr)$$ (3.38)

con il guadagno di Kalman aggiornato ad ogni iterazione come

$$\mathbf{K} = \mathbf{P} \mathbf{H}_i^\top \left( \mathbf{H}_i \mathbf{P} \mathbf{H}_i^\top + \mathbf{R} \right)^{-1}$$ (3.39)

e usando come valore iniziale

$$\mathbf{x}_0 = \hat{\mathbf{x}}^{-}.$$ (3.40)

Il valore del guadagno associato all'ultima iterazione viene infine utilizzato per aggiornare la matrice di covarianza dello stato.

Lo stesso procedimento può essere applicato ai filtri Sigma-Point, ottenendo l'Iterated Sigma Point Kalman Filter (ISPKF) (SSM06), nel quale l'aggiornamento iterativo dello stato assume la forma

$$\mathbf{x}_{i+1} = \hat{\mathbf{x}} + \mathbf{K} \left... ...p} \mathbf{P}^{-1} (\hat{\mathbf{x}}-\mathbf{x}_i) \right).$$ (3.41)

L'iterazione viene normalmente arrestata quando la variazione dello stato tra due iterazioni successive scende sotto una soglia prefissata, oppure dopo un numero massimo di iterazioni. I filtri iterativi richiedono un costo computazionale maggiore rispetto alle versioni standard, ma permettono generalmente una migliore convergenza nei problemi caratterizzati da forti non linearità.