Upload
others
View
4
Download
0
Embed Size (px)
Citation preview
Introduzione ai filtri digitali
Corrado Santoro
ARSLAB - Autonomous and Robotic Systems LaboratoryDipartimento di Matematica e Informatica - Universita di Catania, Italy
Programmazione Sistemi Robotici
Corrado Santoro Introduzione ai filtri digitali
Sistemi, misura e predizione
La misura delle variabili di stato osservabili e un aspettofondamentale del funzionamento di un sistema di controllo
Se la parte di misura non e sufficientemente precisa ocontiene un’elevata quantita di rumore, l’intero sistema dicontrollo non potra operare adeguatamente
Le tecniche utilizzate in questi casi sono:
Filtraggio dei campioni forniti dal sistema di misura
Predizione del comportamento del sistema e adattamentodella misura
Corrado Santoro Introduzione ai filtri digitali
Filtri
Filtri Digitali
Corrado Santoro Introduzione ai filtri digitali
Filtri
FiltriUn filtro e un sistema dinamico in grado di ridurre (tagliare) e/oamplificare alcune frequenze specifiche del segnale misurato
Un filtro si rappresenta con uno spettro di frequenze che indicaappunto quali frequenze saranno tagliate e quali amplificate
L’output del filtro e il prodotto tra lo spettro del segnale in ingresso e lospettro del filtro
Spettro del segnale in ingressoFiltro (e spettro del filtro)
Spettro del segnale in uscita
ff
f
f
Corrado Santoro Introduzione ai filtri digitali
Filtri
Tipologia di FiltriPassa-Basso: passano le frequenze minori della frequenza di taglio
Passa-Alto: passano le frequenze maggiori della frequenza di taglio
Passa-Banda: passano le frequenze attorno alla frequenza di taglio
Arresta-Banda: passano tutte le frequenze tranne quelle attorno allafrequenza di taglio
Corrado Santoro Introduzione ai filtri digitali
Filtri
Filtri DigitaliUn filtro digitale (numerico) e caratterizzato da un formula del tipo:
y(k) + a1y(k − 1) + a2y(k − 2) + · · ·+ amy(k −m) =
= b0u(k) + b1u(k − 1) + · · ·+ bnu(k − n)
y(k) = −a1y(k − 1)− a2y(k − 2) + · · · − amy(k −m) +
+b0u(k) + b1u(k − 1) + · · ·+ bnu(k − n)
u(k), ingresso
y(k), uscita
max(n,m) e l’ordine del filtroI coefficienti ai , bi determinano la tipologia del filtro e la frequenza ditaglio
Corrado Santoro Introduzione ai filtri digitali
Filtri
Ordine del filtroL’ordine di un filtro e legato alla selettivita del filtro stesso (pendenzadelle curve dello spettro)
order = 1order = 2order = 3order = 4
ft
Corrado Santoro Introduzione ai filtri digitali
Filtri
Filtro Passa-Basso di ordine 1
y(k) = (1− α)y(k − 1) + αu(k)
TC =1
2πft
α =∆Tcamp
TC + ∆Tcamp
TC: costante di tempo del filtro
∆Tcamp: intervallo di campionamento
Corrado Santoro Introduzione ai filtri digitali
Filtro Passa-Basso: Esempio
Moto di un robot su piano XY
x(k + 1) = x(k) + vx ∆Ty(k + 1) = y(k) + vy ∆T
vx , vy : velocita X e Y (considerate costanti).Supponiamo che la misura sia affetta da errore casuale di tipoGaussiano:
x(k) = x(k) + σx
y(k) = y(k) + σy
x(k), y(k): valori misuratiσx , σy : variabili casuali gaussiane a media nulla
Corrado Santoro Introduzione ai filtri digitali
Traiettoria Ideale e Misurata
Corrado Santoro Introduzione ai filtri digitali
Traiettoria Ideale e Filtrata
Uso di un Low-pass filter a 5 Hz
Corrado Santoro Introduzione ai filtri digitali
Filtro Passa-Basso e Offset
Presenza di Offset di MisuraI filtri funzionano bene se il rumore di misura ha medianulla
Tuttavia, se e presente un offset di misura, i filtri nonsono in grado di eliminarlo
Consideriamo:
x(k) = x(k) + σx
y(k) = y(k) + σy
x(k), y(k): valori misuratiσx , σy : variabili casuali gaussiane a media non nulla
Corrado Santoro Introduzione ai filtri digitali
Traiettoria Ideale e Filtrata
Rumore a media non nulla con Low-pass filter a 5 Hz
Corrado Santoro Introduzione ai filtri digitali
Filtri
Predizione e Adattamento
Corrado Santoro Introduzione ai filtri digitali
Filtri Predittivi
Filtri PredittiviI filtri predittivi si basano sulla derivazione di un modellodel comportamento del sistema
Attraverso il modello viene effettuata una stima dellevariabili di stato
Tale stima viene confrontata con i dati dei sensori (per levariabili di stato misurabili)
L’errore viene fornito ad un controllore PI il cui output vienesommato alla stima
Corrado Santoro Introduzione ai filtri digitali
Filtri Predittivi
Filtri Predittivi
Modello delSistema
Dati dai sensori
Stato
Nuovo Stato(predizione)
+
-
ControllorePI
Errore sullapredizione
+
Correzionedella predizione
Corrado Santoro Introduzione ai filtri digitali
Filtri Predittivi
Filtri PredittiviI filtri predittivi permettono di attenuare i problemi relativi aoffset e rumore di misura
Tuttavia pongono il problema di richiedere la taratura di KPe KI
Valori “troppo piccoli” producono una bassa correzionedell’errore
Valori “troppo grandi” potrebbero produrre una crescitadell’errore o innescare oscillazioni nella stima
La scelta di KP e KI dovrebbe essere tale da poterminimizzare l’errore... facile a dirsi, ma a farsi?
Corrado Santoro Introduzione ai filtri digitali
Filtri di Kalman
Filtri di KalmanIl filtro di Kalman e un filtro predittivo di tipo proporzionaleil cui guadagno varia ad ogni ciclo
Il guadagno viene calcolato usando un approccio che hal’obiettivo di minimizzare l’errore
Il calcolo del guadagno si basa su un approccio statistico erichiede la caratterizzazione statistica del modello delsistema e della misura
Corrado Santoro Introduzione ai filtri digitali
Filtro di Kalman
Modello di sistemaIl sistema (discretizzato) si considera caratterizzato dallaseguente legge di aggiornamento:
x(k + 1) = A x(k) + w
Dove:x : vettore di stato
A: matrice di stato
w : vettore dell’incertezza (rumore) relativa alcomportamento del sistema
Corrado Santoro Introduzione ai filtri digitali
Filtro di Kalman
Modello della misuraI sensori si considerano caratterizzati dalla seguente relazione:
z(k) = H x(k) + v
Dove:z: valori misurati
H: matrice specifica quali grandezze sono misurate
v : vettore dell’incertezza (rumore) di misura
Corrado Santoro Introduzione ai filtri digitali
Filtro di Kalman
Caratterizzazione statisticaNei modelli indicati w e v sono delle grandezze aleatorie(variabili random)
Il filtro di Kalman necessita della conoscenza dellastatistica di w e v
In particolare, e necessario conoscere la varianza (eco-varianza) di ogni elemento dei vettori w e vOssia la varianza dell’incertezza del sistema e della misuraper ognuna delle variabili di stato
Corrado Santoro Introduzione ai filtri digitali
Filtro di Kalman
Matrice di CovarianzaSia q = [q1, ...,qn] un vettore di variabili aleatorie, la matrice dicovarianza Q e una matrice n × n, simmetrica, in cui ognielemento (i , j) e definito da:
Qi,j =1
N − 1
N∑k=1
(q(k)i − qi)(q(k)
j − qj)
Dove:q(k)
i : insieme dei valori assunti dalla variabile aleatoria qi
N: dimensione dell’insieme dei valori assunti dalle variabilialeatorieqi : media (valore atteso) dalla variabile aleatoria qi
Se q1, ...,qn sono statisticamente indipendenti, Q e unamatrice diagonale
Corrado Santoro Introduzione ai filtri digitali
Filtro di Kalman
Matrici di CovarianzaSi definiscono:
Q: matrice di covarianza dell’incertezza del sistema w
R: matrice di covarianza dell’incertezza di misura v
Corrado Santoro Introduzione ai filtri digitali
Filtro di Kalman
Step di predizioneSiano:
x : vettore di stato reale (che non si conosce)
x : vettore di stato stimato
L’algoritmo del filtro di Kalman opera come un filtro predittivo:1 x(k) = A x(k − 1), stima2 E(k) = z(k)− Hx(k), calcolo dell’errore rispetto alla stima3 K (k) = ...., calcolo del guadagno ottimo4 x(k) = x(k) + K (k)(z(k)− Hx(k)), correzione della stima
Corrado Santoro Introduzione ai filtri digitali
Filtro di Kalman
Calcolo del Guadagno ottimo
Il calcolo di K (k) si basa sul tentativo di minimizzare l’erroretra stato predetto e stato reale:
min{x(k)− x(k)}
Noi non conosciano x(k) tuttavia abbiamo la caratterizzazionestatistica della sua incertezza w .
A partire dalla matrice di covarianza Q, l’algoritmo del filtro diKalman determina la covarianza dell’errore P e quindi calcolail valore di K che minimizzi tale covarianza.
Corrado Santoro Introduzione ai filtri digitali
Filtro di Kalman
Calcolo Guadagno Ottimo1 P(k) = A P(k) AT + Q
aggiornamento della stima della covarianza dell’errore
2 K (k) = P(k) HT (H P(k) HT + R)−1
calcolo del guadagno ottimo3 P(k) = (I − K (k) H) P(k)
Aggiornamento della stima della covarianza dell’erroresulla base del guadagno ottimo
Corrado Santoro Introduzione ai filtri digitali
Filtro di Kalman
Algoritmo
1 x = A x Predizione
2 P = A P AT + Q Aggiornamento Covarianza Errore
3 K = P HT (H P HT + R)−1 Calcolo Guadagno
4 x = x + K (z − Hx) Correzione Misura
5 P = (I − K H) P Correzione Covarianza Errore
Corrado Santoro Introduzione ai filtri digitali
Low-pass vs. Kalman
Corrado Santoro Introduzione ai filtri digitali
Introduzione ai filtri digitali
Corrado Santoro
ARSLAB - Autonomous and Robotic Systems LaboratoryDipartimento di Matematica e Informatica - Universita di Catania, Italy
Programmazione Sistemi Robotici
Corrado Santoro Introduzione ai filtri digitali