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
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 2 / 25
Sommario
1 Recap Lezione Precedente
2 Sistemi in Spazio di Stato
3 Filtro di Kalman in Matlab
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 3 / 25
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])
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 5 / 25
Sistemi in Spazio di Stato
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)
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 7 / 25
Sistemi in Spazio di Stato
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)
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 8 / 25
Sistemi in Spazio di Stato
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)
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 9 / 25
Sistemi in Spazio di Stato
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
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 10 / 25
Sistemi in Spazio di Stato
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
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 11 / 25
Sistemi in Spazio di Stato
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
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 12 / 25
Sistemi in Spazio di Stato
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 ).
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 13 / 25
Sistemi in Spazio di Stato
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)
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 14 / 25
Sistemi in Spazio di Stato
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.
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 15 / 25
Sistemi in Spazio di Stato
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
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 16 / 25
Filtro di Kalman in Matlab
Filtro di Kalman in Matlab
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)
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 18 / 25
Filtro di Kalman in Matlab
Filtro di Kalman
Schema ricorsivo per stima e predizione:
Stima:
x(t|t) = x(t|t − 1) + L(t) [y(t)− C x(t|t − 1)]
P(t|t) = P(t|t − 1)− P(t|t − 1)C ′Λ−1(t)CP(t|t − 1)
= (I − L(t)C )P(t|t − 1)(I − L(t)C )′ + L(t)RL′(t)
Predizione:
x(t + 1|t) = F x(t|t) + SR−1y(t)
P(t + 1|t) = FP(t|t)F ′ + Q
Condizioni iniziali:
x(t0|t0 − 1) = µ0, P(t0|t0 − 1) = Σ0
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 19 / 25
Filtro di Kalman in Matlab
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 ]
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 20 / 25
Filtro di Kalman in Matlab
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
Filtro di Kalman in Matlab
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)
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 22 / 25
Filtro di Kalman in Matlab
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.
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 23 / 25
Filtro di Kalman in Matlab
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
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 24 / 25
Filtro di Kalman in Matlab
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.
G.A. Susto (DEI - UniPD) SeF 2016 - Lab.3 19 Maggio 2016 25 / 25