1. Lecture 13: Mapping Landmarks CS 344R: Robotics Benjamin Kuipers

2. Landmark Map Locations and uncertainties of n landmarks, with respect to a specific frame of reference. –World frame: fixed origin point –Robot frame: origin at the robot Problem: how to combine new information with old to update the map.

3. A Spatial Relationship is a Vector A spatial relationship holds between two poses: the position and orientation of one, in the frame of reference of the other. A B

4. Uncertain Spatial Relationships An uncertain spatial relationship is described by a probability distribution of vectors, with a mean and a covariance matrix.

5. A Map with n Landmarks Concatenate n vectors into one big state vector And one big 3n  3n covariance matrix.

6. Example The robot senses object #1. The robot moves. The robot senses a different object #2. Now the robot senses object #1 again. After each step, what does the robot know (in its landmark map) about each object, including itself?

7. Robot Senses Object #1 and Moves

8. Robot Senses Object #2

9. Robot Senses Object #1 Again

10. Updated Estimates After Constraint

11. Compounding

13. Rotation Matrix B G

14. Compounding Let x AB be the pose of object B in the frame of reference of A. (Sometimes written B A.) Given x AB and x BC, calculate x AC. Compute C(x AC ) from C(x AB ), C(x BC ), and C(x AB, x BC ).

15. Computing Covariance Apply this to nonlinear functions by using Taylor Series. Consider the linear mapping y = Mx+b

16. Inverse Relationship

17. The Inverse Relationship Let x AB be the pose of object B in the frame of reference of A. Given x AB, calculate x BA. Compute C(x BA ) from C(x AB )

18. Composite Relationships Compounding combines relationships head- to-tail: x AC = x AB  x BC Tail-to-tail combinations come from observing two things from the same point: x BC = (  x AB )  x AC Head-to-head combinations come from two observations of the same thing: x AC = x AB  (  x CB ) They provide new relationships between their endpoints.

19. Merging Information An uncertain observation of a pose is combined with previous knowledge using the extended Kalman filter. –Previous knowledge: –New observation: z k, R Update: x = x(new)  x(old) Can integrate dynamics as well.

20. EKF Update Equations Predictor step: Kalman gain: Corrector step:

21. Reversing and Compounding OBJ1 R = (  ROBOT W )  OBJ1 W = WORLD R  OBJ1 W

22. Sensing Object #2 OBJ2 W = ROBOT W  OBJ2 R

23. Observing Object #1 Again OBJ1 W = ROBOT W  OBJ1 R

24. Combining Observations (1) OJB1 W = OJB1 W (new)  OBJ1 W (old) OJB1 R = OJB1 R (new)  OBJ1 R (old)

25. Combining Observations (2) ROBOT W (new) = OJB1 W  (  OBJ1 R ) ROBOT W = ROBOT W (new)  ROBOT W (old)

26. Useful for Feature-Based Maps We’ll see this again when we study FastSLAM.