Page 45 - Read Online

Page 138 Ortiz et al. Intell Robot 2021;1(2):131-50 I http://dx.doi.org/10.20517/ir.2021.09

( − K ) is invertible, and we have

−1 − −1 −1
P +2 ≤ ( − K ) P +2 ( − K ) (32)

According to EKF,
−

−1
P +1 = P +2 + 1 = (P +2 + 1 )

Thus,
−1 − −1 − −1 −1
P +1 = (P +2 + 1 )

By the following matrix inversion lemma,
−1 −1
(Γ −1 + Ω) −1 = Γ − Γ(Γ + Ω ) Γ
where Γ and Ω are two non-singular. matrices,

−1
P −1 = − [P −1 − P −1 (P −1 + −1 ) P −1 ] −1
+1 +2 +2 +2 +2
Using Equation (32) and defining = ( − K ),

P −1 ≤ − − [P −1
+2
−1 −1 +1 −1 −1 − −1 −1 −1 (33)
−1
− P +2 (P + ) P ]
+2 1 +2
Now,
P −1 = P −1 ( − K ) −1 = P −1 −1
+2 +2 +2

Hence,

−1 −
P −1 ≤ ( − ( + P −1 −1 ) )P −1
+1 +2 +2
Combining the last term of Equation (29) with the first term on the right side of Equation (25),

( ) ( − K ) P −1 ( − K ) ( )
+1
≤ ( ) (1 − (1 + ¯ ¯ / ) (1 + ¯ + ) )P −1 ( ) (34)
¯
−1
2
−1

2 +2
≤ (1 − ) P −1
k ( )k

+2
p p p
¯

where k k = ( ) ≤ ¯ , k k = ( ) ≤ ¯, kK k = (K K ) ≤ , = Λ , ≤ P +2 ≤

−1
¯ , ≤ 1 , and
1
= < 1
¯

2

(1 + ¯ ¯ / )(1 + ¯ + )
Combining Equation (26), the first term of Equation (29), and Equation (34),

+1 = (1 − ) P −1
k ( )k 2

+2
+ max [Λ] ¯ + max P −1 ( ¯ + ¯ )
+1 (35)
≤ (1 − ) ( ) P −1 ( )
+2
+ max [Λ] ¯ + max P −1 ( ¯ + ¯ )
+1