M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

… se si impiega anche la conoscenza del modello del sistema, si perviene al

Filtro di Kalman

Filtro di Kalman

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

È possibile costruire un filtro ricorsivo che prenda in ingresso tutte le

misure disponibili e combini tra loro le informazioni contenute in ciascun

dato, compresa la conoscenza del modello dello strumento che lo ha

fornito

Il temine ricorsivo è fondamentale per un’implementazione al calcolatore

del filtro di Kalman, esso indica infatti che l’algoritmo non richiede la

memorizzazione e la rielaborazione di tutta la storia dei dati ogni volta che

è disponibile una nuova misura

Filtro di Kalman

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Stima Ottima dello Stato del Sistema

Controlli

Stato del sistema (desiderato, ma non noto)

Modello di stato del sistema Modello di osservazione

Filtro di Kalman

Strumenti di misura

Sistema

Fonti di incertezzasul sistema

Fonti di incertezzasulle misure

Misure (Osservazioni)

Stima Ottima dello Stato del Sistema

Controlli

Stato del sistema (desiderato, ma non noto)

Modello di stato del sistema Modello di osservazione

Filtro di Kalman

Strumenti di misura

Sistema

Fonti di incertezzasul sistema

Fonti di incertezzasulle misure

Misure (Osservazioni)

La conoscenza degli ingressi (controlli) e uscite (misure) può essere utilizzata per effettuare una stima dello stato del sistema

Filtro di Kalman

Stima dello stato basata

su modello del sistema

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Le fonti di incertezza sul modello del sistema o incertezza di modello

derivano dalla non completa conoscenza di tutte le variabili che influiscono

sulla sua dinamica e da scostamenti dal modello matematico spesso

semplificato.

Le fonti di incertezza sulle misure comprendono:

- la non perfetta conoscenza di quali sono le relazioni tra le variabili di stato

e le uscite misurate (incertezza di modello degli strumenti) – effetti

principalmente modificanti

- l’incertezza dovuta agli errori casuali e sistematici – effetti principalmente

interferenti

Filtro di Kalman – fonti incertezza

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Sotto le seguenti ipotesi il filtro di Kalman fornisce una stima ottima

dello stato:

- il sistema sia descritto da un modello lineare

- il rumore associato al sistema e alle misure sia bianco e Gaussiano

(ovvero a media nulla, con densità di potenza

spettrale distribuita uniformemente su tutta la banda

di frequenze e con distribuzione di probabilità

Normale, e non correlato nelle sua realizzazione

ovvero non c’è correlazione tra un suo campione ed il

successivo!)

Filtro di Kalman - ipotesi

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Il modello di sistema lineare è giustificabile per varie ragioni.

- spesso un modello lineare si adatta bene allo scopo

- in presenza di non linearità, l’approccio ingegneristico più utilizzato è

quello della linearizzazione del modello attorno a una qualche

configurazione del sistema, ad esempio un punto o una traiettoria

- la teoria dei sistemi lineari è molto più completa e pratica di quelli non

lineari

Esistono dei metodi per estendere l’applicazione del filtro di Kalman ai

sistemi non lineari qualora essi dovessero rivelarsi inadeguati: Filtro di

Kalman esteso

Filtro di Kalman – ipotesi di linearità

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

L’ipotesi di rumore bianco implica che i valori di rumore non sono correlati

tra di loro nel tempo.

In pratica, se si conosce quanto vale il rumore all’istante attuale, ciò non

aggiunge nessuna informazione ai fini di una previsione su quale sarà il

suo valore in un altro istante.

Filtro di Kalman – ipotesi di rumore bianco

Inoltre un rumore bianco ha uno spettro con uguale densità di potenza per

tutte le frequenze, ciò implica che un tale segnale ha potenza infinita!,

pertanto un rumore bianco non può esistere in natura

Poiché però ogni sistema fisico ha una banda passante limitata nello

