Filtro di Kalman

Il filtro di Kalman (WB95) 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.

Per ragioni storiche il filtro di Kalman si riferisce propriamente al solo filtraggio di un sistema dove la transizione di stato e l'osservazione sono funzioni lineari dello stato corrente.

Seguendo la teoria dei sistemi lineari, la dinamica di un sistema “lineare” tempo continuo è rappresentata da una equazione differenziale del tipo

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

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)$$ (3.12)

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 tempo continuo in un sistema lineare del tipo

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

Se il sistema si evolve seguendo questo modello è chiamato Linear-Gaussian State Space Model o Linear Dynamic System. Se i valori delle matrici sono indipendenti dal tempo, il modello è chiamato stazionario.

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 matrici di covarianza rispettiva $\mathbf{Q}$ e $\mathbf{R}$ conosciute (si suppone rumore gaussiano bianco). $\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 $\mathbf{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 $\mathbf{z}_{k}$. Tutte queste matrici, rappresentanti il modello del sistema, devono essere conosciute con assoluta precisione, pena l'introduzione di errori sistematici.

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

Sia $\hat{\mathbf{x}}^{-}_{k}$ la stima a priori dello stato del sistema, basata sulla stima ottenuta al tempo $k-1$ e dalla dinamica del problema, e $\hat{\mathbf{x} }_{k}$ la stima dello stato del problema a posteriori dell'osservazione $\mathbf{z}_k$ e basata su di essa. Da queste definizioni è possibile definire l'errore della stima a priori e a a posteriori come

$$\begin{array}{l} \mathbf{e}^{-}_{k} = \mathbf{x}_k - \hat{\... ...bf{e}_{k} = \mathbf{x}_k - \hat{\mathbf{x}}_{k} \\ \end{array}$$ (3.14)

A questi errori è possibile associare

$$\begin{array}{l} \mathbf{P}^{-}_k = \E[\mathbf{e}^{-}_k {\m... ...\mathbf{P}_k = \E[\mathbf{e}_k \mathbf{e}^{\top}_k] \end{array}$$ (3.15)

le matrici di covarianza a priori e a posteriori rispettivamente.

L'obiettivo del filtro di Kalman è minimizzare la covarianza dell'errore a posteriori $\mathbf{P}_k$ e fornire un metodo per ottenere la stima di $\hat{\mathbf{x} }_{k}$ data la stima a priori $\hat{\mathbf{x}}^{-}_{k}$ e l'osservazione $\mathbf{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})$$ (3.16)

spostando il problema della stima a quello di 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 rappresenta la discrepanza tra l'osservazione predetta e quella realmente avvenuta. È da notare che la metrica usata per calcolare il residuo può dipendere dalle peculiarità del 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{\mathbf{x}}_k$ che della covarianza $\mathbf{P}_{k}$. La stima a priori $\hat{\mathbf{x}}^{-}_{k}$ viene dalla buona conoscenza della dinamica del sistema (3.13):

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

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

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

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

Nella seconda fase viene calcolato il guadagno

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

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

Usando questo valore per il guadagno $\mathbf{K}$, la stima a posteriori della matrice di covarianza diventa

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

Per poter unificare le diverse varianti dei filtri di Kalman si possono tradurre queste equazioni usando le matrici di varianza-covarianza

$$\begin{array}{l} \cov (x_k, z_k) = \mathbf{P}^{-}_{k} \mat... ...mathbf{H}_k \mathbf{P}^{-}_{k} \mathbf{H}_k^{\top} \end{array}$$ (3.21)

in modo da poter scrivere l'equazione (3.19) come

$$\mathbf{K}_k = \cov (x_k, z_k) \left( \cov (z_k) + \mathbf{R}_k \right)^{-1}$$ (3.22)

e, sostituendo le covarianze (3.21) in (3.20) si ottiene

$$\mathbf{P}_{k} = \mathbf{P}^{-}_{k} - \mathbf{K}_k \cov (x_k, z_k)^{\top}$$ (3.23)

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

Kalman richiede tuttavia un valore iniziale della variabile di stato e della matrice di covarianza: 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.