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
Contents The EKF: Linear Propagation of Means and Covariances. The UKF: A Deterministic Particle Filter. UT of Vehicle States. Comparison. Conclusions. 2
The EKF: Linear Propagation of Means and Covariances x( k k) P( k k) 3
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
The EKF: Linear Propagation of Means and Covariances 5
The EKF: Linear Propagation of Means and Covariances sample mean sample covariance 6
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
EKF Prior Innovation Posterior 8
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
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
The UKF: A Deterministic Particle Filter Algorithm to select sigma points O(n 3 ) 11
The UKF: A Deterministic Particle Filter Prior Innovation Posterior 12
The UKF: A Deterministic Particle Filter sigma points 13
The UKF: A Deterministic Particle Filter EKF UKF sample covariance 14
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
UT of Vehicle States Adding one landmark to the state 16
UT of Vehicle States 17
UT of Vehicle States The state vector has one more dimension: x=[x,y,f 1 ] T f 1 18
UT of Vehicle States and the sample covariance to approximate is now a hyperellipsoid 19
UT of Vehicle States the increase in dimensions requires more sigma points 20
UT of Vehicle States the increase in dimensions requires more sigma points new scaled out 21
UT of Vehicle States and the UKF computed estimates reflect the change UKF 22
UT of Vehicle States EKF sample covariance UKF vehicle only UKF with one landmark 23
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
UT of Vehicle States the full UKF underestimates the vehicle prior in SLAM! 25
UT of Vehicle States EKF Covariance UT Covariance only for vehicle New Covariance Must be psd 26
Experiments 27 Covariances Errors
Final Paths 20 15 Path Estimated Path GPS Path Est. Beac. GPS Beacons 20 15 Path Estimated Path GPS Path Est. Beac. GPS Beacons 10 10 5 5 North Meters 0 5 North Meters 0 5 10 10 15 15 20 20 25 10 5 0 5 10 15 20 East Meters Fully observable EKF SLAM 20 15 10 Path 25 10 5 0 5 10 15 20 East Meters Estimated Path GPS Path Est. Beac. GPS Beacons Fully observable UKF SLAM 5 North Meters 0 5 10 15 20 25 10 5 0 5 10 15 20 East Meters Fully observable using UT only for vehicle 28
Experiments Car Park (ACFR Data Base) UKF SLAM UKF SLAM only for vehicle 29
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