# Filtro di Kalman

Il filtro di Kalman è un efficiente filtro ricorsivo che valuta lo stato di un sistema dinamico a partire da una serie di misure soggette a rumore. Per le sue caratteristiche intrinseche è un filtro ottimo per rumori e disturbi agenti su sistemi gaussiani a media nulla. Trova utilizzo come osservatore dello stato, come loop transfer recovery (LTR) e come sistema di identificazione parametrica. Il problema della progettazione del filtro di Kalman è il problema duale del regolatore lineare quadratico (LQR).

## Storia

Il filtro prende il nome da Rudolf E. Kalman, sebbene in realtà Thorvald Nicolai Thiele e Peter Swerling abbiano sviluppato un algoritmo simile in precedenza. Stanley Schmidt è generalmente riconosciuto essere stato il primo a sviluppare una realizzazione pratica di un filtro di Kalman. Durante una visita di Kalman al Centro di Ricerche Ames della NASA Schmidt vide l'applicabilità delle idee di Kalman al problema della stima delle traiettorie per il programma Apollo e finì con l'includerlo nel programma del computer di bordo dell'Apollo. Il filtro fu sviluppato in articoli scientifici da Swerling (1958), Kalman (1960) e Kalman e Bucy (1961).
In seguito, partendo dalla formulazione originaria di Kalman che ora è chiamata il filtro semplice di Kalman, sono stati sviluppati numerosi tipi di filtro di Kalman; alcuni esempi sono il filtro esteso di Schmidt, il filtro di informazione e diversi filtri a radice quadrata sviluppati da Bierman, Thornton e molti altri. Anche l'anello ad aggancio di fase (PLL), circuito elettronico usato in innumerevoli applicazioni quali la radio, i calcolatori elettronici e la trasmissione dati, può essere considerato un filtro di Kalman.

## Filtro di Kalman a tempo continuo
### Definizione del problema

Si consideri l'applicazione del problema ad un generico sistema MIMO: dato un sistema dinamico lineare stazionario soggetto a rumore di processo ${\boldsymbol {v}}_{x}(t)$ e rumore di misura ${\boldsymbol {v}}_{y}(t)$, si scrivono le equazioni caratteristiche come:
    ${\begin{aligned}{\boldsymbol {\dot {x}}}(t)&=A{\boldsymbol {x}}(t)+B{\boldsymbol {u}}(t)+{\boldsymbol {v}}_{x}(t)\\{\boldsymbol {y}}(t)&=C{\boldsymbol {x}}(t)+D{\boldsymbol {u}}(t)+{\boldsymbol {v}}_{y}(t)\end{aligned}}$ con ${\boldsymbol {v}}(t)={\begin{bmatrix}{\boldsymbol {v}}_{x}(t)\\{\boldsymbol {v}}_{y}(t)\end{bmatrix}}$
dove i rumori ${\boldsymbol {v}}_{x}(t)$ e ${\boldsymbol {v}}_{y}(t)$ sono incorrelati nel tempo, congiuntamente gaussiani a media nulla. Senza troppa perdita di generalità si può considerare $D=0$ (caso di un sistema strettamente proprio). Dato il rumore, si scriva una matrice $V$ detta matrice di covarianza come:
    $V={\begin{bmatrix}{\tilde {Q}}&Z\\Z&{\tilde {R}}\end{bmatrix}}\,$
dove
    $Z=\mathrm {Cov} [{\boldsymbol {v}}_{x}(t),{\boldsymbol {v}}_{y}(t)]$
    ${\tilde {Q}}=\mathrm {Var} [{\boldsymbol {v}}_{x}(t)]$
    ${\tilde {R}}=\mathrm {Var} [{\boldsymbol {v}}_{y}(t)]$
