Sensor fusion and state estimation in mobile robotics (PDF, 2.5Mb ...

Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 2010-11, 8ο Εξάμηνο 

Μάθημα: Ρομποτική ΙΙ. Διδάσκων: Κ.Τζαφέστας 

ΕΝΟΤΗΤΑ 2: 

Αυτόνομα Ευφυή Κινούμενα 

Ρομποτικά Συστήματα 

Σύνθεση αισθητηριακών πληροφοριών 

(data / sensor fusion) 

Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτική ΙΙ (Κ. Τζαφέστας) 

Αυτόνομα Ευφυή Κινούμενα Ρομπότ 

– Περιεχόμενα Ενότητας 

• Μηχανισμοί Κίνησης στο χώρο (locomotion) 

• Αισθητήριες Διατάξεις (sensing) 

– Proprioception (internal state ) / Exteroception etc. 

– Σύνθεση αισθητηριακών πληροφοριών (sensor fusion) 

• Αρχιτεκτονικές ελέγχου 

(mobile robot control architectures) 

• Αυτοεντοπισμός θέσης / Χαρτογράφηση 

(self-localisation / map-building) 

• Σχεδιασμός Δρόμου - Πλοήγηση 

(path planning / navigation) 


perception 

action 

1 

motion 

control 

sensing 

reasoning 

2


(sensor fusion) – Εισαγωγή (1) 

Βασικό Πρόβλημα: 

Εκτίμηση ενός φυσικού μεγέθους (π.χ. θέση x του ρομπότ) συνδυάζοντας 

n διαφορετικές ενδείξεις {z i } (i=1,...,n) (π.χ. από διαφορετικούς αισθητήρες) 

Έστω ότι όλες οι μετρήσεις zi έχουν την ίδια «αξιοπιστία», και έστω ότι 

κάθε μέτρηση είναι το αποτέλεσμα μιας στοχαστικής (Gaussian) 

διαδικασίας με μέση τιμή zi και διασπορά σ 2 

i = σταθ. για όλα τα i. 

Τότε έχουμε πολύ απλά για την «συνολική εκτίμηση» της μεταβλητής x: 

2 

2 

(unweighted averaging) ˆ σ = i 

και 

(1) 

ˆ = i 

n 

∑z x 

σ 

n 




Έστω ότι όλες οι μετρήσεις z i δεν έχουν την ίδια «αξιοπιστία», δηλαδή ότι 

για κάθε μέτρηση z i αντιστοιχεί διαφορετική διασπορά (variance) σ i 2 

Τότε για την «συνολική εκτίμηση» ˆx της μεταβλητής x: πρέπει κάθε 

μέτρηση να ληφθεί υπ’όψη με διαφορετική βαρύτητα, δηλ. να αθροιστεί 

με κατάλληλο βάρος, για τον υπολογισμό μιας «ανισοβαρούς βέλτιστης 

μέσης τιμής» (weighted mean – σταθμισμένος μέσος). 

Απλό Παράδειγμα –1 

Έστω ότι αποκτούμε σειριακά δύο μετρήσεις, z 1 (τη χρονική στιγμή t 1 ) και 

z 2 (τη χρονική στιγμή t 2 ≅t 1 ) με τυπική απόκλιση σ z1 και σ z2 αντίστοιχα. 

( ) 

Έχουμε: ˆ ⎛t ⎞ 

⎜ 1⎟= E x| z1= 

και 

⎝ ⎠ 1 

Με την υπόθεση της Gaussian κατανομής, μπορούμε να υπολογίσουμε μια 

συνολική βέλτιστη εκτίμηση, με δεδομένη και τη μέτρηση z2 , ως εξής: 

x z ˆ σ ⎛t⎞ ⎜ 1⎟ σ 

⎝ ⎠ z1 

= 

σ σ 

2 2 

xˆ⎛t⎞ z2 z1 

⎜ 2 ⎟= 

⋅ z 2 2 1+ ⋅z 

⎝ ⎠ 2 2 2 

σz1+ σz2 σz1+ σz2 

και 

1 = 1 + 1 

σ σ σ 

( t ) 

2 2 2 

ˆ 2 z1 z2 


(2) 

3 

4



Απλό Παράδειγμα –1 (συνέχεια) 

ˆ ⎛t⎞ ⎜ 1⎟ ⎝ ⎠ 1 

Η παραπάνω σχέση (2) γράφεται επίσης ως εξής : 

= x z 

ˆ σ ⎛t⎞ ⎜ 1⎟ σ z1 

και: 

⎛t ⎞ ⎛ 

2 t ⎞ 

1 K⎛ ⎛ 

⎛ ⎞ 

⎞ 

t ⎞ 

⎜ ⎟= ⎜ ⎟+ ⎜ 2⎟⎜ 2− ⎜t ⎟ 

⎜ 1⎟ 

⎝ ⎠ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠⎟ 

⎝ ⎠ 

xˆ xˆ z xˆ 

όπου: 

Kt 

⎛ ⎞ 

⎜ 2 ⎟ 

⎝ ⎠ = 

σ 

σ σ 

2 

z1 

2 2 

z1+ z2 

2⎛ 2 2 

t ⎞ ⎛ 

2 t ⎞ 

1 K⎛t ⎞ ⎛ 

2 t ⎞ 

⎜ ⎟= ⎜ ⎟− ⎜ ⎟ ⎜ 1⎟ 

⎝ ⎠ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ 

ˆ σ ˆ σ ˆ σ 

(3) 

⎝ ⎠ = 




μ z1K z1, z ⎛ ⎞ 

2 ⎜ 2 1⎟ 

⎝ ⎠ 

xˆ = = + ( ) z −z 

όπου: 

και: 

2 

K( 

z z ) 

σ z 

2 

1 

1, 2 = 2 2 

σ z1+ σ z2 

ˆ σ = ˆ σ −K( 

) ˆ σ 

2 2 

z1 z1, z2 

z1 


Γραφική απεικόνιση – Συνδυασμός 

δύο μετρήσεων z 1 και z 2 


5 

6




Εισάγουμε τώρα μια απλή δυναμική συμπεριφορά (πεδίο του χρόνου) στο 

σύστημα: 

dx/dt = u + w 

όπου u σήμα ελέγχου που αντιστοιχεί στην ονομαστική ταχύτητα του ρομπότ, 

και w σήμα θορύβου που οφείλεται στην ύπαρξη αβεβαιότητας (διαταραχών) 

στην πραγματική κίνηση του ρομποτικού συστήματος. Υποθέτουμε συνήθως 

ότι το w μοντελοποιείται ως «λευκός Γκαουσσιανός θόρυβος»: (0,σ w 2 ). 

− 

xˆ t = xˆ 

t + u t −t 

⎛ ⎞ 

⎜ 3 ⎟ 

⎝ ⎠ 

⎛ ⎞ 

⎜ 2 ⎟ 

⎝ ⎠ 

⎛ 

⎜ 

⎝ 3 

⎞ 

2⎟ 

⎠ 

2⎛ − ⎞ 

⎜t3⎟= ⎝ ⎠ 

2⎛t 

⎞ 

⎜ 2 ⎟+ 

⎝ ⎠ 

2 ⎛ ⎞ 

⎜t3−t2⎟ ⎝ ⎠ 

ˆ σ ˆ σ σw 




t=t 3 – : 

t=t 3 : 

(4) 

− 

xˆ t = xˆ 

t + u t −t 

⎛ ⎞ ⎛ ⎞ ⎛ ⎞ 

⎜ 3 ⎟ ⎜ 2 ⎟ ⎜ 3 2⎟ 

⎝ ⎠ ⎝ ⎠ ⎝ ⎠ 

2⎛ − ⎞ 2 2 

t ⎛ 

3 t ⎞ ⎛ ⎞ 

⎜ ⎟= ⎜ 2 ⎟+ 

⎜t3−t2⎟ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ 

ˆ σ ˆ σ σw 

⎛ ⎞ 

⎛t ⎞ ⎛ −⎞ 3 t3 K⎛ ⎛ ⎞ t ⎞ − 

⎜ ⎟= ⎜ ⎟+ ⎜ ⎜ 

3⎟ 

3− 

⎜t ⎟ 

3 ⎟ 

⎝ ⎠ ⎝ ⎠ ⎝ ⎠⎜ ⎝ ⎠⎟ 

⎝ ⎠ 

xˆ xˆ z xˆ 

2⎛ 2 2 

t ⎞ ⎛ −⎞ 3 t3 K⎛ ⎛ ⎞ 

t ⎞ − 

⎜ ⎟= ⎜ ⎟− ⎜ 3⎟ 

⎜t3 ⎟ 

⎝ ⎠ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ 

ˆ σ ˆ σ ˆ σ 

όπου: 

2 ⎛ − ⎞ ˆ σ ⎜t3⎟ ⎛ ⎞ ⎝ ⎠ 

⎜ 3 ⎟= 

⎝ ⎠ ⎡ 2 − 2 

ˆ σ ⎛ ⎤ 

t ⎞ 

⎢ ⎜ 3 ⎟+ 

σ 

⎝ ⎠ z3⎥ 

⎣ ⎦ 

Kt 

Αρχική πρόβλεψη στη χρονική στιγμή t 3 – 

⎛ − ⎞ 

⎜ 3 ⎟ 

⎝ ⎠ 

xˆ 

t 

2 ⎛ − ⎞ 

⎜ 3 ⎟ 

⎝ ⎠ 

, ˆ σ t 

Νέα μέτρηση z 3 

Βασική δομή αναδρομικού φίλτρου 

Kalman στο διακριτό χρόνο, για τον 

αναδρομικό υπολογισμό σε κάθε 

χρονική στιγμή της νέας «βέλτιστης» 

εκτίμησης, και των στοχαστικών 

ιδιοτήτων αυτής (διασπορά) 


7 

8


– Φίλτρο Kalman – Εισαγωγή (1) 

Φίλτρο Kalman: αναδρομικός αλγόριθμος εκτίμησης της κατάστασης ενός 

συστήματος (recursive state-estimation), χρησιμοποιώντας γραμμικό δυναμικό 

μοντέλο της συμπεριφοράς του συστήματος (linear plant model) καθώς και 

μοντέλο της λειτουργίας των αισθητήρων (sensor/measurement model). 

Συμβολισμοί/Ορολογία 

xˆ 

( kk | ) 

1 

: εκτίμηση κατάστασης στη χρονική στιγμή k, χρησιμοποιώντας 

δεδομένα μετρήσεων μέχρι και τη χρονική στιγμή k 1 

⎛ ⎞ 

x() k ⎜ηήxˆ() − ⎟= 

xˆ( 

k| k−1) 

⎝ k ⎠ 

(prediction) 

⎛ ⎞ 

xˆ( k) ⎜ηήxˆ( k + ) ⎟= 

xˆ( 

k| k) 

⎝ ⎠ 

(updated estimate) 

: πρόβλεψη κατάστασης τη στιγμή k, 

χρησιμοποιώντας δεδομένα μετρήσεων 

μέχρι και τη χρονική στιγμή k-1 

: νέα (ανανεωμένη) εκτίμηση κατάστασης 

τη στιγμή k, χρησιμοποιώντας δεδομένα 

μετρήσεων μέχρι και τη χρονική στιγμή k 



– Φίλτρο Kalman – Εισαγωγή (2) 

Μοντέλο μετρήσεων (ή μοντέλο αισθητήρων): 

[ ξ] 

z( k) = hx( k), + v ( k) 

(measurement/sensor model) 

noise function, with covariance matrix C v 

model of the environment 

Γραμμικό μοντέλο μετρήσεων: z( k) = H⋅ 

x( k) + v( 

k) 

Μοντέλο συστήματος (plant model): 

[ ] 

x( k+ 1) = Φ x( k), u( k) + w( 

k) 

state transition function 

noise function, covariance matrix C w 

control input 

Γραμμικό μοντέλο συστήματος: x( k+ 1) = A⋅ x( k) + B⋅ 

u( k) + w( 

k) 

(6) 


(5) 

9 

10

Σύνθεση αισθητηριακών πληροφοριών: 

Deterministic Least-Squares estimation 

z = H⋅ 

x+ v (z: διάνυσμα l x 1, x: διάνυσμα n x 1) 

Εαν l ≥ n τότε: 

( ) 1 − 

⎡ T T⎤ 

xˆ= H 

⎢⎣ ⋅H ⋅H ⋅z 

⎥⎦ 

least-squares estimate 

⇒ minℑ 

1 = 

T 

z −H⋅xˆ z−H⋅xˆ ( ) 1 − 

( ) ( ) 

⎡ T -1 T -1⎤ 

ˆ = HRH 

⎢⎣ HR ⋅ 

⎥⎦ 

⇒ minℑ 

2 = 

T −1 

z −H⋅xˆ R z−H⋅xˆ x z weighted least-squares estimate 

( ) ( ) 

Deterministic optimal (in the least-squared sense) solutions: 

no probability density function associated with x and z 



Maximum-Likelihood Estimation 

z = H⋅ 

x+ v 

Maximum-likelihood estimate ˆx 

ˆx : μεγιστοποιεί την πιθανότητα p(z | x), δεδομένων των στατιστικών 

ιδιοτήτων του σήματος θορύβου v στις μετρήσεις των αισθητήρων 

Εαν υποθέσουμε κανονική (Gaussian) κατανομή για το θόρυβο v, με 

μηδενική μέση τιμή και μήτρα διακύμανσης (covariance) R, τότε: 

1 

( | ) 

/2 1/2 

(2 ) l 

p z x = ⋅e 

π R 

T 

( z H x −1 

) R ( z H x) 

1 

⎡ ⎤ 

− 

2⎢ 

− ⋅ − ⋅ ⎥ 

⎣ ⎦ 

T −1 

[ p z x ] ⇔ ( z − H⋅x) R ( z−H⋅x) max ( | ) min ⎡ ⎤ 

⎣ ⎦ 


(7) 

(8) 

p(z | x) ακολουθεί την κατανομή πιθανότητας p(v) 

⇔ (8) 

Άρα εδώ: maximum-likelihood estimate ≡ weighted least-squares estimate 

11 

12


Minimum-Variance Bayesian Estimate 

p( z | x) p( 

x) 

p( 

xz | ) = 

p( 

z) 

: Bayes theorem 

(a-posteriori conditional density function) 

( p(x), p(z) : a-priori probability density functions ) 

Minimum-variance Bayes’ estimate ˆx 

ˆx : ελαχιστοποιεί το συναρτησιακό: 

∞ ∞ ∞ 

T 

min ℑ= ... x−xˆ x−xˆ p( x| z) 

dx1dx2... dxn 

∫ ∫ ∫ S 

−∞ −∞ −∞ 

( ) ( ) 

Ex 

{ ( 

T 

ˆ) S( 

ˆ) 

} 

δηλαδή: ℑ= variance( xz | ) = x−x x−x| z 

(διακύμανση) (S: θετικά ορισμένη μήτρα nxn) 

∫∫ ∫ 

απ’όπου παίρνουμε: xˆ = ... x⋅p( x| z) ⋅ dx1dx2…dx = E ( x| z) 

n x 

και εάν x και v ακολουθούν κανονική (Gaussian) κατανομή, έχουμε: 

( ) 1 

−1 T −1 − 

T −1 

xˆ= ⎡ P ⎤ 

0 + H R H H R ⋅z 

⎢⎣ ⎥⎦ 

όπου P (9) 

0 : a-priori covariance 

matrix of x = cov(x) 



Αναδρομικά φίλτρα (recursive linear filters) 

Απλό Παράδειγμα-2: Εκτίμηση του βαθμωτού μεγέθους 

x στη βάση k μετρήσεων: z i=h⋅x+v i (i=1,…,k) 


−1 

k 

k i= 

1 

i 

⇒ xˆ ( k) = h 

∑z 

νέα μέτρηση zk+1 −1k+ 1 

xˆ( k+ 1) = h z 

k+ 1∑ i 

i= 1 

⇒ 

⎛ −1 k ⎞ −1 

xˆ( k 1) k ⎜h ⎟ 

+ = z h 

⎜ ⎟ k 1 

k 1 k i z + 

+ ∑ + 

⎜ ⎟ 

i 1 ⎟ k 1 

⎝ = + 

⎠ 

⇒ 

−1 

xˆ( k+ 1) = k xˆ( k) 

+ h z k+ 

1 

k+ 1 k+ 

1 

1 

ˆ( k 1) ˆ( k) k 1 ˆ( 

k) 

k 1 

h 

ή 

− 

x + = x + z + −h⋅x + 

⎛ ⎞ 

⎜ ⎟ 

⎝ ⎠ 

Γενικεύοντας για διανυσματικά μεγέθη z i , x: z i=H i⋅x+v i (i=1,…,k) και 

εισάγοντας και δυναμική στο χώρο κατάστασης: x k+1 = Ax k + Bu k + w k 

xˆ( k+ 1) = xˆ( k+ 1| k) + K z −H⋅xˆ( k+ 1| k) 

όπου: 

⎛ ⎞ 

k+ 

1⎜ 

k 1 k 1 

⎟ 

⎝ + + 

⎠ 

xˆ( k+ 1| k) = A⋅xˆ( k) 

+ B⋅uk 

(10) 

13 

14


– Φίλτρο Kalman (1) 

Φίλτρο Kalman – Υποθέσεις (assumptions) 

1. Γραμμικό μοντέλο συστήματος: 

xˆ( k+ 1| k) = A⋅xˆ( k) 

+ B⋅uk 

x( k+ 1) = A⋅ x( k) + B⋅ 

u( k) + w( 

k) 

2. Γραμμικό μοντέλο μετρήσεων: z( k) = H ⋅ x( k) + v( 

k) 

3. Θόρυβος συστήματος: Γκαουσιανός, λευκός, με μηδενική μέση τιμή E[wk ]=0, 

T 

και ασυσχέτιστος (uncorrelated) covariance matrix: C ( k ) = E[( 

w )( w ) ] 

(linear plant model) 


k 

(K-1) 

Έστω: ⎧ T ⎫ 

P( k) = E⎨e( k) ⋅e 

( k) 

⎬(state 

covariance matrix) όπου: e( k) = x( k) − xˆ( 

k) 

(K-1) 

⎩ ⎭ 

⎧ T ⎫ 

Ποιό το: P( k+ 1| k) = E⎨e( k+ 1| k) ⋅ e ( k+ 1| k) 

⎬ όπου: e( k+ 1| k) = x( k+ 1) − xˆ( 

k+ 1| k) 

⎩ ⎭ 

w k k 

T 

4. Αντίστοιχα για το θόρυβο μετρήσεων v(k), E[vk ]=0, C ( k ) = R = E[( 

v )( v ) ] 

Είναι: 

⎧ 

⎪ ( k+ 1| k) = E ⎛ 

⎨⎜ ⎪⎝ ⎩ 

( k+ 1) − ˆ( k+ 1| k) ⎞ ⎛ 

⎟⋅ ⎜ 

⎠ ⎝ 

T ⎫ 

⎪ 

( k+ 1) − ˆ( 

k+ 1| k) 

⎞ 

⎟ ⎬ 

⎠ ⎪⎭ 

( k+ 1) = A⋅ ( k) 

+ B⋅ 

k + k k k k 

P x x x x 

v k k k 



όπου: x x u w και: xˆ( + 1| ) = A⋅xˆ( ) + B⋅uk 

Άρα: 

⎧ T ⎫ 

⎪⎡ ⎛ ˆ ⎞ ⎤ ⎡ ⎛ ⎤ ⎪ 

k ˆ ⎞ 

⎨⎢ ⎜x x ⎟ w ⎥ ⎢ ⎜x x ⎟ wk⎥ 

⎬ 

⎪⎣ ⎝ ⎠ ⎦ ⎣ ⎝ ⎠ ⎦ 

⎩ ⎪⎭ 

P( k+ 1| k) = E A ( k) − ( k) + ⋅ A ( k) − ( k) 

+ 

T 

⇒ + = ⋅ ⋅ + 

P( k 1| k) A P( k) A C ( k) 

(state covariance matrix) (K-2) 

: Μήτρα συνδιακύμανσης της πρόβλεψης 

w 

υποδηλώνει πώς εξελίσσεται χρονικά η διακύμανση της μεταβλητής 

κατάστασης του συστήματος, δηλαδή πώς μεταβάλλεται η «αξιοπιστία» της 

εκτίμησης την οποία έχουμε σε κάθε k για την τρέχουσα κατάσταση x k του 

συστήματος, χωρίς νέα δεδομένα ανάδρασης z k (corrective feedback). 


15 

16



Νέα ανανεωμένη εκτίμηση της κατάστασης (updated estimate) χρησιμοποιώντας 

δεδομένα ανάδρασης (μετρήσεις) και από τη χρονική στιγμή k+1: 

xˆ( k+ 1) = xˆ( k+ 1| k) + K z −H⋅xˆ( k+ 1| k) 

⎛ ⎞ 

k+ 

1⎜ 

k 1 k 1 

⎟ 

⎝ + + 

⎠ 

r(k+1) 

r(k+1): innovation, difference between actual sensor reading : 

and predicted sensor data : 

(K-3) 

zk+ 

1 

Hk 1 ⋅xˆ 

( k+ 1| k) 

Εύρεση βέλτιστου κέρδους ανάδρασης του φίλτρου (Kalman gain) K k+1 

Υπολογισμός της νέας μήτρας διακύμανσης P(k+1) για την ανανεωμένη 

εκτίμηση xˆ 

( k + 1) της κατάστασης του συστήματος 




Εύρεση βέλτιστου κέρδους ανάδρασης του φίλτρου (Kalman gain) K k+1 

[ ] 

min ℑ = E ⎡ 

⎣e e ⎤ 

⎦= trace P 

k+ 1 

T 

k+ 1 k+ 1 k+ 

1 

Είναι: P( k+ 1) = E e( k+ 1) ⋅ e ( k+ 

1) όπου: e( k+ 1) = x( k+ 1) − xˆ( 

k+ 

1) 

⎡ T ⎤ 

⎢ ⎥ 

⎣ ⎦ 

Δεδομένου ότι από την (Κ3): xˆ( k+ 1) = ( − K H ) xˆ( 

k+ 1| k) 

+ K 

έχουμε: e( k+ 1) = ( I− 

Kk+ 1Hk+ 1) e( k+ 1| k) + Kk+ 1( Hk+ 

1x( k+ 

1) −zk+ 

1) 

Άρα: 

I z 

k+ 1 k+ 1 k+ 1 k+ 

1 

 

vk+ 

1 

T 

⎡ ⎤ ⎡ ⎤ 

T 

⎢⎣ k+ 1 k+ 1 ⋅ ⋅ 

⎥⎦ ⎢⎣ k+ 1 k+ 1⎥⎦ k+ 1 k+ 

1 k+ 

1 

P( k+ 1) = I−K H P( k+ 1| k) 

I− K H + K R K 

Ισχύει όμως: 

∂ 

∂ 

T 

⎡ ⎤ 

A ⎣ ⎦= 

2 ABA AB , όταν: BT =B (δηλ.: B συμμετρική μήτρα) 


+ 

(K-4) 

17 

18



Εύρεση βέλτιστου κέρδους ανάδρασης του φίλτρου Kalman (συνέχεια) 

T 

min ℑ k+ 1= E⎡ ⎣ek+ 1ek+ 1⎤ ⎦ = trace[ 

Pk+ 

1] 

Άρα πρέπει: ∂ { trace [ ( k 1) ] } 

∂ K P + = 0 

k+ 

1 

ℑk+ 

1 

( ) ⋅⎡ ( k 1| k) 

⎤⋅ 

⎢ ⎥ ( ) 

και από τη σχέση (Κ-4): 

T 

k+ 1 k+ 1 ⎣ ⎦ k+ 1 k+ 1 k+ 

1 

−2I− K H P + H + 2K 

R = 0 

−1 

T ⎡ T 

⎤ 

k+ 1= ( k+ 1| k) ⋅ k+ 1⋅⎢k+ 1 ( k+ 1| k) 

k+ 1+ 

k 1⎥ 

⎣ + ⎦ 

K P H H P H R (K-5) 

Bέλτιστο κέρδος ανάδρασης (optimal Kalman gain) 




Υπολογισμός της νέας μήτρας διακύμανσης P(k+1) για την ανανεωμένη 

εκτίμηση xˆ 

( k + 1) της κατάστασης του συστήματος 

Από τη σχέση (Κ-4): 

T 

⎡ ⎤ ⎡ ⎤ 

T 

⎢⎣ k+ 1 k+ 1 ⋅ ⋅ 

⎥⎦ ⎢⎣ k+ 1 k+ 1⎥⎦ k+ 1 k+ 

1 k+ 

1 

P( k+ 1) = I−K H P( k+ 1| k) 

I− K H + K R K 

Αντικαθιστώντας την (Κ-5): 

( k+ 1) = − ( k+ 1| k) 

P I K H P (K-4)’ 

⎡ ⎤ 

⎢⎣ k+ 1 k+ 

1⎥⎦ 

⋅ 


19 

20

Σύνθεση αισθητηριακών πληροφοριών – 

Φίλτρο Kalman: Σύνοψη 

Αρχική εκτίμηση x ˆ( k = 0) με C ( ) 

μέ 

w k 

xˆ( k+ 1| k) = A⋅xˆ( k) 

+ B⋅u 

k (linear plant model) (K-1) 

T 

P( k+ 1| k) = A⋅ P( k) ⋅A 

+ C w( 

k) 

(state covariance matrix) (K-2) 

Νέα ανανεωμένη εκτίμηση κατάστασης και μήτρα διακύμανσης 

xˆ( k+ 1) = xˆ( k+ 1| k) + K z −H⋅x ˆ( 

k+ 1| k) 

(K-3) 

⎡ ⎤ 

⎢⎣ k+ 1 k+ 

1⎥⎦ 

⋅ 

και C ( k ) = R , ( = 1, …) 

v k k 

⎛ ⎞ 

k+ 

1⎜ 

k 1 k 1 

⎟ 

⎝ + + 

⎠ 

P( k+ 1) = I−K H P ( k+ 1| k) 

(K-4)’ 

όπου, βέλτιστο κέρδος ανάδρασης (optimal Kalman gain) 

−1 

T ⎡ T 

⎤ 

k+ 1= ( k+ 1| k) ⋅ k+ 1⋅⎢k+ 1 ( k+ 1| k) 

k 1 k 1⎥ 

⎣ 

+ + + ⎦ 

K P H H P H R (K-5) 



– Παραδείγματα 

Παράδειγμα 1 

Έστω ολονομικό (omni-directional) ρομπότ πάνω στο επίπεδο -xy 

Το διάνυσμα κατάστασης του ρομπότ είναι: x(k) = [ x(k), y(k) ] T 

Το διάνυσμα σημάτων ελέγχου είναι: u(k) = [ Δx(k), Δy(k)] T 

Η εξίσωση κατάστασης είναι: 

(δηλαδή, Α=Ι) 

⎡x( k) +Δ x( k) + wx( k) 

⎤ 

x( 

k + 1) = 

⎢yk ( ) +Δ yk ( ) + wy( k) 

⎥ 

⎣ ⎦ 

Έστω ότι το ρομπότ διαθέτει αισθητήρα που παρέχει μια μέτρηση της 

θέσης [x(k),y(k)] του ρομπότ από ένα σημείο αναφοράς [0,0] 

x( k) + vx( k) 1 0 xk ( ) vx( k) 

( k) 

= 

⎡ ⎤ ⎡ ⎤⎡ 

⎤ ⎡ ⎤ 

Δηλαδή: z 

= + 

⎢⎣yk ( ) + vy( k) ⎥⎦ ⎢⎣0 1⎥⎦⎢⎣yk 

( ) ⎥⎦ 

⎢⎣vy( k) 

⎥ 

(Π.1-2) 

⎦ 

 

Hk 

x( 

k ) v( 

k ) 


(Π.1-1) 

21 

22

Σύνθεση αισθητηρίων πληροφοριών 

– Παραδείγματα (συνέχεια) (1) 

Παράδειγμα 1 (συνέχεια) 

Έστω C w (k)= C w =σταθ. και C v (k)=R=C v =σταθ. 

Έχουμε, εφαρμόζοντας το αναδρομικό φίλτρο Kalman: 

με 

xˆ( k) + Δx( 

k) 

xˆ( 

k+ 1| k) 

= 

⎡ ⎤ 

⎢⎣yk ˆ( ) +Δyk 

( ) ⎥⎦ 

(Π.1-3) 

P( k+ 1| k) A P( k) ⋅ A C ( k) = P( k) 

C 

T 

= ⋅ + w + w 

Βέλτιστο κέρδος ανάδρασης φίλτρου Kalman (H=I): 

−1 

k 1 ( k 1| k) ⎡ ( k 1| k) 

⎤ 

+ = + ⋅ + + 

⎢⎣ ⎥⎦ 

K P P R 

(Π.1-5) 

(Π.1-4) 

Νέα ανανεωμένη εκτίμηση κατάστασης και νέα μήτρα διακύμανσης 

xˆ( k+ 1) = xˆ( k+ 1| k) + K z −xˆ( 

k+ 1| k) 

και P( k+ 1) = ⎡I−K ⎤ 

1 P( 

k+ 1| k) 

⎛ ⎞ 

k+ 

1⎜ 

k 1 

⎟ 

⎝ + 

⎠ 

πρόβλεψη/εκτίμηση 

κατάστασης (a-priori) 

αβεβαιότητα 

πρόβλεψης 

Kalman (optimal) 

feedback gain 

⎢⎣ k+ 

⎥⎦ 


2 2 

2 2 

σx + σwx 

y + wy 

xˆ = x+ ( z ) 

2 2 2 x −x 

yˆ= y+ ( z ) 

2 2 2 y −y 

σx + σw + σ x v 

σ x 

y + σw + σ y vy 


(Π.1-6) 




2 2 

Έστω, π.χ.: Cw = diag [ σw ( k), σ ( ) ] 

x w k y 

Έχουμε, εφαρμόζοντας το αναδρομικό φίλτρο Kalman: 

2 2 2 2 

Pk ( + 1| k) = Pk ( ) + Cw = diag [ ( σx( k) + σw ( k) ) , ( σ ( ) ( ) ) ] 

x y k + σw 

k y 

Βέλτιστο κέρδος ανάδρασης φίλτρου Kalman (H=I): 

[ ] -1 

Kk ( + 1) = Pk ( + 1| k) Pk ( + 1| k) + R = 

2 2 

⎡σx + σwx = ⎢ 

⎣ 0 

2 2 2 

0 ⎤⎡σx + σw + σ x vx 

2 2 ⎥⎢ 

σy + σw 0 

y⎦⎣ 0 ⎤ 

2 2 2⎥ 

σy + σw + σ y vy⎦ 

2 2 2 2 2 

⎡( σx + σw ) ( σ ) 

x x + σw + σ x vx 

= ⎢ 

⎣ 0 

Νέα ανανεωμένη εκτίμηση κατάστασης 

0 ⎤ 

2 2 2 2 2 ⎥ 

( σy + σw ) ( σ ) 

y y + σw + σ y vy 

⎦ 

σ σ 

-1 

23 

24



Παράδειγμα 2–Επεκταμένο Φίλτρο Kalman (Extended KF) 

Έστω κινούμενο ρομπότ διαφορικής οδήγησης (differential drive) 

Το διάνυσμα κατάστασης του ρομπότ είναι: x(k) = [ x(k), y(k), θ(k) ] T 

Το διάνυσμα σημάτων ελέγχου είναι: u(k) = [ D(k), Δθ(k) ] T 

Το μη-γραμμικό μοντέλο του συστήματος στο χώρο κατάστασης: 

x( k+ 1) = Φ[ 

x( k), u( k) ] + w( k) 

με Φ[ x u ] 

⎡x( k) + D( k)cos ( θ ( k) 

) ⎤ 

⎢ ( θ ) ⎥ 

⎢⎣ θ( k) +Δθ( 

k) 

⎥⎦ 

( k), ( k) = y( k) + D( k)sin ( k) 

C w (k): μήτρα διακύμανσης (covariance matrix) της τυχαίας διαταραχής w(k) 

( k)( k) 

⎡ T ⎤ 

w( k) E ⎢ ⎥ 

⎢⎣ ⎥⎦ 

⎡ 2 ⎤ 

⎢σ x 0 0 

2 ⎥ 

⎢ 0 σ y 0 ⎥ 

⎢ 2⎥ 

⎢ 0 0 σθ 

⎥ 

⎣ ⎦ 

C = w w = και E [ ] = 0 



w 

k 

(Π.2-1) 

(Π.2-2) 




Έστω τώρα ότι το κινούμενο ρομπότ διαθέτει αισθητήριο σύστημα που 

δίνει την απόσταση d του ρομπότ από κάποια συγκεκριμένη θέση 

αναφοράς (ορόσημο, landmark): [xb , yb ] (π.χ. special signal-emitting beacon) 

Το μη-γραμμικό μοντέλο μετρήσεων για το σύστημα, είναι τώρα: 

k = h[ k ξ] 

+ v k 

[ x( 

), ] = 

2 

= ( ( ) − b) + ( ( ) − b) 

z( ) x( ), ( ) με 

2 2 

ξ ⎡ ⎤ 

h k d x k x y k y 

⎣ ⎦ 

και γνωστή μήτρα διακύμανσης C v (k)= C v για το θόρυβο v(k) 

(Π.2-3) 

25 

26




( ˆ 

⎡xˆ( k) + D( k)cos θ ( k) 

) ⎤ 

⎢yk ˆ( ) Dk ( )sin ( ˆ θ ( k) 

) ⎥ 

⎢ ˆ 

⎣ θ( k) +Δθ( 

k) 

⎥⎦ 

x ˆ( k+ 1| k) 

= + 

(Π.2-4) 

( ˆ θ ) 

( ˆ θ ) 

⎡ ⎤ 

1 0 −Dk 

( )sin ( k) 

Ak =∇ Φ | = ⎢0 x= xˆ ( k) 

⎢0 1 

0 

Dk ( )cos 

1 

( k) 

⎥ 

⎥ 

(Π.2-5) 

⎣ ⎦ 

T 

P( k+ 1| k) = A⋅ P( k) ⋅A 

+ C w( 

k) 

(Π.2-6) 

πρόβλεψη/εκτίμηση 

κατάστασης (a-priori) 

γραμμικοποίηση 

χώρου κατάστασης 

αβεβαιότητα 

πρόβλεψης 





⎡2( xk ˆ( ) − xb) 

⎤ 

k+ 1 =∇ h x= xˆ( k) = ⎢2( yk ˆ( ) − yb) 

⎥ 

⎢ ⎥ 

⎣ 0 ⎦ 

H | (Π.2-7) 

Βέλτιστο κέρδος ανάδρασης φίλτρου Kalman: 

−1 

T ⎡ T ⎤ 

k+ 1= ( k+ 1| k) ⋅ k+ 1⋅⎢ k+ 

1 ( k+ 1| k) 

k 1 v ⎥ 

⎣ 

+ + 

⎦ 

K P H H P H C (Π.2-8) 

T 

γραμμικοποίηση 

μοντέλου μέτρησης 

Νέα ανανεωμένη εκτίμηση κατάστασης και νέα μήτρα διακύμανσης 

xˆ( k+ 1) = xˆ( 

k+ 1| k) +K k r( 

k+ 

1) 

και 

+ 1 

P( k+ 1) = I−K H P( 

k+ 1| k) 

⎡ ⎤ 

⎢⎣ k+ 1 k+ 

1⎥⎦ 

⋅ 

με r( k+ 1) = z −h( 

xˆ( 

k+ 1| k) 

) 


Kalman (optimal) 

feedback gain 

⎛ ⎞ 

⎜ k 1 

⎟ 

⎝ + 

⎠ 

(Π.2-10) 

(Π.2-9) 

27 

28



xˆ( 

k) 

xˆ( 

k+ 1| k) 

xˆ( 

k + 1) 

Αρχική Εκτίμηση Δράση / Κίνηση Μέτρηση 

Παράδειγμα 3: x( k+ 1) =Φ ((),()) x k u k + w() 

k z( k+ 1) = h((),()) x k u k + v() 

k 

⎡ ⎤ 

⎢px⎥ ⎡ ⎤ 

⎢ ⎥ ⎢D⎥ x= 

⎢py⎥ u= 

⎢ ⎥ 

⎢ ⎥ ⎢δθ⎥ ⎢θ⎥ ⎣ ⎦ 

⎣ ⎦ 

⎡r⎤ (range) 

z= 

⎢ ⎥ 

⎢ ⎥ 

⎢b⎥ (bearing) 

⎣ ⎦ 

⎡p ( ) ( )cos ( ( ) ) 

x k + D k θ k ⎤ 

Φ( x( k), u( 

k) 

) = ⎢ p ( ) ( )sin ( ( ) ) 

y k + D k θ k ⎥ 

⎢⎣ θ( k) + δθ( 

k) 

⎥⎦ 

2 2 

⎡ ( λx − px) + ( λy 

− py) 

⎤ 

h( 

x( k), u( 

k) 

) = ⎢ λy 

− p ⎥ 

⎛ y 

⎢ 

⎞ 

arc tan⎜ 

⎟−θ⎥ 

⎣ ⎝λx − px 

⎠ ⎦ 




Παράδειγμα 3 (συνέχεια): 

Πρόβλεψη – Μοντέλο κίνησης 

xˆ( 

k+ 1| k) 

= + 

A =∇ Φ | = 

k 

x= xˆ ( k) 

( ˆ 

⎡pˆ x ( k) + D( k)cos θ ( k) 

) ⎤ 

⎢pˆ ( ) ( )sin ( ˆ 

y k D k θ ( k) 

) ⎥ 

⎢ ˆ 

⎣ θ( k) +Δθ( 

k) 

⎥⎦ 

( ˆ θ ) 

( ˆ θ ) 

⎡1 0 −Dk 

( )sin ( k) 

⎤ 

⎢0 1 Dk ( )cos ( k) 

⎥ 

⎢0 0 1 ⎥ 

⎣ ⎦ 

P( k+ 1| k) = A ⋅ P( k) ⋅A 

+ C ( k) 

T 

k k w 

Μοντέλο μέτρησης 

H =∇ h | = 

k+ 

1 

x= xˆ ( k) 

( pˆx −λx) ( pˆ 

y −λy) 

⎡ ⎤ 

0 

⎢ r r ⎥ 

= ⎢ 

( y pˆ y) ( x pˆ 

⎥ 

λ − λ − x 

⎢ ) 

−1⎥ 

2 2 

⎣ r r ⎦ 


k+ 

1 

29 

( k+ 

1 ( ) ) 

xˆ( k+ 1) = xˆ( k+ 1| k) + K z −h 

xˆ( 

k+ 1| k) 

P( k+ 1) = I−K H P( 

k+ 1| k) 

⎡ ⎤ 

⎢⎣ k+ 1 k+ 

1⎥⎦ 

⋅ 

T T −1 

k+ 1= ( k+ 1| k) ⋅ k+ 1⋅⎡ k+ 

1 ( k+ 1| k) 

⎤ 

⎢ k+ 1+ 

v 

⎣ ⎥⎦ 

K P H H P H C 

d 

dx 

2 

( atan x) = 1 ( 1+ 

x ) 

30


– Χαρτογράφηση χώρου 

Τροχιά ρομπότ 

Sensor map 



– Αυτοεντοπισμός Θέσης (1) 

Open loop – no odometry 

Τροχιά ρομπότ 

Εκτίμηση θέσης ρομπότ 

With odometry – 

Robot kinematic uncertainties 

ρομπότ 


With odometry – 

Precise robot kinematics 

(stochastic errors) 


31 

32


– Αυτοεντοπισμός Θέσης (2) 


Τρέχουσα θέση ρομπότ 

οδομετρία 

: Πραγματική τροχιά ρομπότ 

ο : Εκτίμηση θέσης ρομπότ με χρήση οδομετρίας (odometry readings) 

+ : Βελτιωμένη εκτίμηση θέσης ρομπότ (IR readings – “geometric landmarks”) 



βελτιωμένη 

εκτίμηση θέσης 

Particle Filters – Sequential Monte Carlo 

Οι μέθοδοι Monte Carlo περιγράφουν, γενικά, κατανομές πιθανότητας ως σύνολα 

«σταθμισμένων δειγμάτων» (weighted samples) στο χώρο κατάστασης. 

Μελετώντας τα στατιστικά χαρακτηριστικά των δειγμάτων, καθώς εξελίσσονται 

μέσω της δυναμικής του συστήματος και της Bayesian λογικής, μπορεί να 

εκτιμηθεί, μέσω προσομοίωσης, η κατάσταση ενός στοχαστικού συστήματος. 

Particle Filters («φίλτρα σωματίων»): sequential Markov Chain Monte Carlo 

(MCMC) μέθοδοι, βάσει importance sampling («δειγματοληψία σημαντικότητας») 

Πρόβλημα: Η εκτίμηση της «εκ των υστέρων» (posterior) κατανομής πιθανότητας 

k k 

P( xk| Z , U , x0) 

η οποία προσεγγίζεται μέσω της κατανομής ενός συνόλου δειγμάτων {xi ,wi (i=1,…,N) 

(w 

} 

i : βάρη δειγμάτων) 

N 

i i 

( ) ≈∑( − ) 

i P x wδx x (όπου = 1 ) 

i= 

1 

∑ i w 

33 

34

Particle Filters - Εισαγωγή 

Έστω: 

xk 

: διάνυσμα κατάστασης προς εκτίμηση τη χρονική στιγμή k 

uk 

: διάνυσμα σημάτων ελέγχου, εφαρμοζόμενο στο χρονικό διάστημα [k–1, k], 

οδηγώντας την κατάσταση από το xk-1 στο xk . 

zk 

: διάνυσμα μετρήσεων (παρατήρησης) που λαμβάνεται τη χρονική στιγμή k 

k k−1 

Το ιστορικό των διανυσμάτων κατάστασης: X = { x0, x1, , 

xk} = { X , xk} 

k k−1 

Το ιστορικό των διανυσμάτων ελέγχου: U = { u1, u2, , 

uk} = { U , uk} 

k k−1 

Το ιστορικό των διανυσμάτων παρατήρησης: Z = z, z , , 

z = Z , z 

{ 1 2 k} { k} 

• Μοντέλο Παρατήρησης (μέτρησης) – Ανανέωση Εκτίμησης (Bayes’ rule) 

k−1k k k P( zk | xk) P( 

xk | Z , U , x0) 

P( 

xk| Z , U , x0) 

= 

k−1k P z | Z , U 

( k ) 

• Μοντέλο Πρόβλεψης – Μετάβασης Κατάστασης (Markov assumption) 

k−1 k 

P x | Z , U , x = P x | x , u × P x 

k−1 k−1 

| Z , U , x dx 

( ) ∫ ( ) ( ) 

k 0 k k−1 k k−1 0 k−1 


Δειγματοληψία – Importance Sampling 

Περιγραφή κατανομής πιθανότητας μέσω σταθμισμένων δειγμάτων: 

N 

i i 

( ) ≈∑( − ) 

i P x wδx x (όπου = 1 ) 

i= 

1 

∑ i w 

Επιλογή βαρών (weights), ώστε να προκύψει «πιστή» αναπαράσταση της 

εκτιμούμενης κατανομής πιθανότητας P(x): 

Importance Density («πιθανότητα σημαντικότητας») q(x) 

Δειγματοληψία της q(x), και στάθμιση των δειγμάτων ως: 


i 

( ) 

i 

( x ) 

i P 

∝ 

q 

x 

w 

(α) Εαν q(x) ομοιόμορφη κατανομή πυκνότητας πιθανότητας σε όλο το χώρο 

i i 

κατάστασης, τότε: w ∝ P x 

(regular grid model) 

( ) 

(β) Εαν q(x)=P(x), τότε w i (x)=(1/N) (particle distribution, with equal weights) 

35 

36

Δειγματοληψία Κατανομών (1) 

Προσέγγιση συναρτήσεων πυκνότητας πιθανότητας μέσω 

συνόλου δειγμάτων (approximating particle sets) 

Όσο μεγαλύτερος αριθμός δειγμάτων σε ένα διάστημα, τόσο μεγαλύτερη πιθανότητα 

Πώς μπορεί να γίνει η εξαγωγή δειγμάτων από μια συνάρτηση πιθανότητας; 



(α) Δειγματοληψία μέσω «Rejection Sampling» 

Έστω ότι f(x) c keep the sample 

otherwise reject the sample 


37 

38


(β) Δειγματοληψία μέσω «Importance Sampling» 

Χρήση διαφορετικής κατανομής q για την εξαγωγή δειγμάτων 

από την f 

Εισάγοντας ένα βάρος «σημαντικότητας» μπορούμε να 

σταθμίσουμε τα δείγματα, ισοσταθμίζοντας τις «διαφορές 

μεταξύ των q και f» 

w = f / q 

Η f είναι η συνάρτηση «στόχος» 

Η q καλείται συνάρτηση 

«σημαντικότητας» 

(importance or proposal) 


Particle Filters – Εισαγωγή (1) 

Στατιστική μέθοδος που βασίζεται στον κανόνα του Bayes και έχει χρησιμοποιηθεί 

σε πολλά συστήματα παρακολούθησης (tracking) αντικειμένων. 

Η βασική ιδέα της μεθόδου είναι η χρήση δειγμάτων (particles) για την 

αναπαράσταση του μοντέλου του συστήματος (αντί Gaussian κατανομών ή 

οποιουδήποτε άλλου μοντέλου) και η διάδοση μόνο των επικρατέστερων δειγμάτων. 

(“Survival of the fittest”) 

Βασικά Βήματα Αλγορίθμων που χρησιμοποιούν particle filtering: 

• Τοποθέτηση Ν δειγμάτων (particles) στο χώρο κατάστασης του συστήματος 

• Ανάθεση βαρών (wi ) στα δείγματα με βάση τις παρατηρήσεις (μετρήσεις) που είναι 

διαθέσιμες για το σύστημα 

• Επαναδειγματοληψία (resampling): δίνεται περισσότερη βαρύτητα στα δείγματα που 

έχουν μεγαλύτερο βάρος και "ακυρώνονται" τα δείγματα με μικρά βάρη 

• Εξέλιξη δειγμάτων (με βάση το μοντέλο μετάβασης κατάστασης του συστήματος) 

• Επανάληψη των παραπάνω από το δεύτερο βήμα. 


39 

40

Particle Filters – Εισαγωγή (2) 


Particle Filters (1) – Βασικές Σχέσεις 

Αρχική Κατάσταση: η προγενέστερη εκ των υστέρων (posterior) κατανομή 

{δείγματα, βάρη} 

N 

P δ 

{ } 1 

i i Nk 

− 

x , w 

k−1 k-1| k− 1 i= 

1 

i i i 

( x ) = ( x | x 1) 

q P − 

k k k k 

k−1 

k−1i i 

( xk−1| Z ) = ∑wk-1 

( xk−1−xk−1) Δειγματοληψία – Importance Sampling: Μοντέλο μετάβασης κατάστασης 

Πρόβλεψη: 

Nk 

i= 

1 

k−1i i 

( xk | Z ) = ∑wk-1 

( xk −xk) 

P δ 

Nk 

i= 

1 

k i i 

( xk | Z ) = ∑wk 

( xk − xk) 

P δ 

Νέα Δείγματα i 

k 

Ανανέωση Βαρών – Μοντέλο Παρατήρησης 

Βελτιωμένη 

Εκτίμηση: 

i= 

1 

(updated posterior) 

i 

x xk−1 

i i 

από τα με wk = wk−1 

όπου 

(importance sampling based 

on the transition prior) 


w 

πιθανοφάνεια 

μέτρησης 

( | = ) 

P( 

| = ) 

w P z x x 

i i 

i k-1 k = Nk 

j 

∑wk-1 

j= 

1 

k 

zk k 

xk k 

j 

xk 

41 

42

Particle Filters (2) - Υλοποίηση 

• Μετάβαση Κατάστασης: 

{ } 1 

i i Nk 

− 

x , w 

k−1 k-1| k− 1 i= 

1 

( −1, −1, 

) 

i i i 

k = k k k 

x f x n 

Draw N k samples from P(n k-1 ) 

• Ανανέωση Παρατήρησης: zk = h( xk, vk, 

k) 

( v k: noise sequence with known P(v k) ) 

i i 

{ x , w } 

Nk 

k kk | − 1 i= 

1 

( n k: noise sequence with known P(n k) ) 

i i 

{ x , w } 

k kk | − 1 i= 

1 


⇒ 

Nk 

με w = w 

i i 

k k−1 

Υπολογισμός «πιθανοφάνειας» 

(likelihoood) μέτρησης για κάθε δείγμα: 

i i 

Λ x = P z | x = x 

( k) ( k k k) 

i i 

{ x , w } 

( ) P( 

) 

i i i i i 

kk | ∝ kk | −1 Λ k = kk | −1 

k | k 

w w x w z x 

• Επαναδειγματοληψία (resampling): 

Nk 

k kk | i= 1 

Για την αποφυγή προβλημάτων «εκφυλισμού» των δειγμάτων (δηλαδή καταστάσεων όπου όλα τα βάρη 

πλην ενός τείνουν στο μηδέν) επιχειρείται «επαναδειγματοληψία» των δειγμάτων σε τακτά διαστήματα 

ώστε τα δείγματα να παραμένουν σχετικώς ισοσταθμισμένα 

Particle Filters (3) - Resampling 

Sequential Importance Resampling (Σειριακή Επαναδειγματοληψία σημαντικότητας) 

• Υπολογισμός κανονικοποιημένων βαρών (normalized importance weights): 

i i 

i wk−1P( zk | xk) 

wk 

= N 

i i 

w P z | x 

∑ 

i= 

1 

( ) 2 

• Υπολογισμός ενεργού αριθμού δειγμάτων (effective number of particles): 

N eff = 

1 

i 

∑ wk 

(«εξασθένησης» δειγμάτων) 

(αποφυγή sample impoverishment) 

i 

( ) 

k−1k k 

• Έλεγχος επαναδειγματοληψίας με βάση τον ενεργό αριθμό δειγμάτων: 

Εαν N eff

Εφαρμογή Particle Filtering (1-1) 

1. Οπτική Παρακολούθηση Χειρονομιών (visual hand gesture tracking) 

Πιθανότητα χρώματος 

( ( , ) ( , ) ) 

w = λ ⋅ p x y + λ ⋅diff x y ⋅w− 

i i 

k 1 i i 2 i i k 1 

Πιθανότητα κίνησης 



1. Οπτική Παρακολούθηση Χειρονομιών (συνέχεια) 


45 

46


2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (localisation) 

Κάθε δείγμα (particle) αντιστοιχεί σε μια δυνατή κατάσταση 

(θέση ή διάταξη) του ρομπότ στο χώρο 

Η εξέλιξη των δειγμάτων γίνεται με βάση την πιθανότητα 

(σφάλματα) που εισάγει το μοντέλο κίνησης (importance or 

proposal distribution prediction step) 

To μοντέλο παρατήρησης (μετρήσεων) χρησιμοποιείται για τον 

υπολογισμό νέων βαρών για τη στάθμιση των δειγμάτων και την 

επαναδειγματοληψία (correction step) 



2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 

Μοντέλο Κίνησης 


47 

48



Μοντέλο Παρατήρησης (active ranging sensors) 



2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ με sonars 


49 

50



Αρχική Κατανομή 




Κατανομή μετά την ενσωμάτωση 10 scans 


51 

52



Κατανομή μετά την ενσωμάτωση 65 scans 




Εκτιμούμενη Διαδρομή του ρομπότ 


53 

54







55 

56







57 

58







59 

60







61 

62







63 

64







65 

66




Βιβλιογραφία 

• A. Gelb (editor), Applied Optimal Estimation, The MIT Press, 1986. 

• D. Bertsekas, Dynamic Programming and Control, Athena Scientific: 

Mass., USA, 1995. 

• F. Lewis, Optimal Estimation, John Wiley, 1986. 

• B. Anderson, J. Moore, Optimal Filtering, Prentice Hall, 1979. 

• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2007. 

• G. Dudek, M. Jenkin, Computational Principles of Mobile Robotics, 

Cambridge University Press, 2000. 

• R. Siegwart, I. R. Nourbakhsh, Introduction to Autonomous Mobile Robots, 

The MIT Press, Cambridge, MA, 2004 

• Michael Montemerlo, Sebastian Thrun, FastSLAM – A Scalable Method for 

the Simultaneous Localization and Mapping Problem in Robotics, Springer 

Tracts in Advanced Robotics, 2007. 

• Siciliano, Bruno; Khatib, Oussama (Eds.), Springer Handbook of Robotics, 

Part C: Sensing and Perception, Springer 2008. 


67 

68

Sensor fusion and state estimation in mobile robotics (PDF, 2.5Mb ...

Create successful ePaper yourself

Delete template?

Save as template?