ekf for localization
DESCRIPTION
Day 26. EKF for localization. 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
![Page 1: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/1.jpg)
EKF for localizationDay 26
1
![Page 2: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/2.jpg)
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
![Page 3: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/3.jpg)
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),(
![Page 4: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/4.jpg)
4
EKF Prediction Step
positionand
covarianceat previoustime step
![Page 5: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/5.jpg)
5
EKF Prediction Step
uncertaintydue to
control noise(small transl.
and rot. noise)
![Page 6: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/6.jpg)
6
EKF Prediction Step
previousuncertaintyprojectedthroughprocessmodel
![Page 7: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/7.jpg)
7
EKF Prediction Step
netpredicted
uncertainty
![Page 8: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/8.jpg)
8
EKF Prediction Step
uncertaintydue to
control noise(large transl.
and smallrot. noise)
![Page 9: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/9.jpg)
9
EKF Prediction Step
uncertaintydue to
control noise(small transl.
and largerot. noise)
![Page 10: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/10.jpg)
10
EKF Prediction Step
uncertaintydue to
control noise(large transl.
and largerot. noise)
![Page 11: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/11.jpg)
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
)(
![Page 12: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/12.jpg)
12
EKF Observation Prediction Step
landmarkobservation
predictedstate withcovariance predicted
landmarkobservation
![Page 13: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/13.jpg)
13
EKF Observation Prediction Step
landmarkobservation
predictedstate with
uncertainty observationuncertainty
![Page 14: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/14.jpg)
14
EKF Observation Prediction Step
landmarkobservation
predictedstate withcovariance
uncertaintydue to
uncertaintyin predicted
robot position
![Page 15: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/15.jpg)
15
EKF Observation Prediction Step
landmarkobservation
predictedstate withcovariance
innovation(differencebetween
predicted andactual observations)
![Page 16: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/16.jpg)
16
EKF Observation Prediction Step
landmarkobservation
predictedstate with
uncertainty
observationuncertainty
(large distanceuncertainty)
![Page 17: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/17.jpg)
17
EKF Observation Prediction Step
landmarkobservation
predictedstate with
uncertainty
observationuncertainty
(large bearinguncertainty)
![Page 18: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/18.jpg)
EKF Correction Step
• the correction step updates the state estimate using the innovation vector and the measurement prediction uncertainty
18
tttt
ttttt
tTttt
HKI
zzK
SHK
)(
)(
1
![Page 19: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/19.jpg)
19
EKF Correction Stepinnovation(differencebetween
predicted andactual observations)
innovation scaledand mapped into
state space
![Page 20: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/20.jpg)
20
EKF Correction Step
correctedstate mean
correctedstate
uncertainty
![Page 21: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/21.jpg)
21
EKF Correction Step
![Page 22: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/22.jpg)
22
Estimation Sequence (1)
motion modelestimated path
true path
landmarkmeasurement
event
initialposition
uncertainty
predictedposition
uncertainty
correctedposition
uncertainty
EKFestimated
path
![Page 23: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/23.jpg)
23
Estimation Sequence (2)
same as previous but with greater measurement uncertainty
![Page 24: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/24.jpg)
24
Comparison to GroundTruth
![Page 25: EKF for localization](https://reader035.vdocuments.net/reader035/viewer/2022081507/56815aa5550346895dc835d3/html5/thumbnails/25.jpg)
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!