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!