Filtro di Kalman - Soluzione - Dipartimento di Ingegneria dell

ELABORAZIONE dei SEGNALI nei SISTEMI di CONTROLLO
Lezione 22:
Filtro di Kalman - Soluzione
• Soluzione
• Equazione di Riccati
• Filtro di Kalman e Osservatore
• Un problema di ottimo
21-1
Soluzione (1/2)
PROPAGAZIONE DELLA STIMA DELLO STATO
• Aggiornamento alla misura
x̂(t|t) = x̂(t|t − 1) + L(t)e(t) ,
e(t) = y(t) − Cx̂(t|t − 1)
(1)
L(t) = P (t|t −
1)C ′G−1 (t)
,
G(t) = CP (t|t −
1)C ′
+R
• Aggiornamento temporale
x̂(t + 1|t) = Ax̂(t|t) + Bu(t)
(2)
1. Il processo e(t) viene chiamato processo delle innovazioni del processo y(t). e(t) è
△
un processo bianco, a media nulla, e con varianza V ar(e(t)) = G(t).
2. La matrice L(t) viene chiamata Guadagno (del Filtro) di Kalman. Esso dipende
dalla matrice di varianza P (t|t − 1).
3. L’aggiornamento della stima alla misura dipende dal valore assunto dal guadagno di
Kalman L(t) e dall’errore di predizione della misura e(t). Se la misura viene predetta
esattamente (e(t) = 0), essa è ridondante e non concorre a modificare la stima dello
stato.
21-2
Soluzione (2/2)
PROPAGAZIONE DELLA MATRICE DI VARIANZA
• Aggiornamento alla misura
P (t|t) = P (t|t − 1) − P (t|t − 1)C ′ (CP (t|t − 1)C ′ + R)−1 CP (t|t − 1)
(3)
• Aggiornamento temporale
P (t + 1|t) = AP (t|t)A′ + DQD′
(4)
Osservazione - L’equazione di aggiornamento di P (t|t − 1) alla misura può essere riscritta
come:
P −1(t|t) = P −1(t|t − 1) + C ′ R−1C
(5)
Questa equazione descrive l’evoluzione della inversa della matrice P (t|t − 1) chiamata matrice di informazione I(t|t − 1). Formalmente questa ricorsione in I assomiglia alla formula
di aggiornamento temporale per P .
21-3
Equazione di Riccati
Le due equazioni di aggiornamento (al tempo e alla misura) possono essere fuse insieme
ottenendo:
x̂(t + 1|t) = A x̂(t|t − 1) + B u(t) + K(t) (y(t) − Cx̂(t|t − 1)) =
= (A − K(t)C) x̂(t|t − 1) + B u(t) + K(t) y(t)
(6)
K(t) = AL(t) = AP (t|t −
1)C ′G−1(t),
G(t) = R + CP (t|t −
1)C ′
P (t + 1|t) = AP (t|t − 1)A′ + DQD′ − AP (t|t − 1)C ′ G−1(t) CP (t|t − 1)A′
La prima equazione è inizializzata da x̂(1|0) = m0; l’ultima da P (1|0) = P0 .
Quest’ultima equazione viene chiamata Equazione Ricorsiva di Riccati. Da essa dipende
il valore assunto dal Guadagno di Kalman.
Anche per sistemi stazionari, la matrice
P (t|t − 1) e quindi il guadagno K(t) sono tempo-varianti.
Il guadagno del predittore K(t) e il guadagno del filtro L(t) sono legati dalla relazione
K(t) = AL(t).
La matrice A − K(t)C = A (I − L(t)C) rappresenta la matrice della dinamica del filtro.
21-4
Predizione a più passi in avanti e interpolazione
• Predizione a più passi. È facile verificare che per L = 0, 1, . . .:
x̂(k + L + 1|k) = A x̂(k + L|k)
(7)
P (k + L + 1|k) = A P (k + L|k)
A′
+
DQD′
• Interpolazione. È più difficile dimostrare che per k > t:
x̂(t|k) = x̂(t|k − 1) + K(k)e(k)
e(k) = y(k) − Cx̂(k|k − 1)
(8)
K(k) = P (k|k − 1)C ′ (CP (k|k − 1)C ′ + R)−1
P (k + 1|k) = (A − K(k)C) P (k|k − 1)
mentre K(k) e P (k|k − 1) rappresentano il guadagno di Kalman e la matrice di varianza
del filtro di Kalman base.
Si noti come ogni misura successiva all’istante t contribuisca a modificare la stima interpolata di x(t) in maniera proporzionale all’innovazione della misura stessa calcolata dal filtro
di Kalman base. Il peso da assegnare all’innovazione è dato dal guadagno dell’interpolatore
K(k) che differisce dal guadagno di Kalman K(k).
21-5
Il Filtro di Kalman e l’Osservatore (1/2)
Un classico problema della Teoria dei Sistemi è il seguente: dato il sistema dinamico lineare
x(t + 1) = Ax(t) + Bu(t)
(9)
y(t) = Cx(t),
in cui lo stato iniziale non è accessibile, determinare un algoritmo che, elaborando le
uscite, fornisca una approssimazione sempre migliore dello stato corrente x(t).
La soluzione è fornita dall’osservatore (ricostruttore dello stato):
x̂(t + 1) = Ax̂(t) + Bu(t) + K (y(t) − Cx̂(t))
(10)
La scelta del guadagno K che pesa la differenza fra l’uscita del sistema e l’uscita dell’osser△
vatore avviene tenendo conto che l’errore di ricostruzione x
e(t) = x(t) − x̂(t) evolve secondo
l’equazione:
x
e(t + 1) = (A − KC) x
e(t)
(11)
L’asintotica stabilità della matrice A − KC garantisce il raggiungimento dell’obiettivo. La
struttura del ricostruttore è riportata nello schema seguente.
21-6
Struttura dell’Osservatore.
Il Filtro di Kalman e l’Osservatore (2/2)
Confrontando l’osservatore con il filtro di Kalman si nota che essi hanno lo stessa struttura.
Struttura del Filtro di Kalman.
21-8
Tuttavia l’evoluzione dell’errore di stima x
e(t|t − 1) = x(t) − x̂(t|t − 1) è aleatoria dovendo
tener conto dei disturbi di processo e di misura:
x
e(t + 1|t) = (A − K(t)C) x
e(t|t − 1) + Dw(t) − K(t)v(t)
(12)
Adesso il guadagno K(t) non influenza solo la dinamica del filtro A − K(t)C, ma anche
l’amplificazione del disturbo di misura v(t).
Bisogna operare un compromesso tra dinamica rapida (alti valori del guadagno) e reiezione
del disturbo di misura (bassi valori del guadagno).
Nel Filtro di Kalman, la scelta del
guadagno K(t) avviene in modo da minimizzare, istante per istante, la varianza di x
e(t+1|t).
Un problema di ottimo
Sia {x̂(t|N );
t = 1, . . . , N } la successione di stime interpolate dello stato, fornita dal filtro
di Kalman, basata sulle misure y(1), . . . , y(N ).
Si può dimostrare che questa successione è la soluzione del seguente problema di ottimo:
n
ks(1) − m0k2P −1 +
{x̂(t|N ), t = 1, . . . , N } = arg min{s(t),t=1,...,N }
0
+
PN −1
t=1
ks(t + 1) − As(t) − Bu(t)k2Q−1
+
PN
t=1 ky(t)
− Cs(t)k2R−1
o
(13)
In sostanza la successione {x̂(t|N ), t = 1 . . . , N } è fra tutte le successioni {s(1), . . . , s(N )}
quella che meglio interpola una combinazione pesata dei seguenti termini:
• l’incertezza iniziale, rendendo piccolo il termine ks(1) − m0 k2P −1
0
• le misure, rendendo piccolo il termine ky(t) − Cs(t)k2R−1 (rappresenta l’errore di interpolazione della t-esima misura);
• la dinamica, rendendo piccolo il termine ks(t + 1) − As(t) − Bu(t)k2Q−1 che rappresenta
l’errore di interpolazione della dinamica al passo t.
21-10
Si noti che questi ultimi due errori sono pesati in base all’inverso della matrice di varianza
rispettivamente del disturbo di misura e del disturbo di processo.
In altri termini se le misure sono affette da grossi errori (R elevata) non è necessario che la
stima dello stato le riproduca con precisione; e viceversa se la dinamica dello stato è molto
incerta (Q elevata) è opportuno che la stima dello stato segua le misure, anche a scapito
di una sua evoluzione temporale assai irregolare.