Laboratorio di Stima e Filtraggio: Filtro di Kalman in...

Laboratorio di Stima e Filtraggio:

Filtro di Kalman in Matlab

Gian Antonio Susto

(Original work by Chiara Masiero)

DEI - UniPD

19 Maggio 2016

G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 1 / 25

Informazioni

Slides e funzioni disponibili al link

http://automatica.dei.unipd.it/people/gianantoniosusto/

teaching.html

Per generare una password temporanea utilizzare il link

http://www.dei.unipd.it/utentelab


http://automatica.dei.unipd.it/people/gianantoniosusto/teaching.html

http://automatica.dei.unipd.it/people/gianantoniosusto/teaching.html

http://www.dei.unipd.it/utentelab

Sommario

1 Recap Lezione Precedente

2 Sistemi in Spazio di Stato

3 Filtro di Kalman in Matlab


Recap Lezione Precedente

Recap Lezione Precedente

Lez. 2 - Esercizio 2Funzione parte_causale(num,den)

Costruire la funzione parte_causale(num,den)

[nc , dc , nac , dac ] = parte_causale(num, den)

la quale calcola numeratore nc e denominatore dC della parte causale e

della parte strettamente anti-causale (nac , dac) della funzione razionale

F (z) =num(z)

den(z)

Con (num = [2 -1 -5], den = [2 -7 3]) (un polo in 1/2 ed uno in3) otteniamo

(nc , dc) = ([1/3 5/6], [1 -1/2])

(nac , dac) = ([2/3 0], [1 -3])


Sistemi in Spazio di Stato


De�nizione di sistemi in Matlab

