|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
Florian Tschopp and Patrik Schmuck
2020 | Exercise 5 1
Exercise 5 | EKF Simultaneous Localization And Mapping
(SLAM)
Spring 2020
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
▪ Putting everything from exercise 3 and 4 together
− Forward integrate state with wheel odometry (EKF Prediction)
− Update state with measurements (EKF measurement update)
⚫ Detect lines in lidar scan
⚫ Associate lines with previous detections
⚫ Evaluate innovation term for measurements
▪ New: No ground truth map of environment → map is in EKF state
2020 | Exercise 5 2
Goal …
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 3
EKF recap
Prediction
State Propagation
Covariance Propagation
Update
Measurement
Update
Innovation
Optimal gain
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
▪ Landmarks are static:
▪ Known number of landmarks
− Highly simplifies implementation as no bookkeeping necessary
−
▪ First two landmarks/lines remain fixed (global coordinate system)
2020 | Exercise 5 4
Assumptions
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
Task 1
Prediction
State Propagation
Covariance Propagation
Update
Measurement
Update
Innovation
Optimal gain
What is new since Exercise 4?
Suggestion:
Practice derivation of Jacobians by
hand!
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 6
Task 1 - Solution
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 7
Task 2
Prediction
State Propagation
Covariance Propagation
Update
Measurement
Update
Innovation
Optimal gain
- How to reuse measurement function of Ex4?
- Derive Jacobian by hand!
- First two landmarks are fixed → don’t update
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 8
Task 2 - Solution
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
▪ Try it out in the Simulator!
2020 | Exercise 5 9
Task 3
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 10
Real application
https://www.youtube.com/watch?v=ZMAISVy-6ao
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 11
Solutions
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 12
Task 1 - Solution
▪
▪ f = x;f(1) = f(1) + (u(1)+(u(2)))/2 * cos(x(3) + (u(2)-u(1))/(2*b) );f(2) = f(2) + (u(1)+(u(2)))/2 * sin(x(3) + (u(2)-u(1))/(2*b) );f(3) = f(3) + (u(2)-u(1))/(b);
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 13
Task 1 - Solution
▪
▪ F_x(1:3,1:3) = [...1, 0, -sin(x(3) - (u(1) - u(2))/(2*b))*(u(1)/2 + u(2)/2)0, 1, cos(x(3) - (u(1) - u(2))/(2*b))*(u(1)/2 + u(2)/2)0, 0, 1];
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 14
Task 1 - Solution
▪
▪ F_u(1:3,1:2) = [...cos(x(3) - (u(1) - u(2))/(2*b))/2 + (sin(x(3) - (u(1) - u(2))/(2*b))*(u(1)/2 + u(2)/2))/(2*b), cos(x(3) - (u(1) - u(2))/(2*b))/2 - (sin(x(3) - (u(1) - u(2))/(2*b))*(u(1)/2 +
u(2)/2))/(2*b)sin(x(3) - (u(1) - u(2))/(2*b))/2 - (cos(x(3) - (u(1) - u(2))/(2*b))*(u(1)/2 + u(2)/2))/(2*b), sin(x(3) - (u(1) - u(2))/(2*b))/2 + (cos(x(3) - (u(1) - u(2))/(2*b))*(u(1)/2 +
u(2)/2))/(2*b)-1/b, 1/b];
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 15
Task 1 - Solution
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 16
Task 2 - Solution
▪
▪ h = [...m(1) - x(3)m(2) - (x(1)*cos(m(1)) + x(2)*sin(m(1)))];
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 17
Task 2 - Solution
▪
▪ H_x = zeros(2,length(x));H_x(1:2,1:3) = [...
0, 0, -1-cos(m(1)), -sin(m(1)), 0];
%Do not correct first two landmarks as they remain fixedif (idxLandmark>2)
H_x(1,3 + (idxLandmark-1)*2+1) = 1;H_x(2,3 + (idxLandmark-1)*2+1) = x(1)*sin(m(1)) - x(2)*cos(m(1));
H_x(2,3 + (idxLandmark)*2) = 1;end
|Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2020 | Exercise 5 18
Task 2 - Solution