Introduzione ai filtri digitali - unict.itsantoro/teaching/psr/slides/KalmanFiltering.pdf ·...

Introduzione ai filtri digitali

Corrado Santoro

ARSLAB - Autonomous and Robotic Systems LaboratoryDipartimento di Matematica e Informatica - Universita di Catania, Italy

[email protected]

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


Filtri

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


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


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


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


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


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


Traiettoria Ideale e Misurata


Traiettoria Ideale e Filtrata

Uso di un Low-pass filter a 5 Hz


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


Traiettoria Ideale e Filtrata

Rumore a media non nulla con Low-pass filter a 5 Hz


Filtri

Predizione e Adattamento


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


Filtri Predittivi

Filtri Predittivi

Modello delSistema

Dati dai sensori

Stato

Nuovo Stato(predizione)

+

-

ControllorePI

Errore sullapredizione

+

Correzionedella predizione


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?


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


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


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


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


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


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


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


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.


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


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


Low-pass vs. Kalman


Introduzione ai filtri digitali

Corrado Santoro

ARSLAB - Autonomous and Robotic Systems LaboratoryDipartimento di Matematica e Informatica - Universita di Catania, Italy

[email protected]

Programmazione Sistemi Robotici


Introduzione ai filtri digitali - unict.itsantoro/teaching/psr/slides/KalmanFiltering.pdf ·...

Documents

Transcript of Introduzione ai filtri digitali - unict.itsantoro/teaching/psr/slides/KalmanFiltering.pdf ·...