Show PDF File - robotics - Universidad de Zaragoza

¯x(k + 1) = ((¯xr(k + 1)) T , ¯x1, ¯y1, . . . , ¯xm, ˆym) T = ((ˆxr(k) ⊕ x k k+1) T , ˆx1, ˆy1, . . . , ˆxm, ˆym) T ,
P(k + 1|k) = J1P(k)J T 1 + J2PodomJ T 2 ,
⊕ ˆxr(k) x k k+1
ˆxr(k) ⊕ x k ⎡
ˆxk + xodom cos
k+1 = ⎣
ˆ θk − yodom sin ˆ θk
ˆyk + xodom sin ˆ θk + yodom cos ˆ ⎤
θk
⎦ ,
ˆθk + θodom
J1 J2
J1 = blkDiag(j1, I), J2 = ((j2) T , 0) T ⎡
1 0 −xodom sin
j1 = ⎣
,
ˆ θk − yodom cos ˆ θk
0 1 xodom cos ˆ θk − yodom sin ˆ ⎤
θk
⎦ ,
⎡
cos
j2 = ⎣
0 0 1
ˆ θk − sin ˆ θk
sin
0
ˆ θk cos ˆ θk 0
0 0 1
⎤
⎦ .
¯xi(k + 1) = (¯xi, ¯yi) T
¯xr(k + 1) = (¯xk+1, ¯yk+1, ¯ θk+1) T
−(¯xi − ¯xk+1) sin
hi(¯xr(k + 1), ¯xi(k + 1)) = atan2
¯ θk+1 + (¯yi − ¯yk+1) cos ¯ θk+1
(¯xi − ¯xk+1) cos ¯ θk+1 + (¯yi − ¯yk+1) sin ¯
.
θk+1
∂hi
Hi =
0 · · · 0
∂¯xr(k+1)
∂hi
∂¯xr(k + 1) =
∂hi ∂hi
∂¯xk+1 ∂ ¯yk+1
∂hi/∂ ¯ θk+1 = −1
∂hi
∂¯xk+1
∂hi
∂¯xi
= −
=
¯yk+1 − ¯yi
∂hi
∂ ¯ θk+1
(¯xk+1 − ¯xi) 2 2 ,
+ (¯yk+1 − ¯yi)
¯yk+1 − ¯yi
(¯xk+1 − ¯xi) 2 2 ,
+ (¯yk+1 − ¯yi)
∂hi
∂¯xi(k+1)
,
∂hi
∂¯yk+1
∂hi
∂¯yi
0 · · · 0
,
∂hi
∂¯xi(k + 1) =
=
= −
∂hi
∂¯xi
∂hi
∂ ¯yi
¯xk+1 − xi
(¯xk+1 − ¯xi) 2 2 ,
+ (¯yk+1 − ¯yi)
¯xk+1 − ¯xi
h H
,
(¯xk+1 − ¯xi) 2 2 ,
+ (¯yk+1 − ¯yi)
h = (h T 1 , . . . , h T n) T , H = (H T 1 , . . . , H T n) T .