!

p(x x, u)
"

p(x x, u)
!
"

u
x

x
"!

p(x x, u)
#

"%

"

&
'
(

!
%

"
"
'
)&! !*

"

"

"

,
)

!*

'
#

'

/
%/

/
+

"#

$%

"

#
1$.
!
$.
1$.
6 6

234
/

5
/
5.
6 6

7
7
"

" %

#
"
"

,
99

/'

!
4

"

%<

"
%
/
=

/
"

"
%

"

"
"

?
>

/
-

x , y ,

"

x ' , y ' , '

u = rot1 , rot 2 , trans

"

trans = ( x ' x ) 2 + ( y ' y ) 2

rot1 = atan2( y ' y , x ' x )

rot 2 = ' rot1
x , y ,

rot1

rot 2
trans

x ' , y ' , '

& '(

(&

/
%

%
"&

@@

%/

rot1 = rot1 +

1 | rot 1 |+ 2

trans = trans +

rot 2 = rot 2 +

| trans |

| trans |+ 4 | rot 1 + rot 2 |

1 | rot 2 |+ 2

| trans |

(
3

( x) =
2

1
2

1 x2
2
2

0 if | x |> 6 2
2 ( x) =
6 2 | x |
6 2

! %

)*

=%

)a,b*,

%
@

=%

!(

)a,b*,

(
-.
@

! %
# #/
(

=%

)##/(+

trans = ( x ' x ) 2 + ( y ' y ) 2

rot1 = atan2( y ' y, x ' x )
rot 2 = ' rot1
trans = ( x' x) 2 + ( y ' y ) 2
/
"
rot1 = atan2( y' y, x' x)
+
rot 2 = ' rot1
0
p1 = prob( rot1 rot1 , 1 | rot1 | + 2trans )
:
p2 = prob( trans trans , 3trans + 4 (| rot1 | + | rot2 |))
>
@5 p3 = prob( rot 2 rot 2 , 1 | rot 2 | + 2trans )

) *

@@

)&!&A
*

p1 p2 p3
@$

0
"
"

/
"

'
'

"

p(x|u,x)

x
u

@:

1
!(
8
@

%"
=%

8
@

%"
=%

)b*,

%
,

!(

)b*,

@>

@5+
5

'

!(

@5

@5

@5$

@5+

3
8
@

$
+

!
%"

=%

)f,b*,

"#
8

%"

=%

) ! &*,

u = rot1 , rot 2 , trans , x = x, y,

rot1 = rot1 + sample(1 | rot1 | + 2 trans )
trans = trans + sample( 3 trans + 4 (| rot1 | + | rot 2 |))
= + sample( | | + )
rot 2

$
+
0

rot 2

x' = x + trans cos( + rot1 )

y ' = y + trans sin( + rot1 )
' = + +
rot1

rot 2

x ' , y ' , '

rot 2

trans

"#

"4(
C

%
"

>

"#

).

p ( x | u, x' )
=

&

p ( x | u , x ' , m)

, p ( x | u , x ' , m) = p ( x | m ) p ( x | u , x ' )

(
D

"
/

'

'

D
)& &A
! *
D

"
"&

!
D
7

"
&

)& &A
! *
/
/