Il Filtro di Kalman - Automazione@ingre

CONTROLLO DI SISTEMI ROBOTICI
Laurea Specialistica in Ingegneria Meccatronica
CONTROLLO DI ROBOT INDUSTRIALI
IL FILTRO DI KALMAN
Ing. Cristian Secchi
Tel. 0522 522235
e-mail: [email protected]
http://www.dismi.unimo.it/Members/csecchi
Tecnologia e Incertezza
• 
In pratica, l’uscita di un sistema è una misura, ovvero l’uscita di uno o
più sensori che è affetta da un rumore che può essere dovuto a vari
fattori come disturbi, limiti costruttivi e quantizzazione
dell’informazione
• 
I sistemi di attuazione, specialmente quelli low-cost, possono essere
inaccurati e questa inaccuratezza può essere descritta come un
rumore sull’ingresso
• 
I modelli sono per definizione inaccurati e la loro imperfezione può
esssere modellata come un rumore di processo
Tutti questi rumori danno luogo sia a un’incertezza di misura
che a un’incertezza di processo.
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 2
Pag. 1
Tecnologia e Incertezza
x1
x2
m
k
u
b
y
Incertezza di processo: attriti statici, dissipazione della molla,
quantizzazione controllo
Incertezza di misura: quantizzazione della misura (es.: encoder),
rumore
Cristian Secchi
Raggiungibilità e Controllabilità-- 3
Tecnologia e Incertezza
• 
L’incertezza rende problematico il problema della stima dello stato in
quanto il rumore sull’uscita falsa l’informazione sullo stato e il
rumore sul processo falsa il modello utilizzato per l’osservatore.
• 
Se il modello è abbastanza accurato e sensori e attuatori abbastanza
affidabili, l’incertezza può essere trascurabile e l’osservatore dello
stato può essere utilizzato senza problemi
• 
Se l’entità delle incertezze diventa significativa (es.: per applicazioni
low–cost), è necessario considerare il rumore nel problema della
stima dello stato. Infatti l’errore di misura (es.: y − Cxˆ ) viene
integrato dal modello del sistema e, di conseguenza, i rumori
vengono integrati e l’incertezza sulla stima si accumula
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 4
Pag. 2
Esempio
Raggiungibilità e Controllabilità-- 5
Cristian Secchi
Esempio
Se all’uscita e sul processo è sovrapposto un rumore di media 0.1 e
varianza 0.5
y(t)
La stima
degrada!
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 6
Pag. 3
Esempio: Localizzazione
Modellando
l’incertezza e
sfruttando
informazioni note è
possibile gestire
l’incertezza e
ottenere
informazioni molto
sicure
Cristian Secchi
Raggiungibilità e Controllabilità-- 7
Il filtro di Kalman
• 
• 
• 
• 
Sfrutta un modello dell’incertezza di misura e di processo
Consente di costruire un osservatore che fornisce la stima ottima
dello stato bilanciando l’informazione estratta dal modello e quella
estratta dalla misura a seconda dell’affidabilità delle informazioni
E’ usato in moltissime discipline: controlli, robotica, economia,
computer vision…
R.E. Kalman, “A New Approach to Linear Filtering and Prediction
Problems”, Transactions of the ASME 82, 1960
Rudolph Emil Kalman ha ricevuto la
National Medal of Science da Barack
Obama nel 2010
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 8
Pag. 4
Il filtro di Kalman
• 
Il rumore sulla misura e l’incertezza sul modello può essere
modellato come un processo stocastico, ovvero come una variabile
casuale caratterizzato da alcuni parametri (media, varianza, …) che
possono cambiare nel tempo
• 
Il modo in cui vengono descritte le incertezze agenti sul sistema è
cruciale per sviluppare gli strumenti per l’osservazione dello stato.
• 
Il filtro di Kalman utilizza processi Gaussiani per modellare le
incertezze agenti sul sistema. Questa descrizione è spesso (ma non
sempre!) la migliore per molte applicazioni
Raggiungibilità e Controllabilità-- 9
Cristian Secchi
Cenni di Probabilità
• 
Sia S l’insieme di tutti i risultati di un fenomeno aleatorio
•  S={testa,croce}
•  S={1,2,3,4,5,6}
•  S={1♥,2 ♥, …}
• 
Un evento A è un sottoinsieme dei possibili risultati (A ⊆ S)
• 
La probabilità che si verifichi un evento A è data da:
Pr( A) =
Cristian Secchi
Cristian Secchi
numero dei risultati in A
Numero dei risultati possibili
Raggiungibilità e Controllabilità-- 10
Pag. 5
Cenni di Probabilità
• 
Lanciando un dado non truccato con 6 facce,
qual è la probabilità che esca 1 o 2?
S = {1,2,3,4,5,6}
A = {1,2}
Pr( A) =
2 1
=
6 3
Raggiungibilità e Controllabilità-- 11
Cristian Secchi
Assiomi della probabilità
Siano A e B due eventi:
• 
0 ≤ Pr( A) ≤ 1
• 
Pr( S ) = 1
• 
Pr( A ∨ B) = Pr( A) + Pr( B) − Pr( A ∧ B)
True
A
Pr(not ( A)) = 1 − Pr( A)
Pr(∅) = 0
A∧ B
B
Se A e B sono eventi
disgiunti, allora
Pr( A ∨ B) = Pr( A) + Pr( B)
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 12
Pag. 6
Variabili Aleatorie Discrete
• 
Una variabile aleatoria (random variable) discreta è una variabile che
assume un valore casuale in un insieme di valori ammissibili. Ciascun
valore ammissibile è caratterizzato da una certa probabilità di essere
assunto. Formalmente, se X è una variabile aleatoria:
1) X può assumere un valore finito (o contabile) di valori {x1, x2, …, xn}.
2) P(X=xi), or P(xi), è la probabilità che la variabile aleatoria X assuma il
valore xi
3) Esempio: l’esperimento lancio di un dado può essere rappresentato
come una variabile aleatoria discreta X che può assumere i valori
{x1, x2, …, x6} ={1,2,3,4,5,6} e P(xi)=1/6 per i=1,…,6
Raggiungibilità e Controllabilità-- 13
Cristian Secchi
Variabili Aleatorie Continue
• 
Una variabile aleatoria continua è una variabile che assume un valore
casuale in un insieme continuo di valori ammissibili. Ciascun valore
ammissibile è caratterizzato da una certa probabilità di essere assunto.
Formalmente, se X è una variabile aleatoria continua:
1) X può assumere valori in un intervallo continuo [x1,x2].
2) p(X=x), o p(x) (o f(x)), è la funzione di densità di probabilità che la
variabile aleatoria X assuma il valore x
b
Pr( x ∈ (a, b)) = ∫ p( x)dx
p(x)
a
E’ più probabile
essere vicini al
centro della
campana
x
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 14
Pag. 7
La somma delle probabilità è 1
Variabili Aleatorie Discrete
∑ P( x) = 1
x
Variabili Aleatorie Continue
∫ p( x) dx =1
I valori che può assumere una variabile aleatoria possono essere
interpretati come eventi disgiunti
La probabilità che una variabile assuma un valore oppure un altro è la
somma delle probabilità dei valori presi in considerazione
La probabilità che una variabile aleatoria assuma un qualsiasi valore
possibile è 1
Raggiungibilità e Controllabilità-- 15
Cristian Secchi
Probabilità Congiunta e Condizionata
• 
Dati due eventi A e B, la probabilità congiunta è la probabilità che i
due eventi si verifichino contemporaneamente. Date due variabili
aleatorie X e Y la probabilità congiunta che X assuma il valore x e Y
il valore y si indica come P(X=x eY=y) = P(x,y)
• 
Se X e Y sono indipendenti, allora P(x,y) = P(x) P(y)
• 
• 
I lanci di un dado sono indipendenti. La probabilità di ottenre due volte
lo stesso valore è 1/6*1/6=1/36
P(x | y) indica la probabilità che X=x quando Y=y. In altre parole è
la probabilità di x dato y
P(x | y) = P(x,y) / P(y)
P(x,y) = P(x | y) P(y)
• 
Se X e Y sono indipendenti, allora
P(x | y) = P(x)
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 16
Pag. 8
Esempio
Consideriamo un mazzo di carte. Il fenomeno
aleatorio “estrazione della prossima carta” può essere
modellato come una variabile aleatoria. Due giocatori
estraggono una carta a turno e le estrazioni sono
modellate con due variabili aleatorie X e Y.
• 
La probabilità un giocatore estragga un asso dopo che l’altro ha
estratto un asso è data da:
4 3
12
P( X = 1 | Y = 1) = 52 51 =
4
204
52
Raggiungibilità e Controllabilità-- 17
Cristian Secchi
Valore atteso (Expected Value)
• 
Generalizza il concetto di valore medio in un fenomeno aleatorio.
• 
Se il fenomeno è modellato con una variabile aleatoria discreta X
allora il valore atteso è:
n
E[ X ] = ∑ xi P( xi )
i =1
• 
E[X] rappresenta il valore mediamente assunto da X considerando la
probabilità con la quale possono essere assunti i valori ammissibili
• 
Esempio: X può assumere valori {x1,x2}={1,2} con P(x1)=0.1 P(x2)
=0.9. Il valore atteso per X non è la media dei valori ammissibili
(=1.5) ma E[X]=0.1*1+0.9*2=1.9. E[X] è spostato verso 2 perché
2 ha una probabilità di verificarsi molto maggiore. E[X] è la media
dei valori che si ottengono da X dopo un numero abbastanza grande
di esperimenti
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 18
Pag. 9
Valore Atteso (Expected Value)
• 
Nel caso di variabili aleatorie continue, la definizione di valore atteso
è concettualmente la stessa. Se l’intervallo su cui è definita la
variabile aleatoria X è R e p(x) indica la funzione di densità di
probabilità si ha che
∞
E[ X ] =
∫ xp ( x)dx
−∞
Raggiungibilità e Controllabilità-- 19
Cristian Secchi
Varianza
• 
La varianza σ2 di un segnale attorno al suo valore atteso è data da
2
n
2
σ = E[( X − E ( X )) ] = ∑ ( xi − E[ X ])2 P( xi )
Tempo discreto
i =1
"
2
! = E[(X ! E[X]) ] = # (x ! E[X])2 p(x)dx
• 
2
!"
La varianza rappresenta la dispersione di una variabile aleatoria
attorno al suo valore atteso. Maggiore è la varianza, maggiore è la
dispersione intorno al valore medio.
Cristian Secchi
Cristian Secchi
Tempo continuo
E[X]=1
E[X]=1
σ2 =1.41
σ2 =0.32
Raggiungibilità e Controllabilità-- 20
Pag. 10
Variabili Incorrelate
• 
Due variabili aleatorie X e Y si dicono incorrelate, se
E[ XY ] = E[ X ]E[Y ]
• 
Intuitivamente questo implica che non esiste nessuna correlazione
tra due fenomeni aleatori e che la loro combinazione non introduce
nessun termine aggiuntivo al loro valore atteso
• 
Due variabili indipendenti sono incorrelate ma non è sempre vero il
contrario
Raggiungibilità e Controllabilità-- 21
Cristian Secchi
Variabili Aleatorie Vettoriali
• 
Un vettore aleatorio X =(x1,…,xn)T è un vettore le cui componenti
sono variabili aleatorie
• 
Il valore atteso di un vettore aleatorio X è dato da:
⎡ E[ x1 ] ⎤
E[ X ] = ⎢⎢  ⎥⎥
⎢⎣ E[ xn ]⎥⎦
• 
La matrice di covarianza Σ di un vettore aleatorio X è data da
Σ = E[( X − E[ X ])( X − E[ X ])T ]
E’ la generalizzazione della varianza!
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 22
Pag. 11
Variabili Aleatorie Vettoriali
• 
Nel caso scalare, la definizione di matrice di covarianza coincide con
quella di varianza
• 
I termini sulla diagonale sono le varianze delle componenti del
vettore aleatorio X
• 
I termini fuori dalla diagonale della matrice di covarianza indicano
la correlazione che c’è tra le componenti della matrice di covarianza
• 
Se un vettore aleatorio X è costituito da variabili aleatorie
incorrelate, la matrice di covarianza e diagonale
Raggiungibilità e Controllabilità-- 23
Cristian Secchi
Variabili Aleatorie Gaussiane
• 
Una variabile aleatoria continua si dice variabile gaussiana (o
variabile normale o semplicemente gaussiana) se la sua densità di
probabilità è una curva di Gauss del tipo
−
1
f ( x) =
e
σ 2π
( x−µ )2
2σ 2
f(x)
µ
• 
• 
• 
• 
Si dimostra che:
• 
• 
E[X]=µ
E[(X-µ)2]=σ2
σ
x
Il valore atteso si dice anche valore medio o media della gaussiana
Più la campana è “stretta” minore è la varianza
Una variabile aleatoria Gaussiana è completamente descritta dal suo
valore atteso e dalla sua varianza
Cristian Secchi
Cristian Secchi
-σ
Raggiungibilità e Controllabilità-- 24
Pag. 12
Variabili Aleatorie Gaussiane
• 
Un vettore aleatorio si dice gaussiano (o normale) se la sua densità
di probabilità è del tipo
f ( x) =
• 
1
(2π ) n det(Σ)
e
1
− ( x − µ )T Σ −1 ( x − µ )
2
E’ possibile mostrare che la probabilità che nel 95% dei casi il
vettore si trova entro un elissoide di incertezza centrato in µ e
descritto da xTΣx=1
µ
• 
Si dimostra che:
• 
• 
E[X]=µ
E[(X-µ) (X-µ)T]=Σ
Cristian Secchi
Raggiungibilità e Controllabilità-- 25
Variabili Aleatorie Gaussiane e Robotica
• 
Le variabili Gaussiane sono semplici perché sono descritte solo da
due parametri
• 
Consentono di modellare incertezze molto comuni nella robotica
fissa e, soprattutto, mobile
• 
• 
Dove sono? In questa posizione a meno di una certa incertezza
Indicheremo l’insieme delle variabili aleatorie Gaussiane di media µ
e varianza σ2 come N(µ, σ2) e l’insieme dei vettori aleatori di media
µ e matrice di covarianza Σ come N(µ, Σ)
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 26
Pag. 13
Il filtro di Kalman
• 
Illustreremo il filtro di Kalman per sistemi discreti
•  Il filtro viene usato quando è necessario considerare rumore di
processo e/o di misura e, quindi, l’uscita discreta dei sensori
•  Una sua versione estesa è molto utilizzata nell’ambito della
robotica mobile
• 
La versione continua del filtro di Kalman è analoga
Cristian Secchi
Raggiungibilità e Controllabilità-- 27
Filtro di Kalman Discreto
• 
Consente di stimare lo stato x di un sistema lineare discreto
descritto che evolve dall’istante k-1 all’istante k secondo le seguenti
equazioni
⎧ x(k ) = Ax(k − 1) + Bu (k − 1) + w(k )
⎨
⎩ y (k ) = Cx(k ) + v(k )
• 
w(k) è un vettore aleatorio che rappresenta le incertezze di processo
• 
v(k) è un vettore aleatorio che rappresenta l’incertezza sulla misura
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 28
Pag. 14
Filtro di Kalman
• 
w(k) è un processo gaussian con media nulla e matrice di
covarianza data, pari a Q(k)
• 
v(k) è un processo gaussiano con media nulla e matrice di
covarianza data, pari a R(k)
• 
w(k) e v(k) sono variabili indipendenti à L’incertezza sulla misura
non dipende dall’incertezza sul processo e viceversa.
• 
w(k) e v(k) codificano un puro rumore che contiene tutte le
incertezze indeterministiche del sistema
• 
Le matrici Q(k) e R(k) si possono determinare facendo un modello
statistico del processo o del sensore. Anche con stime iniziali
grossolane, il filtro solitamente restituisce buoni risultati.
Cristian Secchi
Raggiungibilità e Controllabilità-- 29
Filtro di Kalman
• 
Lo stato x(0) è un vettore aleatorio gaussiano di media µ0 e
covarianza Σ0.
• 
Lo stato x(k) e l’uscita y(k) sono vettori aleatori gaussiani perché
combinazione lineare di vettori aleatori gaussiani
• 
L’idea alla base del filtro di Kalman è quella di sfruttare la
conoscenza del modello e la descrizione probabilistica dell’incertezza
per decidere come combinare in maniera ottima l’informazione del
processo e l’informazione dell’incertezza
• 
L’uscita del filtro è lo stato del filtro stesso, ovvero un vettore
aleatorio con una certa media e una certa covarianza che
rappresenta una stima statistica dello stato del sistema.
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 30
Pag. 15
Filtro di Kalman - L’algoritmo
• 
• 
• 
E’ ricorsivo
•  Sono necessari solo lo stato stimato all’istante precedente e la
misura attuale per calcolare la stima dello stato attuale
•  Consente di avere all’istante k la stima dello stato x(k) e
solitamente è tutto ciò che serve per pianificare il movimento di
un robot o per controllare un sistema
Lo stato del filtro è rappresentato da due variabili
•  xˆ (k ): Il valore atteso dello stato stimato
•  Σ(k): la matrice di covarianza di xˆ (k ) (una misura
dell’accuratezza della stima dello stato)
Il filtro deve essere inizializzato
e poi cicla tra due stadi:
•  Predizione
•  Aggiornamento
Cristian Secchi
Raggiungibilità e Controllabilità-- 31
Inizializzazione
• 
E’ necessario inizializzare lo stato del filtro con un vettore aleatorio
gaussiano
• 
Come per gli osservatori di stato deterministici, quanto più vicino è
lo stato iniziale del filtro allo stato reale, tanto più veloce sarà la
convergenza del filtro allo stato del sistema
• 
Se si ha una stima approssimativa µ0 dello stato iniziale del sistema,
è bene inizializzare il filtro con tale stima e con una matrice di
covarianza adeguata
• 
Se non si ha nessuna stima dello stato iniziale, si inizializzi il filtro
con un valore casuale e una matrice di covarianza grande (quanto,
dipende dall’applicazione, in modo da segnalare al filtro che la stima
attuale è assolutamente inaccurata).
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 32
Pag. 16
L’algoritmo (Notazione 1)
• 
Predizione
•  Stato Predetto
•  Covarianza Predetta
• 
xˆ (k | k − 1) = Axˆ (k − 1 | k − 1) + Bu (k − 1)
Σ(k | k − 1) = AΣ(k − 1 | k − 1) AT + Q(k )
Aggiornamento
•  Innovazione
•  Covarianza Innovazione
e(k ) = y (k ) − Cxˆ (k | k − 1)
S (k ) = CΣ(k | k − 1)C T + R(k )
K (k ) = Σ(k | k − 1)C T S (k ) −1
•  Guadagno di Kalman
•  Aggiornamento dello stato
•  Aggiornamento Covarianza
xˆ (k | k ) = xˆ (k | k − 1) + K (k )e(k )
Σ(k | k ) = ( I − K (k )C )Σ(k | k − 1)
Raggiungibilità e Controllabilità-- 33
Cristian Secchi
L’algoritmo (Notazione 2)
• 
Predizione
•  Stato Predetto
•  Covarianza Predetta
• 
Aggiornamento
•  Innovazione
•  Covarianza Innovazione
•  Guadagno di Kalman
•  Aggiornamento dello stato
•  Aggiornamento Covarianza
Cristian Secchi
Cristian Secchi
xˆ (k ) − = Axˆ (k − 1) + Bu (k − 1)
Σ(k ) − = AΣ(k − 1) AT + Q(k )
e(k ) = y(k ) − Cxˆ (k ) −
S (k ) = CΣ(k ) − C T + R(k )
K (k ) = Σ(k ) − C T S (k ) −1
xˆ (k ) = xˆ (k ) − + K (k )e(k )
Σ(k ) = ( I − K (k )C )Σ(k ) −
Raggiungibilità e Controllabilità-- 34
Pag. 17
Il filtro di Kalman
Predizione
(1)  Predizione dello stato
xˆ (k ) − = Axˆ (k − 1) + Bu (k − 1)
(2)  Predizione della covarianza
Σ(k ) − = AΣ(k − 1) AT + Q(k )
Aggiornamento
(1)  Calcolo il guadagno di Kalman
K (k ) = Σ(k ) − C T (CΣ(k ) − C T + R) −1
(2)  Aggiorno la stima con la misura
xˆ (k ) = xˆ (k ) − + K (k )e(k )
(3)  Aggiorno la covariaza
xˆ (k − 1) Σ(k − 1)
Σ(k ) = ( I − K (k )C )Σ(k ) −
Raggiungibilità e Controllabilità-- 35
Cristian Secchi
Esempio
⎧ x(k ) = 3 x(k − 1) + 2 u (k − 1) + w(k )
⎪
B
A
⎨
y
(
k
)
=
4
x
(
k
)
+
v
(
k
)