Sistema in spazio di stato{x(t + 1) = Ax(t) + Bu(t)

y(t) = Cx(t) + Du(t)

In Matlab:

Caso continuo: � sys_c = ss(A,B,C,D)

Caso discreto: � sys_d = ss(A,B,C,D,Ts)

Ts è il tempo di campionamento: per non speci�carlo, porlo

uguale a -1

Passare dal sistema alle matrici: � [A,B,C,D] = ssdata(sys)



Alcune funzioni utili

Per passare dalla rappresentazione in spazio di stato ad altre:

Da spazio di stato a TF: � sys_tf = tf(sys_ss)

Ispezionare il sistema con � get(sys_tf)

Da spazio di stato a ZPK: � sys_zpk = zpk(sys_ss)

Ispezionare il sistema con � get(sys_zpk)

Realizzazione minima di un sistema (raggiungibile e osservabile):

� sys_min = minreal(sys)



Analisi dei sistemi - 1

Stabilità:

Autovalori del sistema: � lambda = eig(A) o � lambda =

eig(sys)

Raggiungibilità e osservabilità:

Matrice di raggiungibilità: � R = ctrb(A,B) o � R = ctrb(sys)

Matrice di osservabilità: � O = obsv(A,C) o � R = obsv(sys)

Rango di una matrice: � r_M = rank(M)



Controllabilitá e osservabilitá

PBH Test - La coppia (A,B) é controllabile se e solo se

rank([

A− λI B])

= n

per ogni λ autovalore di A

PBH Test - La coppia (A,C ) é osservabile se e solo se

rank

([A− λI

C

])= n

per ogni λ autovalore di A



Stabilizzabilitá e RivelabilitáCaso continuo

La coppia (A,B) é stabilizzabile se e solo se

rank([

A− λI B])

= n

per ogni λ autovalore di A a parte reale positiva o nulla

La coppia (A,C ) é rivelabile se e solo se

rank

([A− λI

C

])= n

per ogni λ autovalore di A a parte reale positiva o nulla



Stabilizzabilitá e RivelabilitáCaso discreto

La coppia (A,B) é stabilizzabile se e solo se

rank([

A− λI B])

= n

per ogni λ autovalore di A tale che |λ| > 1

La coppia (A,C ) é rivelabile se e solo se

rank

([A− λI

C

])= n

per ogni λ autovalore di A tale che |λ| > 1



Esercizio 1

Scrivere una funzione Matlab [bool_Contr, bool_Stab] =

check_ContrStab(A, B, Ts) - con A ∈ Rn×n, B ∈ Rn×m e Ts unastringa che puó assumere valori 'discreto', 'continuo' - che:

(1) veri�chi la controllabilità della coppia (A,B);

(2) veri�chi la stabilizzabilità della coppia (A,B).

Scrivere una funzione Matlab [bool_Oss, bool_Riv] =

check_OssRiv(A, C, Ts) - con A ∈ Rn×n, C ∈ Rp×n e Ts unastringa che puó assumere valori 'discreto', 'continuo' - che:

(1) veri�chi l'osservabilità della coppia (A,C );

(2) veri�chi la rivelabilità della coppia (A,C ).



Analisi dei sistemi - 2

Altri strumenti o�erti dal Control System Toolbox:

Risposta impulsiva: � [y, t] = impulse(sys)

Risposta al gradino: � [y, t] = step(sys)

Analisi in frequenza: � bode(sys)

Zeri e poli del sistema: � pzmap(sys)

Andamento dell'uscita con ingresso u, vettore dei tempi t e stato

iniziale x0: � y = lsim(sys,u,t,x0)

Soluzione P dell'equazione di Lyapunov discreta

P = APA′ + Q

� P = dlyap(A,Q)



Analisi dei sistemi - Esempio

Si consideri il sistema (oscillatore armonico smorzato) governato

dall'equazione

mx(t) = −k x(t)− µx(t) + u(t), (1)

E' possibile esprimere in forma di stato il sistema precedente, osservando

come uscita la posizione x(t) nella seguente forma

[x(t)

x(t)

]=

[0 1

− km − µ

m

]︸︷︷︸

A

[x(t)

x(t)

]+

[01

m

]︸︷︷︸

B

u(t)

y(t) =[1 0

]︸︷︷︸

C

[x(t)

x(t)

]

Consideriamo il caso m = 0.1 e k = 1.6, con µ variabile.



Analisi dei sistemi - Esempio

Con µ = 0.8 otteniamo A =

[0 1

−16 −8

], B =

[0 10

]T.

Calcolo autovalori: A[z1 z2

]T= λ

[z1 z2

]T, che risolvendo in

λ porta a ottenere λ2 + 8λ+ 16 = (λ+ 4)2 = 0.

Controllabilitá:

rank(C) = rank([

B AB])

= rank

([0 10

10 −80

])= 2

Osservabilitá:

rank(O) = rank

([C CA

]T)= rank

([1 0

0 1

])= 2



Filtro di Kalman: richiami

Ipotesi:

E

[v(t)

w(t)

] [v′(s) w′(s)

]=

[Q S

S ′ R

]δ(t − s)

v(t) = E [v(t)|w(t)] + v(t) (2)

= SR−1 [y(t)− Cx(t)] + v(t) (3)

v(t) ⊥ w(t), Var v(t) = Q := Q − SR−1S ′

De�niamo

F := A− SR−1C

P(t|t − 1) := Var x(t|t − 1), Λ(t) := CP(t|t − 1)C ′ + R,

P(t|t) := Var x(t|t), L(t) := P(t|t − 1)C ′Λ−1(t)



Predittore di Kalman

Stima e predizione possono essere disaccoppiate. Ad esempio, nel caso

della predizione si ottengono le equazioni ricorsive seguenti:

x(t + 1|t) = Ax(t|t − 1) + G (t) [y(t)− C x(t|t − 1)]

P(t + 1|t) = F[P(t|t − 1)− P(t|t − 1)C ′Λ−1(t)CP(t|t − 1)

]F ′ + Q

= Γ(t)P(t|t − 1)Γ′(t) + K (t)RK ′(t) + Q

con

K (t) := FL(t)

G (t) := K (t) + SR−1

Γ(t) := A− G (t)C = F − K (t)C = F [I − L(t)C ]



Filtro di Kalman a regime

La varianza dell'errore di predizione a regime risolve l'equazione di

Riccati

P = F[P − PC ′

(CPC ′ + R

)−1CP]F ′ + Q

Si ha una soluzione unica e stabilizzante se e solo se le due condizioniseguenti sono veri�cate:

1 (F ,C ) deve essere rivelabile2 (F , Q

1

2 ) deve essere stabilizzabile

con F := A− SR−1C e Q := Q − SR−1S ' .

Guadagno del �ltro di Kalman a regime

K = F PC ′[CPC ′ + R

]−1G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 21 / 25


Equazione di Riccati in Matlab

In Matlab usiamo il comando dare:

DARE Solve discrete-time algebraic Riccati equations.

[X,L,G] = DARE(A,B,Q,R,S,E) computes the unique

stabilizing solution X of the discrete-time algebraic

Riccati equation

E'XE = A'XA - (A'XB + S)∧(-1)(B'XB + R) (A'XB + S)' + Q

Il comando dare in Matlab risolve l'equazione duale (quella per il

controllo ottimo).

Nel nostro caso:

� [P,L,G] = dare(F',C',Q-SR∧ (-1)S',R)



Kalman a regime - Esempio

Si consideri il sistema scalare descritto da{x(t + 1) = 0.5x(t) + v(t)

y(t) = 2x(t) + w(t)

con v(·) e w(·) rumori bianchi a media nulla e varianza Q = 19/20,

R = 1, fra loro indipendenti.

Il predittore di regime del sistema esiste in quanto il suo unico

autovalore é λ = 0.5, quindi il sistema é sia rivelabile che stabilizzabile.



Kalman a regime - Esempio

La varianza a regime dell'errore di stima si trova risolvendo l'equazione

di Riccati (caso scalare, S = 0):

P = APF ′ + Q −(APC ′

)2/(CPC ′ + R

)= P

4+ 19

20− P2

4P+1,

che diventa 80P2 − 61P − 19 = 0, che ha due soluzioni (P = 1 e

P = −19

80); solo P = 1 é accettabile (é non negativa e stiamo

calcolando una varianza).

Il guadagno a regime del predittore é (caso scalare, S = 0)

K = APC ′/(CPC ′ + R

)= 1/5,

ne consegue che



Esercizio 2

Scrivere una funzione Matlab [boolStab, boolRiv, K_bar, P_bar] =

kalman_regime(sys) che, a partire da un sistema sys = ss(A, B, C,

D) in spazio di stato,:

veri�chi se un sistema é a tempo continuo o tempo discreto

(suggerimento: con il comando sys.Ts é possibile recuperarne il

tempo di campionamento);

veri�chi se il sistema é stabilizzabile e rivelabile e popoli in maniera

appropriata [boolStab, boolRiv] con (true o false);

se il sistema é sia stabilizzabile che rivelabile, calcoli il guadagno di

Kalman (in K_bar) e la varianza dell'errore di predizione a regime (in

K_bar); se il sistema non é stabilizzabile e rivelabile, le variabili

d'uscita K_bar e P_bar devono essere lasciate vuote.


Laboratorio di Stima e Filtraggio: Filtro di Kalman in...

Documents

Transcript of Laboratorio di Stima e Filtraggio: Filtro di Kalman in...