spazio delle frequenze ed è tipicamente affetto da un rumore a larga

banda, dal punto di vista del sistema è equivalente ad assumere che vi sia

rumore bianco

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Se invece all’interno della banda passante del sistema il rumore non ha

densità spettrale uniforme, oppure è correlato nel tempo, attraverso un

filtro aggiuntivo è possibile riprodurre, partendo da un rumore bianco,

qualsiasi forma di rumore correlato (il filtro consiste in un sistema lineare

chiamato “shaping filter”).

Tale filtro viene poi aggiunto alla dinamica del sistema.

Filtro di Kalman – ipotesi di rumore bianco

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Mentre l’attributo “bianco” per un rumore è riferito alle sue caratteristiche

temporali (o di frequenza), l’attributo “Gaussiano” è riferito alla sua

ampiezza. Cioè, per ogni singolo istante temporale, la densità di probabilità

dell’ampiezza di un rumore Gaussiano ha la nota forma di una campana.

Questa assunzione è giustificata in senso fisico dal fatto che, tipicamente,

vi è un gran numero di piccole sorgenti che contribuiscono a creare il

rumore di misura.

Questo fenomeno è descritto dal teorema del limite centrale:

all’aumentare del numero di variabili casuali indipendenti che si sommano

tra loro, qualora non ve ne sia nessuna preponderante, qualunque sia la

distribuzione di probabilità di ciascuna, la distribuzione di probabilità della

somma tende ad essere quella Gaussiana

Filtro di Kalman – ipotesi di rumore gaussiano

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Filtro di Kalman – modello sistema lineare

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Filtro di Kalman – modello sistema discreto

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

L’ipotesi di base per il filtro di Kalman in cui si assume che il rumore del processo di evoluzione dello stato e il rumore di misura siano a media nulla e temporalmente scorrelati si traduce rispettivamente con le equazioni

{ } { } 0,k kE v E w k= = ∀

{ } { }( ) ( ) ( ), ( ) ( ) ( )T Tij ijE v i v j Q i E w i w j R iδ δ= ⋅ = ⋅

è la stima dello stato all’istante k condizionata alle sole

informazioni ottenute fino all’istante k-1 è detta previsione di un passo in avanti.

Tale simbologia viene impiegata da ora in avanti

ˆ( | 1)x k k −

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Si assume che al passo k-esimo siano noti i seguenti parametri:

la stima ˆ( 1 | 1)x k k− −

la covarianza condizionata ( 1 | 1)P k k− −

Si calcola la previsione:

ˆ ˆ( | 1) ( ) ( 1 | 1) ( ) ( )x k k F k x k k B k u k− = − − +

( | 1) ( ) ( 1 | 1) ( ) ( ) ( ) ( )T TP k k F k P k k F k G k Q k G k− = − − +

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Si calcola l’aggiornamento:

ˆ ˆ ˆ( | ) ( | 1) ( )[ ( ) ( ) ( | 1)]x k k x k k W k z k H k x k k= − + − −

( | ) [1 ( ) ( )] ( | 1)[1 ( ) ( )] ( ) ( ) ( )T TP k k W k H k P k k W k H k W k R k W k= − − − +

1( ) ( | 1) ( )[ ( ) ( | 1) ( ) ( )]TW k P k k H k H k P k k H k R k −= − − +

Dove, la matrice di Kalman:

NOTA:

• la previsione è generata dal filtro utilizzando il modello di evoluzione del misurando

• l’aggiornamento fa uso del modello di misura

Scarto tra misura e previsione della misura

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Stima iniziale

Previsione

Modello del sistema

Misura

Aggiornamento

Filtro di Kalman – schema ricorsivo

€

ˆ x k | k −1( ) P k | k −1( )

€

ˆ x k | k( ) P k | k( )

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Si calcoli l’aggiornamento per un semplice caso scalare:

ˆ ˆ ˆ[ ]Fx x W z x= + ⋅ −

2

2 2x