⎪
C
⎩
Q = 0 .1
R = 0 .8
x(k − 1) = 3 u (k − 1) = 1 Σ(k − 1) = 0.1 y (k ) = 43.78
xˆ (k ) − = Axˆ (k − 1) + Bu (k − 1) = 11
Σ(k ) − = AΣ(k − 1) AT + Q(k ) = 1
e(k ) = y(k ) − Cxˆ (k ) − = −0.22
la covarianza peggiora a causa dell’incertezza di processo
S (k ) = CΣ(k ) − C T + R(k ) = 16.8
L’innovazione è molto incerta
K (k ) = Σ(k ) − C T S (k ) −1 = 0.23
xˆ (k ) = xˆ (k ) − + K (k )e(k ) = 10.94
Il guadagno di Kalman è basso per non dare troppo peso
all’innovazione che introdurrebbe una grande incertezza
Σ(k ) = ( I − K (k )C )Σ(k ) = 0.08
−
Cristian Secchi
Cristian Secchi
previsione
costruisco l’innovazione usando l’uscita
La correzione allo stato predetto è minima
La stima ottenuta ha poca incertezza grazie alla correzione
tramite l’innovazione
Raggiungibilità e Controllabilità-- 36
Pag. 18
Illustrazione del funzionamento
Un robot si muove lungo l’asse orizzontale. Il robot conosce un modello di
se stesso e riceve informazioni sulla sua posizione da un sensore (GPS,
telecamera, …). Sia il modello che il sensore sono imprecisi e, pertanto,
sia lo stato che la misura sono rappresentabili come variabili aleatorie
gaussiane.
Cristian Secchi
Raggiungibilità e Controllabilità-- 37
Illustrazione del funzionamento
La posizione del robot è una variabile
aleatoria gaussiana. Il robot sa dove si
trova (valor medio à picco della
campana) con una certa imprecisione
(varianza à ampiezza della campana)
Il robot legge l’uscita del sensore per
sapere dove si trova e riceve una misura
dal sensore che dice dove si trova con
una certa imprecisione
Il Filtro di Kalman fonde l’informazione
della posizione del robot con quella
derivante dal sensore e ottiene una stima
più precisa di dove si trova.
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 38
Pag. 19
Illustrazione del funzionamento
Se il robot si muove a destra, ottiene dal
modello (predizione) una stima imprecisa
della sua posizione a causa dell’incertezza del
modello
Il robot può interrogare il sensore che
trasmette una misura incerta (un’altra
gaussiana) della posizione del robot
Integrando la stima ottenuta dal modello e la
misura dello stato tramite il filtro di Kalman si
ottiene una stima migliore della posizione del
robot
Cristian Secchi
Raggiungibilità e Controllabilità-- 39
Commenti
• 
Il filtro di Kalman consente di trovare la stima ottima dello stato nel
caso in cui le incertezze siano modellabili come gaussiane
• 
Aggiorna la stima facendo una media pesata sulle incertezze:
•  Se la covarianza di processo è alta il guadagno di Kalman dà più
• 
• 
peso all’innovazione
Se la covarianza di innovazione è alta, il guadagno dà più peso
alla predizione
Nel tempo continuo, il guadagno di Kalman si ricava in maniera
analoga e può essere usato come guadagno in uno stimatore
asintotico dello stato in catena chiusa per ottenere la stima ottima
dello stato in caso di incertezze gaussiane sul processo e la misura
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 40
Pag. 20
Esempio
Si desidera stimare un valore costante (es.: una tensione) quando la sua
misura è corrotta da un rumore con varianza 0.1 V
Cristian Secchi
Raggiungibilità e Controllabilità-- 41
Esempio
E’ possibile modellare il processo come un sistema LTI con un’incertezza
x(k ) = Ax(k − 1) + Bu (k − 1) + w(k )
x(k ) = x(k − 1) + w(k )
Lo stato non cambia e non c’è
nessun ingresso
x( k ) ∈ R A = 1 B = 0
L’uscita è caratterizzata da un’incertezza:
y(k ) = Cx(k ) + w(k )
y (k ) = x(k ) + w(k )
Si misura l’intero stato
C =1
w(k) è una variabile gaussiana di media nulla e varianza 0.1
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 42
Pag. 21
Esempio
Raggiungibilità e Controllabilità-- 43
Cristian Secchi
Esempio
Si consideri un robot olonomo completamente attuato e che si muove su
un piano.
y
ux
uy
x
La posizione sul piano del robot è rilevata con un sensore GPS
caratterizzato da una certa incertezza
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 44
Pag. 22
Esempio
Il robot può essere modellato come un sistema LTI discreto, supponendo
che la sua velocità non cambi significativamente tra due istanti di
campionamento
⎧ x(k ) = x(k − 1) + u (k − 1) + w(k )
⎨
⎩ y (k ) = x(k ) + v(k )
⎛ x ⎞
⎛ u ⎞
x = ⎜⎜ ⎟⎟ u = ⎜ x ⎟
⎜ u ⎟
⎝ y ⎠
⎝ y ⎠
w(k ) ∈ N (0, Q(k )) v(k ) ∈ N (0, R(k ))
0 ⎞
⎛ 0.05
Il modello è noto con
⎟⎟
Q(k ) = Q = ⎜⎜
una buona precisone
0.05 ⎠
⎝ 0
⎛ 2 0 ⎞
La precisione del un GPS è di
⎟⎟
R(k ) = R = ⎜⎜
un paio di metri
⎝ 0 2 ⎠
Cristian Secchi
Raggiungibilità e Controllabilità-- 45
Osservazione
• 
Il robot parte da x=0 e y=1 ed è eccitato con un ingresso ux(k)=sin
(0.01k) uy(k)=cos(0.1k)]
y (k )
y (k ) e traiettoria reale
L’incertezza corrompe la misura e dall’uscita non è possibile risalire
accuratamente al movimento del robot. La precisione è limitata
dall’incertezza della misura.
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 46
Pag. 23
Esempio
• 
• 
Utilizzando il filtro di Kalman è possibile sfruttare la conoscenza del
modello per costruire una stima dello stato più precisa di quella data
dall’uscita
La stima è inizializzata a
xˆ(0) = [0 1]
xˆ ( k )
Cristian Secchi
⎡4 0⎤
Σ(0) = ⎢
⎥
⎣0 4⎦
xˆ ( k ) e traiettoria reale
Raggiungibilità e Controllabilità-- 47
Esempio
Il filtro parte da una stima molto incerta e man mano che il sistema evolve
la stima diventa più accurata grazie alla fusione dei dati del modello con la
misura.
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 48
Pag. 24
Esempio
• 
Il sistema parte dallo stato iniziale [10;10] e si controlla il sistema
con un feedback
0 ⎞
⎛ − 0.5
⎟⎟ x(k )
u (k ) = ⎜⎜
0
−
0
.
5
⎝
⎠
per rendere il sistema controllato asintoticamente stabile e far
convergere a 0 il movimento del sistema
Cristian Secchi
Raggiungibilità e Controllabilità-- 49
Esempio
u (k ) = Ky (k )
u (k ) = Kxˆ (k )
Il rumore sull’uscita peggiora notevolmente le prestazioni del controllore.
Utilizzando il filtro di Kalman è possibile migliorare notevolmente le
prestazioni del sistema controllato.
Cristian Secchi
Cristian Secchi
Raggiungibilità e Controllabilità-- 50
Pag. 25
CONTROLLO DI SISTEMI ROBOTICI
Laurea Specialistica in Ingegneria Meccatronica
CONTROLLO DI ROBOT INDUSTRIALI
IL FILTRO DI KALMAN
Ing. Cristian Secchi
Tel. 0522 522235
e-mail: [email protected]
http://www.dismi.unimo.it/Members/csecchi
Cristian Secchi
Pag. 26