Filtro di Kalman

Il filtro di Kalman [34] cerca di stimare in presenza di disturbi lo stato interno $\mathbf{x} \in \mathbb{R}^{n}$, non accessibile, di un sistema tempo discreto, la cui conoscenza del modello è completa. Di fatto il filtro di Kalman è lo stimatore ricorsivo ottimo: se il rumore del problema è gaussiano, il filtro di Kalman fornisce la stima ai minimi quadrati dello stato interno del sistema.

Il processo di trovare la miglior stima dello stato interno viene chiamato "`filtraggio"' in quanto è un metodo per filtrare via il rumore.

Seguendo la teoria dei sistemi lineari, la dinamica di un sistema è rappresentata da una equazione differenziale del tipo

$$\dot{\mathbf{x}} = \mathbf{A}(t) \mathbf{x}(t) + \mathbf{B} \mathbf{u}(t) + \mathbf{w}(t)$$ (2.41)

equazione di aggiornamento dello stato, a cui è associata un'osservazione indiretta di questo stato attraverso un sistema lineare:

$$\mathbf{z}(t) = \mathbf{H}(t) \mathbf{x}(t) + \mathbf{v}(t)$$ (2.42)

con $\mathbf{z} \in \mathbf{R}^{m}$ l'osservabile.

Il filtro di Kalman a tempo discreto viene in aiuto dei sistemi reali dove il mondo viene campionato a intervalli discreti, trasformando il sistema lineare in un sistema del tipo

$$\left\{ \begin{array}{l} \mathbf{x}_{k+1} = \mathbf{A}_{k} \mathb... ... \mathbf{z}_k = \mathbf{H}_k \mathbf{x}_{k} + \mathbf{v}_{k} \end{array}\right.$$ (2.43)

Le variabili $w_{k}$ e $v_{k}$ rappresentano rispettivamente il rumore di processo e di osservazione, valor medio nullo $\bar{w_k}=\bar{v_k}=0$ e varianza rispettiva $\mathbf{Q}$ e $\mathbf{R}$ conosciute (si suppone rumore gaussiano bianco), e solitamente ottenere la stima di $\mathbf{Q}$ risulta molto più difficile che ottenere la stima di $\mathbf{R}$. Le due variabili di rumore devono essere scorrelate tra di loro. $\mathbf{A}$ è una matrice $n \times n$ di transizione dello stato, $\mathbf{B}$ è una matrice $n \times l$ che collega l'ingresso di controllo opzionale $u \in \mathbb{R}^{l}$ con lo stato $\mathbf{x}$ e infine $\mathbf{H}$ è una matrice $m \times n$ che collega lo stato con la misura $z_{k}$. Tutte queste matrici devono essere conosciute.

Il filtro di Kalman è un filtro di stima ricorsivo e richiede a ogni iterazione la conoscenza dello stato stimato dal passo precedente $\hat{\mathbf{x}}_{k-1}$ e la corrente osservazione $z_k$ del sistema.

Definiamo $\hat{\mathbf{x}}^{-}_{k}$ la stima a priori dello stato del sistema, basata sulla stima ottenute al tempo $k-1$ e dalla dinamica del problema, e $\hat{x}_{k}$ la stima dello stato del problema a posteriori basato anche sull'osservazione $z_k$. In particolare da quest'ultima affermazione è possibile definire l'errore della stima a priori e a a posteriori come

$$\begin{array}{l} e^{-}_{k} = x_k - \hat{x}^{-}_{k} e_{k} = x_k - \hat{x}_{k} \end{array}$$ (2.44)

A questi errori è possibile associare

$$\begin{array}{l} P^{-}_k = \mathbb{E}[e^{-}_k {e^{-}_k}^{\top}] P_k = \mathbb{E}[e_k e^{\top}_k] \end{array}$$ (2.45)

le matrici di covarianza a priori e a posteriori rispettivamente.

L'obiettivo del filtro di Kalman è minimizzare la covarianza dell'errore a posteriori $P_k$ e fornire un metodo per ottenere la stima di $\hat{x}_{k}$ data la stima a priori $\hat{x}^{-}_{k}$ e l'osservazione $z_k$.

Il filtro di Kalman fornisce una stima dello stato a posteriori attraverso una combinazione lineare della stima dello stato precedente e dell'errore di osservazione:

$$\hat{\mathbf{x}}_{k} = \hat{\mathbf{x}}^{-}_{k} + \mathbf{K}_k( \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x} }^{-}_{k})$$ (2.46)

spostando il problema sul ricavare il fattore di guadagno $\mathbf{K}_k$ (blending factor). La differenza $\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x} }^{-}_{k}$ è chiamata residuo, o innovation, e rappresenza la discrepanza tra l'osservazione predetta e quella realmente avvenuta (di fatto la metrica usata per calcolare il residuo può essere dipendente dal problema).

Il filtro di Kalman viene normalmente presentato in due fasi: aggiornamento del tempo (fase di predizione) e aggiornamento della misura (fase di osservazione).

Nella prima fase si ottiene la stima a priori sia di $\hat{x}_k$ che della covarianza $P_{k}$. La stima a priori $\hat{\mathbf{x}}^{-}_{k}$ viene dalla buona conoscenza della dinamica del sistema 2.43:

$$\hat{\mathbf{x}}^{-}_{k} = \mathbf{A} \hat{\mathbf{x}}_{k-1} + \mathbf{B} \mathbf{u}_{k}$$ (2.47)

e allo stesso modo viene aggiornata la stima a priori della covarianza dell'errore:

$$\mathbf{P}^{-}_{k} = \mathbf{A} \mathbf{P}_{k-1} \mathbf{A}^{\top} + \mathbf{Q}_k$$ (2.48)

Queste sono le miglior stime dello stato e della covarianza dell'istante $k$ ottenibili a priori dell'osservazione del sistema.

Nella seconda fare viene calcolato il guadagno

$$\mathbf{K}_k = \mathbf{P}^{-}_{k} \mathbf{H}_k^{\top} \left( \mathbf{H}_k \mathbf{P}^{-}_{k} \mathbf{H}_k^{\top} + \mathbf{R}_k \right)^{-1}$$ (2.49)

che minimizza la covarianza a posteriori, e con questo fattore viene aggiornato lo stato a posteriori attraverso l'equazione 2.46.

Infine, grazie al guadagno appena ricavato, viene calcolata la stima a posteriori della matrice di covarianza:

$$\mathbf{P}_{k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}^{-}_{k}$$ (2.50)

Come si può facilmente notare la matrice di covarianza e il guadagno di kalman non dipendono minimamente dallo stato ne dalle osservazioni ne tantomeno dal residuo, e hanno una storia indipendente.

Il valore iniziale dello stato deve essere il più simile possibile al valore vero e la somiglianza a questo valore va inserita nella matrice di covarianza iniziale.