A Particle Filter Tutorial for Mobile Robot Localization ... - CIM (McGill)

!"!#! $! %!&

!" # #$% !

& " ' " (

) & * + ,

! + $## !+ $##% $

,& - . $ " ! . * &% , / ,0 + + 0& ' & 1 + / / ,0 * + ! 2 & + 2 & & & *

- * & 3 04 '

, (

+ 0' &

& "

5

" % ' , ,& &* 6 * + ,&

* & , 7 + ' *

& & &%' & 2 ' 8 ,& &% + , *

%

,& '

& "

' & 9 : % 9 " 3 4% 9 7 9 5 %' , ' & ,& % *

9 , 0, 9 5% , 2 9 % ,' 2 , & ,&

& 8 + & 5 * & , ( ' , 0 2 ;' ,& 9 % < ' 9 % 7 9 5 % ' ' ,&

,,

% = & &7 ,& , , ' , 1

& , 2 & " 2 / >' ? ,%' , ( + @ @

, , ' ;& 5% 8

: ; Æ : 9 : :' , : 9 @@ % ' , 9 @ A @ & : ' & : & =6 5 !6 ' ,

$

+ #7 9 7 9 5 9 7 9 5

= &%

9 A 5< =.. % % 9 %< 9 %
% 5 ! " %% 9 ! 9 %%

%

5%

%

>

Single step translation (σtrs=5, σdrf =1)

Single step translation (σtrs=1, σdrf =5)

40 30

50

50

20

20

10

0

0

0

0

−10

−20

−20

−50

−50

−30

−40 0

20

40 60 80 100 100cm distance travelled

0

−40


0


100

0

50 100 150 200cm distance travelled

200

150

100

100 50

50

100

200

50

100

0

0

0

0

−50

−100

−50

−50

−100 −100

−150 50

100 150 200 250 300 300cm distance travelled

−100

0


−200 0

!"

100 200 300cm distance travelled

300

0

200 400 400cm distance travelled

!"

;& $7 2 , 7 % 9 > 9 5 % 9 5 9 > Æ

Æ

9

>%

5 A !

B 2 1 , '

& ' ' &

% ,

,& & , & ,& 2 &<

+ ,& , && & , 6 & = , , , 6 ,& B ! H + &% ;& ,

& * *& ' , ' &' , ' , & & , * + &, . *&

, & ,

I % ' C

Sample Trajectory of an exploring Robot (*: Tracker, o: Odometer, .: Monte Carlo Samples)

Reproducing Thruns results

450

700 σ Trs :3 cm/m σ Rot :2 Deg/360Deg

400

σ Drft :2 Deg/m

600

350

500

300 Y−axis

400

300

250

200

200

150

100 100

0 50

−100 −400

−300

−200

−100

0

100

200

300

400

500

0

100

600

200

300

400

500

600

700

X−axis

!"

!"

;& 7 % G & - ' % . , C#

%' - Æ

, '

, & 2 % *& ' - , 0 & ,

&

+ !%

,& - , &

+ % 0 &

*& 0 , ,& & J & 0 , 2

; , 6 , & * $5%< , & * & & $$% # !

$

"

?

^φ w Observed Robot−Target (Moving)

φ^

θ^ m x m ,y m

ρ Observing Robot−Laser (Stationary) dx= xm− x s dy= ym−ys ρ = dx2 +dy 2

^θ w

x s,ys ^θ s

^θ =atan2(dy,dx)= ^ + ^ θ θs w ^φ =atan2(−dy,−dx)= ^ + ^ φ θm w ^ Tracker Returns: < ρ, θ, φ^>

^ θ

;& 7 , 0 &

& 9 : , & *% & 9 : , &% 0,' 0 : #:

=6 C7 9

9 : 9 #:

A

$ % :

$ % :

C%

, 9 9 0, % , & *% : #: % & 0 9 &% A 5%% & =6 ?7

A : A :% 9 A : A :% A 5% 9

:

$ A : A :

#:

?%

=6 C ? 6 !6 ' 6

,& & ' & ( : #:%' , 2 , ;' I

9 : 0, 0 & =6 I7

A

9 : 9

#:

$ % :

$ % :

9 :

I%

, 9 9 ,& & : =6 H% + =6 I # 2 % % , =6 H 0 & * , , , ,&

% 9

5 $$

5 $$

5 $$

H%

;& >

=6 H : #:% ,&& *

' ,&& , & ; ' & 9 # # # & 9 5## 5## > ' & =6 C 0 : #:

2 9 =6 H 0& & > % = ,

% * , % ) ' 0 * & :% & % & '

7 Æ

9 % % 9 %

5#%

0 * ,&& ' & ;& C'

;& C ,&& ;& C' + ,&& 2 ! ,& & =6 55% 5 % 9 $$

5 $$

H

5 $$

¼

55%

;& >7 0 ,&& &

=6 ? ,& & , & =6 5$% B A 5% 9 : : 9 3 4 9 % & % A 8

% 9

5 $$

5 $$

5

%$& ,& +

0,' 2 K

,&

7

, ,&%7 K 9

5#

;& C7 0 ,&& & ;& >

, ,

B& " 7

9

K

) & %%7

