Show PDF File - robotics - Universidad de Zaragoza

k = 1 t = 0
I k i (t) i k i (t)
I k i (t) =
i k i (t) =
n
[Φ(k, t)]i,j I k j +
j=1
n
[Φ(k, t)]i,j i k j +
j=1
n
k−1
[Ω(k, t, p)]i,j I
p=1 j=1
p
j ,
k−1
n
p=1 j=1
Φ(k, t), Ω(k, t, p), Ψ(t1, t2) ∈ R2n×2n
t
hγI
Φ(k, t) = Ψ (τ + (k − 1)l, t − 1 + (k − 1)l)
0
τ=1
l
hγI
Ω(k, t, p) = Ψ (τ + (p − 1)l, t − 1 + (k − 1)l)
0
τ=1
Ψ(t1, t2) = A(t2) . . . A(t1 + 1)A(t1),
[Ω(k, t, p)]i,j i p
j ,
A(t + kl) = Ak (t) t k
ˆx k j j x
ˆx k j = H k i x + v k j , with E v k j = 0,
i k j = (H k j ) T (Σ k j ) −1 ˆx k j
i k j = (H k j ) T (Σ k j ) −1 v k j + I k j x.
ik i (t)
i k n
i (t) =
j=1
n
[Φ(k, t)]i,j(H k j ) T (Σ k j ) −1 v k j +
k−1
[Ω(k, t, p)]i,j(H
p=1 j=1
k−p
j
n
[Φ(k, t)]i,j I k j +
j=1
) T (Σ k−p
n
j
) −1 v k−p
j
+
k−1
[Ω(k, t, p)]i,j I
p=1 j=1
p
j
Ik i (t)x Ik i (t) ˆx k i (t) = (Ik i (t)) −1ik i (t)
ˆx k i (t) = x + (I k i (t)) −1
n
+
(I k i (t)) −1
k−1
n
j=1
[Ω(k, t, p)]i,j(H
p=1 j=1
k−p
j
[Φ(k, t)]i,j(H k j ) T (Σ k j ) −1 v k j
) T (Σ k−p
j
x,
) −1 v k−p
j
v k j k ∈ {1, . . . , K} j ∈ V
ˆx k i (t) x
,
,
.