280 likes | 511 Views
Day 24. EKF and UKF. EKF and RoboCup Soccer. simulation of localization using EKF and 6 landmarks (with known correspondences) robot travels in a circular arc of length 90cm and rotation 45deg. EKF Prediction Step.
E N D
Day 24 EKF and UKF
EKF and RoboCup Soccer • simulation of localization using EKF and 6 landmarks (with known correspondences) • robot travels in a circular arc of length 90cm and rotation 45deg
EKF Prediction Step • recall that in the prediction step the state mean and covariance are projected forward in time using the plant model
EKF Prediction Step position and covariance at previous time step
EKF Prediction Step uncertainty due to control noise (small transl. and rot. noise)
EKF Prediction Step previous uncertainty projected through process model
EKF Prediction Step net predicted uncertainty
EKF Prediction Step uncertainty due to control noise (large transl. and small rot. noise)
EKF Prediction Step uncertainty due to control noise (small transl. and large rot. noise)
EKF Prediction Step uncertainty due to control noise (large transl. and large rot. noise)
EKF Observation Prediction Step • in the first part of the correction step, the measurement model is used to predict the measurement and its covariance using the predicted state and its covariance
EKF Observation Prediction Step predicted state with covariance predicted landmark observation landmark observation
EKF Observation Prediction Step predicted state with uncertainty observation uncertainty landmark observation
EKF Observation Prediction Step uncertainty due to uncertainty in predicted robotposition predicted state with covariance landmark observation
EKF Observation Prediction Step innovation (difference between predicted and actual observations) predicted state with covariance landmark observation
EKF Observation Prediction Step predicted state with uncertainty observation uncertainty (large distance uncertainty) landmark observation
EKF Observation Prediction Step predicted state with uncertainty observation uncertainty (large bearing uncertainty) landmark observation
EKF Correction Step • the correction step updates the state estimate using the innovation vector and the measurement prediction uncertainty
EKF Correction Step innovation (difference between predicted and actual observations) innovation scaled and mapped into state space
EKF Correction Step corrected state mean corrected state uncertainty
Estimation Sequence (1) EKF estimated path initial position uncertainty true path landmark measurement event predicted position uncertainty corrected position uncertainty motion model estimated path
Estimation Sequence (2) same as previous but with greater measurement uncertainty
EKF Summary • Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2) • Not optimal! • Can diverge if nonlinearities are large! • Works surprisingly well even when all assumptions are violated!