% 9 L %

" 7 . ' , ,& & 1 ,

, & %

9

K

7

' 55

^ θw Observing Robot−Laser (Moving)

Observed Robot−Target (Stationary)

ρ

^ θ

θm x m ,y m

dx= x s − x m dy= y s −y m ρ = dx2 +dy 2

x s,ys

^ =atan2(dy,dx)= ^θ+ θ θw m ^ =atan2(−dy,−dx)= ^φ+ θ φw s ^ ^ Tracker Returns: < ρ, θ, φ >

φ^ w

θs

φ^

;& ?7 F

& , 9 : : #: % 9 : % 0, 0 9

=6 5 =6 C%

A : 9 $ % : #:

$ %

5%

:

, 9 9 % & &% 0 : #: % 0, & & % 9 % & =6 5

A #: A : % 9 A #: A : % A 5% 9

:

$ A #: A :

:

5%

+ & & ,& , =6 H' 55' 5$

%& '

5$

110

Stationary Robot

Stationary Robot

100

Tracker

100

90 80

80

70 60

60

50

Motion

40

40

Motion

30 20

20

10

Moving Robot

Moving Robot 0

20

40

60

80

100

0

120

0

20

!"

40

60

80

100

120

!"

120

120

Stationary Robot 100

100

80

Stationary Robot

80

60

40

Motion

60

Motion

Tracker

40

Motion

20

Motion

20

0

0

Moving Robot

−20

Moving Robot

−20

−40

−40 0

50

100

150

200

0

!"

50

100

150

200

!"

;& I7 % * % & 0 % % & 0

5

;& I

, *

& #'#' #'5## + *& I & 5## '5## $# ;& I 0 0 & ,&% + 5## ' 5## *& I , & , & ,& 0 &% ;

' *& I , & & ,&

5 + / +