x z

Wσ

σ σ=

+Dove, la matrice di Kalman:

Filtro di Kalman – filtraggio Bayesiano!

2 2 2

2 2 2 2 2 2

2 2

2 2 2 2

ˆ ˆ ˆ ˆ ˆ[ ]

ˆ1

x x xF

x z x z x z

x x

x z x z

x x z x x z x

z x

σ σ σσ σ σ σ σ σ

σ σσ σ σ σ

= + ⋅ − = + ⋅ − ⋅ =+ + +

⎛ ⎞⋅ + − ⋅ =⎜ ⎟+ +⎝ ⎠

2 2

2 2 2 2ˆx z

x z x z

z xσ σ

σ σ σ σ⋅ + ⋅

+ +

2

2

:

( | 1)

( )

1

x

z

ipotesi

P k k

R k

H

σ

σ

− =

==

(combinazione Bayesiana della misura z e della previsione x)

Si noti anche che se misura e previsione sono uguali non c’è aggiornamento

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Teorema di Bayes

Abbiamo visto i fondamenti dell’applicazione del teorema di

Bayes e del filtro di Kalman, per la combinazione delle

informazioni, ora vediamo un esempio concreto di SENSOR

FUSION!

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

• Odometers

• Gyro - No drift

- Limited repeatability (motion, poor environment, …)

1° STEP (a & b)1° STEP (c)

2° STEP

• Laser Triangulating Scanner

- Smooth updates

- Drift

1 & 2° STEP: Smooth updates & no drift

Esempio di applicazione (veicoli mobili)

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Sensor Fusion

Ln

Ln CX

En

En CX

Fn

Fn CX

Ek

Ek CX

Delay Comp.

Ckn

Ckn CX ++

Esempio di applicazione

( ) ( ) En

Ln

En

Ln

Ln

Ln

En

En

Fn XCCCXCCCX

11 −−+++=

( ) Ln

Ln

En

En

Fn CCCCC

1−+=

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Laser Scan data

LS_NAV computation time

Data Fusion

Odometric estimation

Si fondono i dati all’istante Ts e poi si somma la variazione di posa e l’accumulo di covarianza

Ts1 = nTl

time

Tcomp = nTl+ kTo

Ts2 = (n+m)Tl

Tl

To

Esempio di applicazione

€

Xn+kC = Xn

F + XkE E

kFn

Ckn CCC +=+

€

XkE

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

L’incertezza del LS_NAV è funzione della velocità angolare che provoca un degrado della mappa dovuto al moto del veicolo sovrapposto al moto di scansione del raggio laser

( ) ⎟⎟⎠

⎞⎜⎜⎝

⎛⋅+⋅⋅+⋅=

• 22

0 11 δβα vCC Ln

•

δv : velocità lineare ed angolare

Esempio di applicazione – stima incertezza laser

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

1.4 1.6 1.8 2 2.2 2.4 2.6 2.8 3 3.2 3.4

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

LS NAV,Odometric,Fused Trajectory

X [m]

Y [m]

searching range inputLS NAV estimated poseLS NAV first guess poseodometric traj.fused traj.

In evidence initial guess of pose and searching fields of possible solutions derived from computed covariance given as an input to the LS_NAV algorithm

Esempio di applicazione

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

50 60 70 80 90 100 110

-35

-30

-25

-20

-15

-10

-5

0

5

10

LS NAV and Fused Attitude

Odom. cycle time

[deg]

fused attitudeLS NAV attitudefused attitude uncertainty (95%)

76 78 80 82 84 86

-8

-7

-6

-5

-4

-3

-2

-1

0

1

2

LS NAV and Fused Attitude. Zoom

Odom. cycle time

[deg]

fused attitudeLS NAV attitudefused attitude uncertainty (95%)

Assetto fuso e relativo intervallo di confidenza

Esempio di applicazione – assetto fuso

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

-0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5-2.5

-2

-1.5

