Il filtro di Kalman e la stima/ricostruzione dello stato e Controllo/Anno... · Processi aleatori e...

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


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


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.


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.


Il contesto

d x(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.


Il contesto

d x(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.


Il problema

Problema (Stima ottima)

Dato il sistema dinamico

d x(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)]}.


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.


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.



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 .



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.


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(τ)δ(τ − σ).



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}



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τ



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τ



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−τ )]T

dτ

+d

dt

∫ t

0

eAo(t−τ ) Q(τ)[

eAo(t−τ )]T

dτ



d

dteAo tP0

[

eAo t]T

= Ao

{

eAotP0

[

eAo t]T

}

+{

eAo tP0

[

eAo t]T

}

ATo

d

dt

∫ t

0


eAo(t−τ )]T

dτ

= LR(t)LT + Ao

∫ t

0


eAo(t−τ )]T

dτ

+

∫ t

0


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



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−τ )]T

dτ

+Ao

∫ t

0

eAo(t−τ ) Q(τ)[

eAo(t−τ )]T

dτ

+{

eAotP0

[

eAo t]T

}

ATo +

∫ t

0


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


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


Il filtro di Kalman: riepilogo

d x(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)


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.


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


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)


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 ′


Il filtro di Kalman e la stima/ricostruzione dello stato e Controllo/Anno... · Processi aleatori e...

Documents

Transcript of Il filtro di Kalman e la stima/ricostruzione dello stato e Controllo/Anno... · Processi aleatori e...