1. Autonomous Navigation for Flying Robots Lecture 6.3: EKF Example
Jürgen Sturm
Technische Universität München

2. Autonomous Navigation for Flying Robots
Example in 2D
State
Jürgen Sturm
Autonomous Navigation for Flying Robots

3. Autonomous Navigation for Flying Robots
Example in 2D
State
Odometry
Jürgen Sturm
Autonomous Navigation for Flying Robots

4. Autonomous Navigation for Flying Robots
Example in 2D
State
Odometry
Observations of visual marker
in global coordinate frame
in local
frame!
See lecture 2.3 for more details on
coordinate transforms
Jürgen Sturm
Autonomous Navigation for Flying Robots

5. Autonomous Navigation for Flying Robots
Motion Model
Motion function
Derivative of motion function
Jürgen Sturm
Autonomous Navigation for Flying Robots

6. Autonomous Navigation for Flying Robots
Sensor Model
Let’s construct the sensor model
The marker is located at (given in global/world coordinates)
We need to compute where is the pose of the marker relative to the robot!
Jürgen Sturm
Autonomous Navigation for Flying Robots

7. Autonomous Navigation for Flying Robots
Sensor Model
Transformation matrix corresponding to (global) robot pose
Relation between global and local coordinates
Jürgen Sturm
Autonomous Navigation for Flying Robots

8. Autonomous Navigation for Flying Robots
Sensor Model
Finally, we get
Jürgen Sturm
Autonomous Navigation for Flying Robots

9. Autonomous Navigation for Flying Robots
Sensor Model
Now derive the observation function with respect to all components of its argument
That’s it!
Jürgen Sturm
Autonomous Navigation for Flying Robots

10. Extended Kalman Filter (EKF)
For each time step, do
Apply motion model (prediction step) with
Apply sensor model (correction step)
with and
Jürgen Sturm
Autonomous Navigation for Flying Robots

11. Autonomous Navigation for Flying Robots
2D EKF Example
Dead reckoning (no observations)
Large process noise Q in x+y
Jürgen Sturm
Autonomous Navigation for Flying Robots

12. Autonomous Navigation for Flying Robots
2D EKF Example
Dead reckoning (no observations)
Large process noise Q in x+y+yaw
Jürgen Sturm
Autonomous Navigation for Flying Robots

13. Autonomous Navigation for Flying Robots
2D EKF Example
Now with observations (limited visibility)
Assume robot knows correct starting pose
Jürgen Sturm
Autonomous Navigation for Flying Robots

14. Autonomous Navigation for Flying Robots
2D EKF Example
What if the initial pose (x+y) is wrong?
Jürgen Sturm
Autonomous Navigation for Flying Robots

15. Autonomous Navigation for Flying Robots
2D EKF Example
What if the initial pose (x+y+yaw) is wrong?
Jürgen Sturm
Autonomous Navigation for Flying Robots

16. Autonomous Navigation for Flying Robots
2D EKF Example
If we are aware of a bad initial guess, we set the initial sigma to a large value (large uncertainty)
Jürgen Sturm
Autonomous Navigation for Flying Robots

17. Autonomous Navigation for Flying Robots
2D EKF Example
Jürgen Sturm
Autonomous Navigation for Flying Robots

18. Autonomous Navigation for Flying Robots
Lessons Learned
2D example of an EKF
Derivation of motion model
Derivation of sensor model
Several example runs
Jürgen Sturm
Autonomous Navigation for Flying Robots