Unscented Transformation of Vehicle States in SLAM

Transcription

1 Unscented Transformation of Vehicle States in SLAM Juan Andrade-Cetto Teresa Vidal-Calleja Alberto Sanfeliu Institut de Robòtica I Informàtica Industrial, UPC-CSIC, SPAIN ICRA 2005, Barcelona 12/06/2005 1

2 Contents The EKF: Linear Propagation of Means and Covariances. The UKF: A Deterministic Particle Filter. UT of Vehicle States. Comparison. Conclusions. 2

3 The EKF: Linear Propagation of Means and Covariances x( k k) P( k k) 3

4 The EKF: Linear Propagation of Means and Covariances x( k k) P( k k) x ( k + 1 k) = f ( x( k k), u( k),0) T P( k + 1 k) = F P( k k) F + G T VG 4

5 The EKF: Linear Propagation of Means and Covariances 5

6 The EKF: Linear Propagation of Means and Covariances sample mean sample covariance 6

7 The EKF: Linear Propagation of Means and Covariances EKF mean EKF covariance T T x ( k + 1 k) = f ( x( k k), u( k),0) P( k + 1 k) = F P( k k) F + G VG sample mean sample covariance 7

8 EKF Prior Innovation Posterior 8

9 The UKF: A Deterministic Particle Filter Can we do better? One choice is: The Unscented Transformation (Julier and Uhlmann) that approximates a nonlinear mean and covariance to the 2nd order. By deterministically choosing a set of geometrically distributed particles (sigma points). 9

10 The UKF: A Deterministic Particle Filter A set of are deterministically chosen to satisfy a condition set that determines the information that should be captured about x. mean covariance skew 10

11 The UKF: A Deterministic Particle Filter Algorithm to select sigma points O(n 3 ) 11

12 The UKF: A Deterministic Particle Filter Prior Innovation Posterior 12

13 The UKF: A Deterministic Particle Filter sigma points 13

14 The UKF: A Deterministic Particle Filter EKF UKF sample covariance 14

15 UT of Vehicle States Two caveats of using UKF in SLAM: Using the UT for the linear part of the model (the map) underestimates the vehicle priors when the state space grows. The Cholesky decomposition needed to compute the square root of P has time complexity O(n 3 ) with respect to the dimension of the sigma point set (twice the number of states). 15

16 UT of Vehicle States Adding one landmark to the state 16

17 UT of Vehicle States 17

18 UT of Vehicle States The state vector has one more dimension: x=[x,y,f 1 ] T f 1 18

19 UT of Vehicle States and the sample covariance to approximate is now a hyperellipsoid 19

20 UT of Vehicle States the increase in dimensions requires more sigma points 20

21 UT of Vehicle States the increase in dimensions requires more sigma points new scaled out 21

22 UT of Vehicle States and the UKF computed estimates reflect the change UKF 22

23 UT of Vehicle States EKF sample covariance UKF vehicle only UKF with one landmark 23

24 UT of Vehicle States adding a second 1-d landmark to the state vector EKF UKF with two landmarks UKF vehicle only UKF with one landmark sample covariance 24

25 UT of Vehicle States the full UKF underestimates the vehicle prior in SLAM! 25

26 UT of Vehicle States EKF Covariance UT Covariance only for vehicle New Covariance Must be psd 26

27 Experiments 27 Covariances Errors

28 Final Paths Path Estimated Path GPS Path Est. Beac. GPS Beacons Path Estimated Path GPS Path Est. Beac. GPS Beacons North Meters 0 5 North Meters East Meters Fully observable EKF SLAM Path East Meters Estimated Path GPS Path Est. Beac. GPS Beacons Fully observable UKF SLAM 5 North Meters East Meters Fully observable using UT only for vehicle 28

29 Experiments Car Park (ACFR Data Base) UKF SLAM UKF SLAM only for vehicle 29

30 Conclusions UT allows better nonlinear mean and covariance estimations than EKF. In SLAM, as the state space increases, the UKF produces more σ points, giving conservative estimates of the actual vehicle covariance. Substituting the prior computation of the vehicle covariance in the EKF, with the one computed using the UT, produces tighter covariance estimates. Consequently, applying UT to vehicle states only, gives better data association than the full UKF implementation. 30