Come ipotesi aggiuntive si prendano: ${\tilde {R}}\succ 0$ (definita positiva), ovvero la componente di rumore di misura è a covarianza non nulla su ogni uscita, $Z=0$, ovvero i rumori su stato e uscita sono incorrelati; lo stato è modellizzato come una variabile casuale gaussiana tale che:
    ${\boldsymbol {x}}_{0}={\boldsymbol {x}}(0)$
    $\mathrm {E} [{\boldsymbol {x}}(0)]={\overline {\boldsymbol {x}}}_{0}$
    $\mathrm {E} [({\boldsymbol {x}}_{0}-{\overline {\boldsymbol {x}}}_{0})({\boldsymbol {x}}_{0}-{\overline {\boldsymbol {x}}}_{0})']={\tilde {P}}\succeq 0$
e inoltre i rumori e lo stato sono incorrelati, ovvero:
    $\mathrm {E} [{\boldsymbol {x}}_{0}{\boldsymbol {v}}']=0$

#### Osservatore dello stato

Si consideri a questo punto l'osservatore
    ${\frac {\mathrm {d} {\boldsymbol {\hat {x}}}(t)}{\mathrm {d} t}}={\boldsymbol {\dot {\hat {x}}}}(t)=A{\boldsymbol {\hat {x}}}(t)+B{\boldsymbol {u}}(t)+L(t)[{\boldsymbol {y}}(t)-C{\boldsymbol {\hat {x}}}(t)]$
Da qui, con semplici passaggi algebrici per sostituzione, è possibile scrivere la dinamica dell'errore nel seguente modo:
    ${\boldsymbol {\dot {e}}}(t)={\boldsymbol {\dot {x}}}(t)-{\boldsymbol {\dot {\hat {x}}}}(t)$
    ${\boldsymbol {\dot {e}}}(t)=A_{c}(t){\boldsymbol {e}}(t)+B_{c}(t){\boldsymbol {v}}(t)$
dove si definiscono
    $A_{c}(t)=A-L(t)C$
    $B_{c}(t)=I-L(t)$
Scrivendo ora
    ${\overline {\boldsymbol {e}}}(t)=\mathrm {E} [{\boldsymbol {e}}(t)]$
    ${\overline {\boldsymbol {\dot {e}}}}(t)=A_{c}(t){\overline {\boldsymbol {e}}}(t)+B_{c}\mathrm {E} [{\boldsymbol {v}}(t)]=A_{c}(t){\overline {\boldsymbol {e}}}(t)$
Il valore atteso dell'errore segue una dinamica in evoluzione libera non forzata, come nel caso di un sistema autonomo. Si definisca in questo caso la matrice di covarianza dell'errore
    ${\tilde {P}}(t)=\mathrm {E} [{\boldsymbol {e}}(t){\boldsymbol {e}}(t)']$
    ${\tilde {P}}(0)={\tilde {P}}_{0}$
L'obiettivo di ottimizzazione è quindi trovare $L(t)$ che minimizza la cifra di merito
    $\min _{L(t)}{\boldsymbol {\gamma }}'{\tilde {P}}(t){\boldsymbol {\gamma }}=\min _{L(t)}\|{\boldsymbol {\gamma }}\|_{{\tilde {P}}(t)}^{2}$
con ${\boldsymbol {\gamma }}$ vettore generico di dimensioni opportune.

### Forma del filtro di Kalman

Si dimostra che il guadagno $L(t)$ che risolve il problema di ottimizzazione ha la forma
    $L(t)={\tilde {P}}(t)C'{\tilde {R}}^{-1}$
dove ${\tilde {P}}(t)$ è la soluzione dell'equazione di Riccati scritta nella forma
    ${\dot {\tilde {P}}}(t)=A{\tilde {P}}(t)+{\tilde {P}}(t)A'+{\tilde {Q}}-{\tilde {P}}(t)C'{\tilde {R}}^{-1}C{\tilde {P}}(t)$
con condizione iniziale
    ${\tilde {P}}(0)={\tilde {P}}_{0}$
Si tratta esattamente del caso duale del problema di ottimizzazione per il controllo LQ.

### Proprietà del filtro di Kalman

Si studiano qui le proprietà del filtro di Kalman su orizzonte temporale infinito. Un risultato importante riguarda la stabilità del filtro: si definisca la matrice $B_{q}$ come partizione della matrice ${\tilde {Q}}$, ovvero tale che
    $B_{q}B_{q}'={\tilde {Q}}$
Allora il filtro di Kalman risulta stabile se la coppia $(A,\ B_{q})$ è raggiungibile e se la coppia $(A,\ C)$ è osservabile.
Sotto queste ipotesi lo stimatore ottimo è:
    ${\boldsymbol {\dot {\hat {x}}}}(t)=A{\boldsymbol {\hat {x}}}(t)+B{\boldsymbol {u}}(t)+L^{*}[{\boldsymbol {y}}(t)-C{\boldsymbol {\hat {x}}}(t)]$
con
    $L^{*}={\tilde {P}}^{*}C'{\tilde {R}}^{-1}$
dove ${\tilde {P}}^{*}$ è l'unica soluzione definita positiva dell'equazione stazionaria di Riccati
    $0=A{\tilde {P}}^{*}+{\tilde {P}}^{*}A'+{\tilde {Q}}-{\tilde {P}}^{*}C'{\tilde {R}}^{-1}C{\tilde {P}}^{*}$

#### Stabilità asintotica

Sotto le ipotesi considerate il filtro di Kalman è asintoticamente stabile, in quanto si verifica che tutti i poli del filtro hanno parte reale negativa, ovvero gli autovalori della matrice $(A-L^{*}C)$ sono a parte reale negativa.

## Filtro di Kalman a tempo discreto
### Definizione del problema

Si consideri l'applicazione del problema a un generico sistema MIMO: dato un sistema dinamico lineare stazionario soggetto a rumore di processo ${\boldsymbol {v}}_{x}(k)$ e rumore di misura ${\boldsymbol {v}}_{y}(k)$, si scrivono le equazioni caratteristiche come:
    ${\begin{aligned}{\boldsymbol {x}}(k+1)&=A{\boldsymbol {x}}(k)+B{\boldsymbol {u}}(k)+{\boldsymbol {v}}_{x}(k)\\{\boldsymbol {y}}(k)&=C{\boldsymbol {x}}(k)+{\boldsymbol {v}}_{y}(k)\end{aligned}}$ con ${\boldsymbol {v}}(k)={\begin{bmatrix}{\boldsymbol {v}}_{x}(k)\\{\boldsymbol {v}}_{y}(k)\end{bmatrix}}$
dove i rumori ${\boldsymbol {v}}_{x}(k)$ e ${\boldsymbol {v}}_{y}(k)$ sono incorrelati, gaussiani a media nulla.
Ne risultano le formule (stima filtrata dello stato, varianza dell'errore di stima filtrata, predittore dello stato, varianza dell'errore di predizione):
    ${\begin{aligned}{\boldsymbol {\hat {x}}}(k|k)&={\boldsymbol {\hat {x}}}(k|k-1)+L(k)e(k)\\P(k|k)&=\left(I-L(k)C\right)P(k|k-1)\\{\boldsymbol {\hat {x}}}(k+1|k)&=F{\boldsymbol {\hat {x}}}(k|k)+ZR^{-1}y(k)\\P(k+1|k)&=FP(k|k)F'+M\\\end{aligned}}$
Dove:
    ${\begin{aligned}L(k)&=P(k|k-1)C'[CP(k|k-1)C'+R]^{-1}\\e(k)&=y(k)-C{\boldsymbol {\hat {x}}}(k|k-1)\\N(k)&=CP(k|k-1)C'+R\\F&=A-ZR^{-1}C\\M&=Q-ZR^{-1}Z'\end{aligned}}$