Il filtro di Kalman e la stima/ricostruzione dello stato P. Valigi Ottimizzazione e Controllo 27 Aprile 2014 OeC - UniPG - 27.04.2014 Kalman
Welcome message from author
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
Il filtro di Kalman e la stima/ricostruzione dellostato
P. Valigi
Ottimizzazione e Controllo
27 Aprile 2014
OeC - UniPG - 27.04.2014 Kalman
Il problema della stima deterministica
Problema (Stima dello stato)
Dato il sistema dinamico deterministico
x(t) = Ax(t) + Bu(t)
y(t) = C(t)x(t),
determinare uno stimatore asintotico dello stato:
x(t) = E {x(t)|(y(τ ), u(τ )), τ ∈ [0,∞]} ,
tale da garantire convergenza asintotica dell’errore di stima:
x(t) := x(t)− x(t),
indipendentemente dalla condizione iniziale x(0) = x(0)− x(0):
limt→∞
x(t) = limt→∞
(x(t) − x(t)) = 0, ∀x(0).
OeC - UniPG - 27.04.2014 Kalman
Osservatore asintotico dello stato
Soluzione del problema di stima: osservatore asintotico dello stato.La struttura
˙x(t) = Ax(t) + Bu(t) + L(Cx − y(t)),
x(t0) = 0
x(t) = Ax(t) + Bu(t), x(0) = x0
y = Cx
La dinamica dell’errore di stima x := x − x :
˙x = x − ˙x = Ax(t) + Bu(t) − Ax(t)− Bu(t) − L(Cx − y(t))
= (A+ LC )x
L: matrice di iniezione: progettata in modo da ottenere una dinamicadell’errore di stima asintoticamente stabile.Duale del progetto di K tale che il sistema a ciclo chiuso siaasintoticamente stabile:
x(t) = (A+ BK )x
OeC - UniPG - 27.04.2014 Kalman
Processi aleatori e stimatori
Problema
Costruire uno stimatore per un processo stocastico x(t),sulla base della misura, su un intervallo finito [t0, t1],di un processo stocastico z(τ),funzionalmente legato al processo x(t) di interesse.
Problema
Costruire uno stimatore x(t), condizionato alla misura z(τ), τ ∈ [t0, t1]
x(t) = E{x(t)|z(τ), τ ∈ [t0, t1]}.
Filtraggio: l’istante di interesse t coincide con l’istante finale t1dell’intervallo di misura;
Predizione: l’istante di interesse t e maggiore dell’istante finale t1dell’intervallo di misura;
Smoothing/Regolarizzazione: l’istante di interesse t e minoredell’istante finale t1 dell’intervallo di misura.
OeC - UniPG - 27.04.2014 Kalman
Il contesto
Sistema dinamico a tempo continuo:
d x(t)
dt= A(t)x(t) + B(t)u(t) + v(t)
y(t) = C (t)x(t) + w(t),
x ∈ Rn indica lo stato, u ∈ R
m indica il segnale di ingresso ed y ∈ Rp
indica il segnale di uscita.Le matrici A(t),B(t) e C (t) sono matrici di dimensioni opportune, ingenere dipendenti dal tempo: sistema non stazionario.Il sistema di interesse e un sistema stocastico: la sua evoluzione einfluenzata (anche) da termini di rumore, cioe da segnali descritti tramiteprocessi aleatori.
OeC - UniPG - 27.04.2014 Kalman
Il contesto
d x(t)
dt= A(t)x(t) + B(t)u(t) + v(t)
y(t) = C(t)x(t) + w(t),
L’evoluzione dello stato e influenzata dal processo aleatorio additivo v(t),che si assume bianco, gaussiano, a media nulla.Il valore (la misura) dell’uscita e influenzata da un segnale descrittotramite un processo aleatorio additivo w(t), che si assume bianco,gaussiano, a media nulla.
E [v(t)] = 0, E [v(t)v(τ)′] = Q(t)δ(t − τ)
E [w(t)] = 0, E [w(t)w(τ)′] = R(t)δ(t − τ).
Si assume che le matrici Q(t) ed R(t) siano continue, simmetriche, conQ(t) semidefinita positiva ed R(t) definita positiva.Se R(t) fosse semidefinita positiva, alcune componenti dell’uscita y(t)non sarebbero affette da rumore: il problema sarebbe formalmente piucomplesso, senza sostanziali differenze concettuali.
OeC - UniPG - 27.04.2014 Kalman
Il contesto
d x(t)
dt= A(t)x(t) + B(t)u(t) + v(t)
y(t) = C (t)x(t) + w(t),
I processi v(t) e w(t) vengono di norma assunti indipendenti:
E [v(t)w(τ)′] = 0, ∀t, ∀τ.
Lo stato iniziale x(t0) viene assunto come variabile aleatoria gaussiana,con valor medio x0, covarianza P0, incorrelata con i processi aleatori v(t)e w(t):
E [x(t0)] = x0, E [(x(t0)− x0)(x(t0)− x0)′] = P0,
E [x(t0)v(t)′] = 0, ∀t, E [x(t0)w(t)′] = 0, ∀t.
OeC - UniPG - 27.04.2014 Kalman
Il problema
Problema (Stima ottima)
Dato il sistema dinamico
d x(t)
dt= A(t)x(t) + B(t)u(t) + v(t)
y(t) = C(t)x(t) + w(t),
con i processi aleatori v(t) e w(t) e la variabile aleatoria x(t0) tali che:
E [v(t)] = 0, E [v(t)v(τ )′] = Q(t)δ(t − τ )
E [w(t)] = 0, E [w(t)w(τ )′] = R(t)δ(t − τ ).
E [x(t0)] = x0, E [(x(t0)− x0)(x(t0)− x0)′] = P0,
E [x(t0)v(τ )′] = 0, ∀t, E [x(t0)w(τ )′] = 0,∀t.
determinare uno stimatore condizionato dello stato:
x(t) = E{x(t)|(y(τ )′, u(τ )′)′, τ ∈ [t0, t]},
tale da minimizzare la varianza dell’errore di stima:
minE{[x(t) − x(t)]′[x(t)− x(t)]}.
OeC - UniPG - 27.04.2014 Kalman
Il filtro di Kalman
La soluzione del problema di stima e data dal Filtro di Kalman (TC)(Kalman-Bucy).
Osservatore asintotico dello stato, caratterizzato da una matrice diiniezione dell’uscita tempo variante,determinata sulla base della soluzione di un’equazione differenziale, checaratterizza la covarianza dell’errore di stima.La struttura
d x(t)
dt= A(t)x(t) + B(t)u(t) + L(t)(C (t)x − y(t)), x(t0) = x0
L(t) = −P(t)C ′(t)R−1(t)
P(t) = A′(t)P(t) + P(t)A(t) + Q(t)− P(t)C ′(t)R−1(t)C (t)P(t),
P(t0) = P0.
P(t) = A′(t)P(t) + P(t)A(t) + Q(t) − P(t)C ′(t)R−1(t)C (t)P(t),Equazione differenziale di Riccati, analogamente/dualmente al caso delcontrollo ottimo su orizzonte finito.
OeC - UniPG - 27.04.2014 Kalman
Il filtro di Kalman: proprieta
d x(t)
dt= A(t)x(t) + B(t)u(t) + L(t)(C(t)x − y(t)), x(t0) = x0
L(t) = −P(t)C ′(t)R−1(t)
P(t) = A′(t)P(t) + P(t)A(t) + Q(t)− P(t)C ′(t)R−1(t)C(t)P(t),
Proprieta:
stimatore lineare ottimo (non esiste un altro dispositivo, statico odinamico, lineare o meno) in grado di fornire una stima migliore (nelsenso della varianza dell’errore);
stima non polarizzata dello stato del sistema, e cioe:
E [x(t)] = E [x(t)];
Il termine y(t) − C (t)x presente nel filtro di Kalman si chiamainnovazione, ed e un processo aleatorio con le stesse statistiche diw(t), in particolare quindi con media nulla, e indipendente dalrumore v(t) sullo stato;
Il termine di innovazione, in quanto processo aleatorio, puo esserenon nullo anche per valori della stima coincidenti con lo stato.
OeC - UniPG - 27.04.2014 Kalman
Il filtro di Kalman: proprieta
Non polarizzazione.
d
dt(x − x) = Ao(x − x) + v + Lw , Ao := A+ LC ,
Valore atteso e(t) dell’errore di stima e(t) := x(t)− x(t):
d e
dt=
d
dtE (x − x) = E {Aoe + v + Lw} = Ao e,
la cui soluzione e data dalla funzione identicamente nulla, a patto dipartire dalla condizione iniziale E (x(0)− x(0)) = 0, cosa sempre possibilein conseguenza della conoscenza della statistica della variabile aleatoriacondizione iniziale: E (x(t0)) = x0.L’errore quadratico medio di stima e pari a:
E{[x − x ]′[x − x ]} = E{e ′ e} = traccia[P]
La covarianza dell’errore di stima e pari a:
E{[x − x ][x − x ]′} = E{e e ′} = P .
OeC - UniPG - 27.04.2014 Kalman
Il filtro di Kalman: proprieta
Equazione differenziale di Riccati
P = ATP + PA+ Q − PCTR−1CP ,[
L = −PC ′R−1]
puo essere riscritta nella forma seguente, che evidenzia dipendenza dallamatrice di iniezione/guadagno L(t):
P = [A+ LC ]P + P[A+ LC ]T + Q + LRLT , P(t0) = P0,
e consente di determinare la varianza e la covarianza dell’errore di stimaanche per matrici L(t) non ottime.La matrice L(t) e un compromesso (il compromesso ottimo), tra lapossibilita di utilizzare misure dell’uscita per correggere la stima dellostato, e la necessita di non corrompere la stima corrente dello stato acausa del rumore aggiunto alle misure.Proporzionalita tra la matrice dei guadagni e la matrice della covarianzadell’errore: maggiore e l’errore di stima, maggiore e il peso attribuito allemisure.La proporzionalita con il termine CTR−1 indica una proporzonalita tral’affidabilita delle misure ed il guadagno dell’osservatore/filtro.
OeC - UniPG - 27.04.2014 Kalman
Evoluzione della covarianza
Come evolve P(t) = E{
e(t) e(t)T}
? P(t) = E{
[e − e][e − e]T}
Poiche e = Aoe + Lw + v , ne segue
e(t) = eAo te0 +
∫ t
0
eAo(t−τ )Lw(τ) dτ +
∫ t
0
eAo(t−τ )v(τ) dτ
La covarianza:
P(t) = E{
e(t) e(t)T}
=
= E
{[
eAo te0 +
∫ t
0
eAo(t−τ )Lw(τ) dτ +
∫ t
0
eAo(t−τ )v(τ) dτ
]
·
[
eAote0 +
∫ t
0
eAo(t−σ)Lw(σ) dσ +
∫ t
0
eAo(t−σ)v(σ) dσ
]T}
Note:
Modello stazionario (per semplicita)
E{·} e lineare e si puo scambiare con le operazioni di integrazione;
e0, w(·) e v(·) sono tra loro incorrelati;
E{v(τ)v(σ)T } = Q(τ)δ(τ − σ), E{w(τ)w(σ)T } = R(τ)δ(τ − σ).
OeC - UniPG - 27.04.2014 Kalman
Evoluzione della covarianza
P(t)=E{
e(t) e(t)T}
=E
{[
eAo t e0 +
∫
t
0eAo (t−τ)
Lw(τ) dτ +
∫
t
0eAo (t−τ)
v(τ) dτ
]
·
[
eAo t e0 +
∫
t
0eAo (t−σ)
Lw(σ) dσ +
∫
t
0eAo (t−σ)
v(σ) dσ
]T
= E
{
eAo te0e
T0
[
eAo t
]T}
+ E
{
eAo te0
[∫ t
0
eAo (t−σ)
Lw(σ) dσ
]T}
+E
{
eAo te0
[∫ t
0
· · · v(σ) dσ
]T}
+E
{
∫ t
0
eAo (t−τ)
Lw(τ ) dτ
[∫ t
0
eAo (t−σ)
Lw(σ) dσ
]T}
+E
{∫ t
0
· · ·w(τ )dτ · · · eT0
}
+ E
{
∫ t
0
· · ·w(τ )dτ
[∫ t
0
· · · v(σ)dσ
]T}
+E
{
∫ t
0
eAo (t−τ)
v(τ ) dτ
[∫ t
0
eAo (t−σ)
v(σ) dσ
]T}
+E
{∫ t
0
· · · v(τ )dτ · · · eT0
}
+ E
{
∫ t
0
· · · v(τ )dτ
[∫ t
0
· · ·w(σ)dσ
]T}
OeC - UniPG - 27.04.2014 Kalman
Evoluzione della covarianza
E
{
eAo te0e
T0
[
eAo t
]T}
= eAo tE
{
e0eT0
}[
eAo t
]T
= eAo tP0
[
eAo t
]T
E
{
∫ t
0
eAo (t−τ)
Lw(τ ) dτ
[∫ t
0
eAo (t−σ)
Lw(σ) dσ
]T}
E
{∫ t
0
∫ t
0
eAo (t−τ)
Lw(τ )w(σ)TLT[
eAo (t−σ)
]T
dσ dτ
}
=
∫ t
0
∫ t
0
eAo (t−τ)
L E{
w(τ )w(σ)T}
LT[
eAo (t−σ)
]T
dσ dτ
=
∫ t
0
∫ t
0
eAo (t−τ)
LR(τ )δ(τ − σ)LT[
eAo (t−σ)
]T
dσ dτ
=
∫ t
0
eAo (t−τ)
LR(τ )LT[
eAo (t−τ)
]T
dτ
OeC - UniPG - 27.04.2014 Kalman
Evoluzione della covarianza
E
{
∫ t
0
eAo (t−τ)
v(τ ) dτ
[∫ t
0
eAo (t−σ)
v(σ) dσ
]T}
=
∫ t
0
∫ t
0
eAo (t−τ)
E{
v(τ )v(σ)T}[
eAo (t−σ)
]T
dσ dτ
=
∫ t
0
∫ t
0
eAo (t−τ)
Q(τ )δ(τ − σ)[
eAo (t−σ)
]T
dσ dτ
=
∫ t
0
eAo (t−τ)
Q(τ )[
eAo (t−τ)
]T
dτ
E quindi:
P(t) = eAotP0
[
eAo t]T
+
∫ t
0
eAo(t−τ )LR(τ)LT[
eAo(t−τ )]T
dτ
+
∫ t
0
eAo(t−τ ) Q(τ)[
eAo(t−τ )]T
dτ
OeC - UniPG - 27.04.2014 Kalman
Evoluzione della covarianza
P(t) = eAo tP0
[
eAo t
]T+
∫
t
0eAo (t−τ)
LR(τ)LT
[
eAo (t−τ)
]Tdτ +
∫
t
0eAo (t−τ)
Q(τ)
[
eAo (t−τ)
]Tdτ
Ora si puo derivare la matrice di covarianza:
d
dtP(t) =
d
dteAo tP0
[
eAo t]T
+d
dt
∫ t
0
eAo(t−τ )LR(τ)LT[
eAo(t−τ )]T
dτ
+d
dt
∫ t
0
eAo(t−τ ) Q(τ)[
eAo(t−τ )]T
dτ
OeC - UniPG - 27.04.2014 Kalman
Evoluzione della covarianza
d
dteAo tP0
[
eAo t]T
= Ao
{
eAotP0
[
eAo t]T
}
+{
eAo tP0
[
eAo t]T
}
ATo
d
dt
∫ t
0
eAo(t−τ )LR(τ)LT[
eAo(t−τ )]T
dτ
= LR(t)LT + Ao
∫ t
0
eAo(t−τ )LR(τ)LT[
eAo(t−τ )]T
dτ
+
∫ t
0
eAo(t−τ )LR(τ)LT[
eAo(t−τ )]T
dτATo
d
dt
∫ t
0
eAo(t−τ ) Q(τ)[
eAo(t−τ )]T
dτ
= Q(t) + Ao
∫ t
0
eAo(t−τ ) Q(τ)[
eAo(t−τ )]T
dτ
+
∫ t
0
eAo(t−τ ) Q(τ)[
eAo(t−τ )]T
dτATo
OeC - UniPG - 27.04.2014 Kalman
Evoluzione della covarianza
P(t) = eAo tP0
[
eAo t
]T+
∫
t
0eAo (t−τ)
LR(τ)LT
[
eAo (t−τ)
]
Tdτ +
∫
t
0eAo (t−τ)
Q(τ)
[
eAo (t−τ)
]
Tdτ
Complessivamente
P = Ao
{
eAo tP0
[
eAo t]T
}
+ Ao
∫ t
0
eAo(t−τ )LR(τ)LT[
eAo(t−τ )]T
dτ
+Ao
∫ t
0
eAo(t−τ ) Q(τ)[
eAo(t−τ )]T
dτ
+{
eAotP0
[
eAo t]T
}
ATo +
∫ t
0
eAo(t−τ )LR(τ)LT[
eAo(t−τ )]T
dτATo
+
∫ t
0
eAo(t−τ ) Q(τ)[
eAo(t−τ )]T
dτATo
+LR(t)LT + Q(t)
= AoP(t) + P(t)ATo + LR(t)LT + Q(t), P(0) = P0
OeC - UniPG - 27.04.2014 Kalman
Matrice innovazione ottima
P = AoP(t) + P(t)ATo + LR(t)LT + Q(t), P(0) = P0, Ao = A+ LC
= AP + PAT + LCP + PCTLT + LRLT + Q completando il quadrato
= AP + PAT + LCP + PCTLT + LRLT + Q ± PCTR−1CP
= AP + PAT + Q − PCTR−1CP + (PCT + LR)R−1(PCT + LR)T
= AP + PAT + Q − PCTR−1CP , se L = −PCTR−1
OeC - UniPG - 27.04.2014 Kalman
Il filtro di Kalman: riepilogo
d x(t)
dt= A(t)x(t) + B(t)u(t) + v(t)
y(t) = C (t)x(t) + w(t),
d x(t)
dt= A(t)x(t) + B(t)u(t) + L(t)(C (t)x − y(t)), x(t0) = x0
L(t) = −P(t)C ′(t)R−1(t)
P(t) = A′(t)P(t) + P(t)A(t) + Q(t)− P(t)C ′(t)R−1(t)C (t)P(t),
Proprieta:
stimatore lineare ottimo
stima non polarizzata
filtro su orizzonte finito/non stazionario (anche per sistemistazionari)
OeC - UniPG - 27.04.2014 Kalman
Esempio: stima costante
R il parametro da misurare/stimare.Modello differenziale:
d x(t)
dt= 0, x(t0) = R
y(t) = x(t) + w(t),
w(t): rumore misura, covarianza E [w(t)w(τ )] = σ2δ(t − τ ) (si assume nota:modello rumore di misura), condizione iniziale x(t0) variabile gaussiana, medi R(e.g., il valore nominale) e varianza p0, (si assume nota: modello dell’incertezzaparametrica). E [w(t)x(t0)] = 0.Stimatore ottimo:
˙x(t) = L(t)(x(t)− y(t)),
p(t) = −1
σ2p2(t), p(0) = p0, p(t) =
p0
1 + (p0/σ2)t
L(t) =p0/σ
2
1 + (p0/σ2)t
limt→∞ p(t) = 0,⇒ la stima asintotica e corretta.Inoltre, limt→∞ L(t) = 0 ⇒ al crescere del tempo le misure hannoun’importanza sempre minore.
OeC - UniPG - 27.04.2014 Kalman
Filtro di Kalman a tempo discreto
Si consideri quindi il sistema a tempo discreto:
x(t + 1) = A(t)x(t) + B(t)u(t) + v(t)
y(t) = C (t)x(t) + w(t),
I processi aleatori additivi v(t) e w(t) si assumono bianchi, gaussiani, amedia nulla. Formalmente:
E [v(t)] = 0, E [v(t)v(τ)′] = Q(t)δ(t − τ)
E [w(t)] = 0, E [w(t)w(τ)′] = R(t)δ(t − τ).
Si assume che le matrici Q(t) ed R(t) siano simmetriche, con Q(t)semidefinita positiva ed R(t) definita positiva.I processi v(t) e w(t) vengono di norma assunti indipendenti (gaussiani):
E [v(t)w(τ)′] = 0, ∀t, ∀τ.
Il valore iniziale x(t0) viene assunto come variabile aleatoria gaussiana,valor medio x0, covarianza P0, incorrelata con i processi aleatori v(t) ew(t):
E [x(t0)] = x0, E [(x(t0)− x0)(x(t0)− x0)T ] = P0, E [x(t0)v(t)
T ] = 0, ∀t, E
OeC - UniPG - 27.04.2014 Kalman
Filtro di Kalman a tempo discreto
Nel caso dei sistemi a tempo discreto e frequente la formulazione delfiltro ottimo in termini di predittore ad un passo, e cioe in termini distima ottima x(t|t − 1) al passo t, condizionata alle misure di y ed u finoal passo t − 1.Stima a priori o predizione
x(t|t − 1) = A(t)x(t − 1|t − 1) + B(t)u(t − 1)
P(t|t − 1) = A(t)P(t − 1|t − 1)A′T (t) + Q(t), P(t0|t0 − 1) = P0
Il ruolo delle misure nel calcolo del valore filtrato (stima a posteriori ocorrezione):
x(t|t) = x(t|t − 1) + Le(t)(C (t)x(t|t − 1)− y(t))
W (t) = [C (t)P(t|t − 1)CT (t) + R ]−1
Le(t) = −P(t|t − 1)CT (t)W (t),
P(t|t) = (I − Le(t)C (t))P(t|t − 1)
OeC - UniPG - 27.04.2014 Kalman
Relazioni di dualita
Eq. Riccati LQR Eq. Riccati KF
EDO all’indietro nel tempo EDO in avanti(segno meno,condizione al contorno per t = T ) (condizione al contorno per t = 0)
−Pc = PcA+A′Pc−PcBR−1B ′Pc+Q Po = A′Po + PoA+Q − PoC
′R−1CPo
Pc(T ) = S Po(0) = P0 (covarianza x0)
matrice A matrice A′
matrice B matrice C ′
OeC - UniPG - 27.04.2014 Kalman
Related Documents