" B + , 0& & ' 5C7$C? $IH' 5HH> $ " /0 G ( " 1 & 0 ' 5$%7$>5 $C' + 5HH? " / 0 ; 0& ! ' I%7$5 $>' $### M / 7 , 0& "##$ ' & #I> #H#' . &' !+'' " I 5 5HH > M /' N =' G ;& %& ' ( ) E ./E 5 >CII5 #>I O + D ' B

' "+' 5HHC C M /' N =' G ;&' B " &7 6 ' 5%7$5 $H' + 5HH? ? M / G6 & ;& " * ' 5$C%7ICH II#' 5HHC I . " /1 + , - =, + ' ' 5HH H M ! ' ! 2' ; + * . %& ' 5C7$ ?' 5HHH 5# D0 .& !& G D +

& "##/ ' ' & $?I $?II' 5HH?

5

55 + ! D M D 0 . 1 , ' 5$%75C 5?C' " 5HH> 5$ ; 0

' B /& ' ; ' . )& & ' 1 ! === ' M 5HHH 5 ; 0

' ; ' B /& ' . "

1 0 ##1' " 5HHH 5 ; 0

+ . G $ 1 & & . 2332 ===' " $##$ 5> + ' E ; ' E ( ) ' ' .& P &' . . =&& . ' M $##5 5C (& 0 " M0 ' E ./E7 #>$5>CI?C> ! & ) ' " $### 5? (& 0 ! Q & P 1 , - ' " ' "E' 5HHC === 5I G ;&' M /' N = B 7 . &' )" "=+" H $5' ) " & ' + +' ) " & ' + +' "' ).+' 5HH 5H ;0 ' .& ' ; + ' R R0 ' R +' D 0 ' R 0 E & & 0 & 0 11 & % ' $' & 5># 5>>' 5HH> $# + (

4 " ' ! &' " ' 5H?

$5 ; (2 ' ! "

' " ' ( P

' Q

" & & 1 ' 5' & 5 5I ===' 5HH> $$ EM (' M . ' +;" . E J & 5 ' 5#$%75#? 55' + 5HH $ " +, / 0 ! &

0& ! ' $H5%7$ $I' 5HHI 5>

$ " +, / 0 7 )& , & 0& ,0 6 ! ' 5' & IH H#I' 5HHI $> M ' F

B-0' M +' " & + = & & 1 * 0 1' & $>5I $>$' . ; ' !+' ).+' + $### $C M ' F

B-0' M +' " & + ; 1 * 0 1' & $>5 $>?' . ; ' !+' ).+' + $### $? = D + , * & '7 8 ' I$. %7> >' 5HC# $I . D& ( . ) & & 0 1' & $#5 $#I ===' 5HHC $H D 1 . N . & & & ===' ' ' ' & $IHC $H#' 5HHI # D 1' .& N' .& E & ' E 0 . . & ' $' & 5$5 5$C ===' + 5HHC 5 D 1 .& E & ! & , ' $' & 5$># 5$>? ===' 5HH $ M M G N& ; B " 1 0& & ' ?%7?C I$' M 5HH5 ! G " & & * 0 ' 5$%7H 5#C' 5HH? M . G' & !' G&0 + ,0 6

& & + ' E ; ' EM (' ' ) ' .& P &' M $##5 > ; G = " (

& & & ' 7 H' 5HH? C ;& G = " 0, & $ & ' & $H $?>' 5HHI

5C

? M " ! 0 +, / 0 + 0&

- ! ' & >?$ >?I' 5HHH I " D1 (& 0 & & ' . &' !+' 5HH === H " 0 ' . 5H?H

' 5 + ' E, R0'

# M& B "' !& D& 0' ; N D 2 & - ,

9 ' $' & H# H>' , ' EM' ).+' 5HHH 5 ; E " !& & "##: ' & CI> CH5' M 5HH $ 0 ' (& 0' = & " 1 & ' ' , ' $## === " 0 & ; ' < ' . ! . ' " (

)' " ' S ' ! ' ; $## 7JJ,,, &

J T J J " 0 + * 1

!" # #$' !

& " ' " (

)' I# ) .' " ' SU ' !+E++ N+ $+?' $## > .& (& + /0 / 0 * &7 + * ,0 1 2333 ' & $HI> $HH$' . ; ' ! ' + $$ $I $### === C .& (& + /0 !

1 7 + 0 * 1 & 2333 ' & $H>I $HC>' . ; ' ! ' +

$$ $I $### === ? .& (& + /0 1 6 + 0+1' & 5?H 5II' D

' ' ).+' F C $### .& I E . F 0 1' 5HHH 5?

H . ! F ' >%7>C CI' B 5HIC ># .' " . ' ! = & M ! ( B &' ' ! ' & 5C? 5H .& P &' 5HH# >5 M .

' + / 0' " ' M " ! 0 F- 1 /

! ' & 5#CI 5#?>' 5HHH >$ . ' ; ' B /& ' ; 0

1 - ' 5#57HH 55' $##5 > E0 P ' / ,-' / D + * 1 & ===' ' ' 5' & ? 5$' " $##$ > ( B' ! B1 ' =, 0 D& 0 & & * 99= ' 5' & >H> C#5 ===' 5HH

+ , 0& + , . +5 . +$ ,0 0&' . + , 1

& 0, ' , & 2 - & + 3 4 $#

" '

&% "' '

* &% D * & $#' I' H & ' 6 ' 2 ' ' .' . ! ,0 & & & 2 H' ># + = D * & =D;%' , 6 $' 55

= D ; 5I

0 1 & C' >' ? D 1 ' 6 , & *' 1' & 0 5' #' $H , ' & 1 , , , , F &

& " ! .

5> ,% 0, 2 2 * 6 , , * & ( $$ 0& & & * &

2 & & 5$' 5' $C' >' 5' & &

1 1 , & 0 % >$' & & $> 6 , $ * & V, & 6 $ 0& & - 6 ?' >5

0& & &% & && & '

C' 5C , ,

, & 7 6 , ' & , ' * %' , &' C & / ; J0 ;' '8% ? !& D 5# '8

! D ; " # 2

0 2 ' & & - I & &

, - 1 ,

; & &

& ' & *& 5C * 0 1 6 C + 6

0 0, 5H' ' $5 + 6 , 0

& 1 1' 0 $ 1 & 5H

& .

& * & $' >' C' >' I' 5' & & *& 5' 5? / && , ,

& + ' " 0 & & $I * 0 0, = ' & & "' &

& & &% & 0

/ & ,0

% %'

; ( 6 ( D * $?' I' > ( = D ; =D;%

1& 6 H' ># ; ' ( , Æ 7 , & & * & , & & , , 6 & &

( $$% 9 9 : ' = 0 %

& =6 5>

9

! %

5>%

,' ,

* , 2 !

< ' + ! . !% 2 : + 6 % & = 9 6 5C ( )

*+

$#

9(

) %

5C%

, ( ) % 0, 9 0, ' 0, & ' & 7 9 5 %% ' * & & % 0 =6 5?% & & & &

% 9

%

%

5?%

E % =6 5>%' 0,

! 7 9 5 5%' , 9 5 B , , / & , =6 5I% & , , & ( ! ;

1& & =6 5H

7 9 5 % 9

7 9 5

5% 9

%

7 9 5 5% 7 9 5 5%

%

7 9 5

5I%

5%

5H%

+ ,

2 '

+ & D * 6 ' 0 & $5

' B , & >' ?' #' , ( ;' '

F & , , V

% 8 .

8 , ,0 2 0 & F 2 ' & , ,

,

&

& *

;& H7 " &

! " # $ % &

+ ,

2 ' E & . , , $$

2 0 , ,

B & @* @+

& , & '

, & ,

!

!"

!"

;& 5#7 ,

& 0 % / % + = 0, & && & , 3!4 & ,

;& H'5# % ,

&

0 , 3 ,4 ! C $ % "' ,

,

& 8 ' 0 0 & ;& 5#% 0 ,

0

& ( , $ , $

$

#$ & B 2 & ' '

% 2 Error in Rotation from Odometer (for three different speeds) 0.8

0.6

Error (in degrees)

0.4

0.2

0

−0.2

−0.4

−0.6 −50

−40

−30

−20

−10 0 10 Rotation angle (in degrees)

20

30

40

50

;& 557 = 2 & 2 34 5#' 3 4 >#' 3A4 H#' % ;' , 2 2 & ;& 55'5$ ;& 55% ;& 5$%< J & , & , , &

% 1 " ;& 55'5$

& % + ,

& & .& &' & 3A4 *&% 2 2 , ; 2 , H# ,

,

, , V , , V % , F &* &

; ;& 5 , & = & 2 ' , ; 6 &' Æ

$

Error in Rotation from Intended Angle (for three different speeds) 1.5

1

Error (in degrees)

0.5

0

−0.5

−1

−1.5

−2 −50

−40

−30

−20

−10 0 10 Rotation angle (in degrees)

20

30

40

50

;& 5$7 = 2 & 2 ;& 55%

0

; ,

& + 1 ( , 6 & & 0

! , ;& H% , , + 2 , 2 + , 0 +% , & 9 9 # # # % ;& 5>

5## , 5C> 2 V% * & & O R W

5

2

2 $#' C#' 5##% & 5## & &* , & , '

& , ,0 $>

Histogram of Error (E=1.4055 σ=0.2934) 6

8

5 Number of samples

Number of samples

Histogram of Error (E=0.0882 σ=0.0962) 10

6 4 2

4 3 2 1

0 −0.4

−0.2 0 0.2 Error in degrees (Carpet 1)

0 0.5

0.4

12

10

10

8 6 4 2 0

2

Histogram of Error (E=0.7484 σ=0.1477)

12 Number of samples

Number of samples

Histogram of Error (E=0.5296 σ=0.2143)

1 1.5 Error in degrees (Plastic 1)

8 6 4 2

0

0.5 1 Error in degrees (Carpet 2)

0

1.5

0

0.5 1 Error in degrees (Tile Floor)

1.5

;& 57 = 2 H# % Æ

" # , &

& + 5 &

& & 2 ;& 5C 5$# , & ' , O % & V ;& 5?

;' 1

! '

& & , , & & & ( * & & B & , ' ' 6 " ! % ( 0

& ' ,'

& , $C

Histogram of Error (E=−0.4557 σ=0.1334)


12

6

10

5

8

4

6

3

4

2

2

1

0 −0.6

−0.4 −0.2 0 Error in degrees (Carpet 1)

0 −2.5

0.2


−2 −1.5 Error in degrees (Plastic 1)

−1


10

7 6

8

5 6

4

4

3 2

2 0 −1

1 −0.8 −0.6 −0.4 Error in degrees (Carpet 2)

0 −1.5

−0.2

−1 Error in degrees (Tile Floor)

−0.5

Error distribution from intended angle (−90°)

;& 57 = 2 H# % Æ

& , &' 2 , V B & @* @+ & , & '

, & ; &

, ( , 1 * (

& , (

& , 2 & , & , V

& 3 4

! + , , $55 & , & 6 $# - ./ 012 3 ,

$

$?

Histogram of error in X

Histogram of error in Y

30

25

25

20

20 15 15 10 10 5

5 0 −2

−1 0 1 Error in cm (E=0.0924 σ=0.7906)

0 −2

2

Histogram of error in Θ

−1 0 1 Error in cm (E=0.3422 σ=0.6201)

2

Position of the Robot

30

2

25 1 20 Y

15

0

10 −1 5 0 −1

−2 85

−0.5 0 0.5 1 Error in degrees (E=0.0601 σ=0.3075)

90

95 X

100

105

;& 5>7 = 5## V' 5C>

@ % 9 A @ A C#

$#%

! " & Æ & , , , 0

7 *' & , . ' & & 6 /, * B 2 - & ' & & ' 1 6

,

, ;& 5I% , - '

& ' * ' 4 ( ; & && , 9 A A ' ,

$I

57 " . & O'R % W &% 5## 2

,

;& 5I% ; , & %

9 5 $' 9 9

6 $5

9 : : , : 9 9 A A

9 A 9 A % A % 9 % A % A $ % , % 9 # ) ' % 9 % 9

9 $ 9

$5%

$

"

' 6 , & ' , 7 9 % - 7 ;& 5I

,

=6 $$ ' & &

$H



7

5 Number of samples

Number of samples

6 5 4 3 2 1 0

0

1 2 3 Error in cm (E=2.4488 σ=0.9003)

4 3 2 1 0

4

0

0.5 1 1.5 Error in cm (E=0.9771 σ=0.4167)


Position of the Robot

5

2

4

1

3 0

Y

Number of samples

2

2 −1

1 0 0.5

1 Error in cm (E=1.0867 σ=0.1810)

−2 105

1.5

110

115 X

120

125 Plastic

;& 5C7 = 5$# '

A @ A % A % 9 9 A @ A % A %

A A

$

%$4 ( + 9 A ,

& B 1 ( , , , ( 6 $5% '

E 9 A 7

9

+

, A

9 # 1

9

$% #



10

8

Number of samples

10

Number of samples

12

8 6 4

6 4 2

2 0 −4

−3 −2 −1 0 Error in cm (E=−1.4318 σ=0.8155)

0 −1

1

−0.5 0 0.5 1 Error in cm (E=0.5329 σ=0.3851) Position of the Robot

8

2

6

1

4

0

Y

Number of samples


2

0 −0.5

1.5

−1

−2 105

0 0.5 1 1.5 Error in degrees (E=0.6308 σ=0.2508)

110

115 X

120

125

Carpet

;& 5?7 = 5$# ! '

9 : : , : 9 9

9

%

9 A A % A A %

A A

9

9 9

%

A A A % A

%% 9 # )

9

% %

% 9

9

9

$% Εθ 2 θ i+1

x i+1,y i+1 ∆ρ+Ερ Finishing Position Starting Position Εθ 1 x i,yi

θi

5 ;& 5I7 F

' & ' * -' % & & & 6 $>' , "# 5% , ( , 1 & 6

"# 5% 9 "# 5% "# 5%

@ @ $ @

$>%

$

Average values of the std along the X,Y,θ during translation 5.5

5 STD along the X axis

Standard Deviation

4.5

4 STD along the Y axis 3.5

3 STD of the orientation θ

2.5

0

5

10 15 Number of steps per translation

20

25

;& 5H7 #### & 5## & 2 5## )& , 2 & , & & 2 &

+ & & ;& 5H 5#### & O ' R & O ## ' 2 &

+ & & , ,& 1 % , 0

$

&& & ,

& , & ,& && , ,

1% ,& &&

" (

& , 6 ,& Æ ' * ,&

' E & F &%% # 5 ;

' , && & , & '

,&' 6

'

, < ' ,& & && ' && + & 3 , 4 & 7 BE 9 5 S 9 B%< . 9 9 EA5%< " #$ %& 9 %< ' & % EA5% 9 5< 95< -95< " $ % / . 9-< 9A5
&& * . * 34 & 3- 4 & ,

,

%# ( & ! & &

& ,& " & & &

& & , &

,& & >

, 2 &

1

C