Filtro di Kalman

Il filtro prende il nome da Rudolf E. Kalman, sebbene in realtà Thorvald Nicolai Thiele^[1] 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.

Si consideri l'applicazione del problema ad un generico sistema MIMO: dato un sistema dinamico lineare stazionario soggetto a rumore di processo ${\displaystyle {\boldsymbol {v}}_{x}(t)}$  e rumore di misura ${\displaystyle {\boldsymbol {v}}_{y}(t)}$ , si scrivono le equazioni caratteristiche come:

${\displaystyle {\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    ${\displaystyle {\boldsymbol {v}}(t)={\begin{bmatrix}{\boldsymbol {v}}_{x}(t)\\{\boldsymbol {v}}_{y}(t)\end{bmatrix}}}$ 

dove i rumori ${\displaystyle {\boldsymbol {v}}_{x}(t)}$  e ${\displaystyle {\boldsymbol {v}}_{y}(t)}$  sono incorrelati nel tempo, congiuntamente gaussiani a media nulla. Senza troppa perdita di generalità si può considerare ${\displaystyle D=0}$  (caso di un sistema strettamente proprio). Dato il rumore, si scriva una matrice ${\displaystyle V}$  detta matrice di covarianza come:

${\displaystyle V={\begin{bmatrix}{\tilde {Q}}&Z\\Z&{\tilde {R}}\end{bmatrix}}\,}$ 

dove

${\displaystyle Z=\mathrm {Cov} [{\boldsymbol {v}}_{x}(t),{\boldsymbol {v}}_{y}(t)]}$ 

${\displaystyle {\tilde {Q}}=\mathrm {Var} [{\boldsymbol {v}}_{x}(t)]}$ 

${\displaystyle {\tilde {R}}=\mathrm {Var} [{\boldsymbol {v}}_{y}(t)]}$ 

Come ipotesi aggiuntive si prendano: ${\displaystyle {\tilde {R}}\succ 0}$  (definita positiva), ovvero la componente di rumore di misura è a covarianza non nulla su ogni uscita, ${\displaystyle Z=0}$ , ovvero i rumori su stato e uscita sono incorrelati; lo stato è modellizzato come una variabile casuale gaussiana tale che:

${\displaystyle {\boldsymbol {x}}_{0}={\boldsymbol {x}}(0)}$ 

${\displaystyle \mathrm {E} [{\boldsymbol {x}}(0)]={\overline {\boldsymbol {x}}}_{0}}$ 

${\displaystyle \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:

${\displaystyle \mathrm {E} [{\boldsymbol {x}}_{0}{\boldsymbol {v}}']=0}$ 

Si consideri a questo punto l'osservatore

${\displaystyle {\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:

${\displaystyle {\boldsymbol {\dot {e}}}(t)={\boldsymbol {\dot {x}}}(t)-{\boldsymbol {\dot {\hat {x}}}}(t)}$ 

${\displaystyle {\boldsymbol {\dot {e}}}(t)=A_{c}(t){\boldsymbol {e}}(t)+B_{c}(t){\boldsymbol {v}}(t)}$ 

dove si definiscono

${\displaystyle A_{c}(t)=A-L(t)C}$ 

${\displaystyle B_{c}(t)=I-L(t)}$ 

Scrivendo ora

${\displaystyle {\overline {\boldsymbol {e}}}(t)=\mathrm {E} [{\boldsymbol {e}}(t)]}$ 

${\displaystyle {\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

${\displaystyle {\tilde {P}}(t)=\mathrm {E} [{\boldsymbol {e}}(t){\boldsymbol {e}}(t)']}$ 

${\displaystyle {\tilde {P}}(0)={\tilde {P}}_{0}}$ 

L'obiettivo di ottimizzazione è quindi trovare ${\displaystyle L(t)}$  che minimizza la cifra di merito

${\displaystyle \min _{L(t)}{\boldsymbol {\gamma }}'{\tilde {P}}(t){\boldsymbol {\gamma }}=\min _{L(t)}\|{\boldsymbol {\gamma }}\|_{{\tilde {P}}(t)}^{2}}$ 

con ${\displaystyle {\boldsymbol {\gamma }}}$  vettore generico di dimensioni opportune.

Si dimostra che il guadagno ${\displaystyle L(t)}$  che risolve il problema di ottimizzazione ha la forma

${\displaystyle L(t)={\tilde {P}}(t)C'{\tilde {R}}^{-1}}$ 

dove ${\displaystyle {\tilde {P}}(t)}$  è la soluzione dell'equazione di Riccati scritta nella forma

${\displaystyle {\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

${\displaystyle {\tilde {P}}(0)={\tilde {P}}_{0}}$ 

Si tratta esattamente del caso duale del problema di ottimizzazione per il controllo LQ.

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 ${\displaystyle B_{q}}$  come partizione della matrice ${\displaystyle {\tilde {Q}}}$ , ovvero tale che

${\displaystyle B_{q}B_{q}'={\tilde {Q}}}$ 

Allora il filtro di Kalman risulta stabile se la coppia ${\displaystyle (A,\ B_{q})}$  è raggiungibile e se la coppia ${\displaystyle (A,\ C)}$  è osservabile.

Sotto queste ipotesi lo stimatore ottimo è:

${\displaystyle {\boldsymbol {\dot {\hat {x}}}}(t)=A{\boldsymbol {\hat {x}}}(t)+B{\boldsymbol {u}}(t)+L^{*}[{\boldsymbol {y}}(t)-C{\boldsymbol {\hat {x}}}(t)]}$ 

con

${\displaystyle L^{*}={\tilde {P}}^{*}C'{\tilde {R}}^{-1}}$ 

dove ${\displaystyle {\tilde {P}}^{*}}$  è l'unica soluzione definita positiva dell'equazione stazionaria di Riccati

${\displaystyle 0=A{\tilde {P}}^{*}+{\tilde {P}}^{*}A'+{\tilde {Q}}-{\tilde {P}}^{*}C'{\tilde {R}}^{-1}C{\tilde {P}}^{*}}$ 

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 ${\displaystyle (A-L^{*}C)}$  sono a parte reale negativa.

Si consideri l'applicazione del problema a un generico sistema MIMO: dato un sistema dinamico lineare stazionario soggetto a rumore di processo ${\displaystyle {\boldsymbol {v}}_{x}(k)}$  e rumore di misura ${\displaystyle {\boldsymbol {v}}_{y}(k)}$ , si scrivono le equazioni caratteristiche come:

${\displaystyle {\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    ${\displaystyle {\boldsymbol {v}}(k)={\begin{bmatrix}{\boldsymbol {v}}_{x}(k)\\{\boldsymbol {v}}_{y}(k)\end{bmatrix}}}$ 

dove i rumori ${\displaystyle {\boldsymbol {v}}_{x}(k)}$  e ${\displaystyle {\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):

${\displaystyle {\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:

${\displaystyle {\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}}}$ 

• Magni L., Scattolini R., Complementi di controlli automatici, Pitagora Editrice, Bologna, 2006.
• Colaneri P., Locatelli A., Controllo robusto in RH2/RH, Pitagora Editrice, Bologna, 1993.
• K. Zhou, J. C. Doyle, K. Glover, Robust and optimal control, Prentice Hall, 1996.
• P. Dorato, C. Abdallah, V. Cerone Linear quadratic control: an introduction, Prentice Hall, 1995.

• Regolatore lineare quadratico (LQR)
• Loop transfer recovery (LTR)
• Controllo LQG
• Controllo robusto
• Sistema dinamico
• Sistema dinamico lineare stazionario
• Controlli automatici
• Equilibrio di un sistema dinamico.
• Controllabilità di un sistema dinamico.
• Identità di Woodbury

•   Wikimedia Commons contiene immagini o altri file sul filtro di Kalman

• (EN) Eric W. Weisstein, Kalman Filter, su MathWorld, Wolfram Research.  

  Portale Ingegneria

  Portale Matematica