ekf and ukf
DESCRIPTION
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. - PowerPoint PPT PresentationTRANSCRIPT
EKF and UKFDay 24
1
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
2
EKF Prediction Step• recall that in the prediction step the
state mean and covariance are projected forward in time using the plant model
3
tTtttt
ttt
RGG
ug
1
1),(
4
EKF Prediction Step
positionand
covarianceat previoustime step
5
EKF Prediction Stepuncertainty
due tocontrol noise(small transl.
and rot. noise)
6
EKF Prediction Stepprevious
uncertaintyprojectedthroughprocessmodel
7
EKF Prediction Stepnet
predicteduncertainty
8
EKF Prediction Stepuncertainty
due tocontrol noise(large transl.
and smallrot. noise)
9
EKF Prediction Stepuncertainty
due tocontrol noise(small transl.
and largerot. noise)
10
EKF Prediction Stepuncertainty
due tocontrol noise(large transl.
and largerot. 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
11
tTtttt
tt
QHHS
hz
)(
12
EKF Observation Prediction Step
landmarkobservation
predictedstate withcovariance predicted
landmarkobservation
13
EKF Observation Prediction Step
landmarkobservation
predictedstate with
uncertainty observationuncertainty
14
EKF Observation Prediction Step
landmarkobservation
predictedstate withcovariance
uncertaintydue to
uncertaintyin predicted
robot position
15
EKF Observation Prediction Step
landmarkobservation
predictedstate withcovariance
innovation(differencebetween
predicted andactual observations)
16
EKF Observation Prediction Step
landmarkobservation
predictedstate with
uncertainty
observationuncertainty
(large distanceuncertainty)
17
EKF Observation Prediction Step
landmarkobservation
predictedstate with
uncertainty
observationuncertainty
(large bearinguncertainty)
EKF Correction Step• the correction step updates the state
estimate using the innovation vector and the measurement prediction uncertainty
18
tttt
ttttt
tTttt
HKIzzK
SHK
)()(
1
19
EKF Correction Stepinnovation(differencebetween
predicted andactual observations)
innovation scaledand mapped into
state space
20
EKF Correction Stepcorrected
state meancorrected
stateuncertainty
21
EKF Correction Step
22
Estimation Sequence (1)
motion modelestimated path
true path
landmarkmeasurement
event
initialposition
uncertainty
predictedposition
uncertainty
correctedposition
uncertainty
EKFestimated
path
23
Estimation Sequence (2)
same as previous but with greater measurement uncertainty
24
Comparison to GroundTruth
25
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!