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

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.99)

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.$$ (2.100)

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 varianza 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}$$ (2.101)

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}$$ (2.102)

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})$$ (2.103)

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 (2.100):

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

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.105)

Queste sono le miglior 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}$$ (2.106)

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

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}$$ (2.107)

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}$$ (2.108)

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

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

e, sostituendo le covarianze (2.108) in (2.107) si ottiene

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

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.

Filtro di Kalman monodimensionale

È interessante mostrare, come esempio, il caso semplificato di filtro di Kalman di stato monodimensionale e coincidente con l'osservabile. Le equazioni di transizione e di osservazioni sono

$$\begin{array}{l} x_{i} = x_{i-1} + u_{i} + w_{i} \\ z_{i} = x_{i} + v_{i} \end{array}$$ (2.111)

dove $w_i$ è il rumore di processo la cui varianza $q_i$ rappresenta la stima della probabilità di variazione del segnale stesso (bassa se il segnale varia poco nel tempo, alta se il segnale varia molto) mentre $v_i$ è il rumore di osservazione di varianza $r_i$, rumore associato all'osservazione dello stato.

Il ciclo di predizione è molto semplice e diventa:

$$\begin{array}{l} x^{-}_{i} = x_{i-1} + u_{i}\\ p^{-}_{i} = p_{i-1} + q_i \end{array}$$ (2.112)

Il guadagno di Kalman $k$ diventa

$$k_i = \frac{p^{-}_{i}}{p^{-}_{i} + r_i}$$ (2.113)

e infine la fase di osservazione diventa

$$\begin{array}{l} x_{i} = x^{-}_{i} + k_i (z_i - x^{-}_i) =... ...(1 - k_i) x^{-}_i \\ p_{i} = (1 - k_i) p^{-}_{i} \end{array}$$ (2.114)

È solitamente possibile stimare a priori il valore di $r$, mentre quello di $q$ va impostato attraverso esperimenti.

Come si vede nella prima delle equazioni (2.114), il fattore $k$ è di fatto un blending factor tra l'osservazione dello stato e lo stato stimato precedente.

Nel caso monodimensionale è facile vedere come il guadagno $k$ e la varianza $p$ sono processi indipendenti dallo stato e dalle osservazioni, tanto meno dall'errore. Se $r$ e $q$ non variano nel tempo, $k$ e $p$ sono sequenze numeriche che convergono a un numero costante determinato solamente dalla caratterizzazione del rumore, indipendentemente dai valori assunti all'inizio. Si confronti questo risultato con quello che si ottiene dall'equazione (2.70).