-1

-0.5

0

0.5Trajectories comparison

X [m]

Y [m]

EncoderSensor Fusion

Esempio di applicazione – compensazione deriva

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

SLAM

Simultanea localizzazione e mappatura di veicoli autonomi in

ambiente non strutturato

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Strutturato:- Ingegnerizzazione dell’ambiente- Bassa flessibilità- Conoscenza a priori della mappa

Non strutturato:- Non necessita di infrastrutture- Alta flessibilità- Costruzione mappa in tempo reale

Passi fondamentali per interagire con ambiente non-strutturato:

- navigazione e propagazione incertezza mediante sistemi di misura autocontenuti

- autolocalizzazione mediante landmark naturali

- fusione tra navigazione incrementale e quella riferita all’ambiente

- mappatura dell’ambiente stesso

Ambiente in cui il robot opera

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

- Veicolo a guida differenziale

- Sistema di acquisizione PXI LabView RTOS

Parametro Incertezza Range

distanze 5 cm 7-50 m

angoli 0.1 ° 0-180 °

- Laser a scansione IR [SICK PLS-101] : Tempo di volo

Risoluzione angolare max: 0.5°

Campo di scansione: (0°,180°)

Distanza massima: 50 m

Accuratezza = f(orientazione,materiali)

1

2D cT=

- 2 Encoder incrementali : nnoo:4000:4000

Veicolo e strumentazione

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

2

2XkC =

xk xy x

y yy y

x y

δ

δ δ

δ δ δδ

σ σ σσ σ σσ σ σ

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

( )

( )

10

10

10

cos

sin

2

Rk R Lk Lk k k

rid

Rk R Lk Lk k k

rid

Rk R Lk Lk k

rid

n R n Rx x

n

n R n Ry y

n

n R n R

b n

π δη

π δη

δ δ πη

+

+

+

Δ ⋅ + Δ ⋅= + ⋅

⋅

Δ ⋅ + Δ ⋅= + ⋅

⋅

Δ ⋅ − Δ ⋅= +

⋅ ⋅

TXk+1 Xk ök ù k ökC = C + J C J⋅ ⋅

( )( )

, 1 , 1

, 1 , 1

, 1

cos

sin

ls k r k ls k

ls k r k ls k

ls k k ls

x x

y y

δ

δ

δ δ ϑ

+ +

+ +

+

= + Δ ⋅

= + Δ ⋅

= +

Parametro Valore Nominale µ

Deviazione standard σ

RR 89.50 mm 0.14 mm

RL 89.50 mm 0.14 mm

L 89.50 mm 0.5 mm

b 89.50 mm 0.5 mm

-1 0 1 2 3 4 5 6 7-1

0

1

2

3

4

5

6

7

8

X (m)

Y (m)

POSA con INCERTEZZA

Equazione cinematica di ricorrenza:

Equazione cinematica di ricorrenza(PLS):

Covarianza della posa:

Propagazione incertezza:

[x,y,δ]δ

PLS

RR

ΔLS

b

θLS

αi,j

RL

kC =ω

Parametri incerti:

Propagazione incertezza odometrica

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Mappatura

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

A

A

A

p

S

S S S

W

AGV

ringhiera

scala

scala

Sono stati fatti compiere al Sono stati fatti compiere al veicolo 2 giri del laboratorio veicolo 2 giri del laboratorio per verificare i metodi per verificare i metodi sviluppatisviluppati

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Sono stati fatti compiere al veicolo 2 giri del laboratorio per verificare i Sono stati fatti compiere al veicolo 2 giri del laboratorio per verificare i metodi sviluppati: filmato che mostra l’ambiente, il veicolo ed una metodi sviluppati: filmato che mostra l’ambiente, il veicolo ed una tecnica di determinazione del percorso mediante scansioni e gestione del tecnica di determinazione del percorso mediante scansioni e gestione del grafo delle profonditàgrafo delle profondità

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion