|
Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASL
Autonomous 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
ASL
Autonomous 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
ASL
Autonomous 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
ASL
Autonomous 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
ASL
Autonomous 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
ASL
Autonomous Systems Lab
2020 | Exercise 5 6
Task 1 - Solution
|
Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASL
Autonomous 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
ASL
Autonomous Systems Lab
2020 | Exercise 5 8
Task 2 - Solution
|
Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASL
Autonomous Systems Lab
▪ Try it out in the Simulator!
2020 | Exercise 5 9
Task 3
|
Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASL
Autonomous 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
ASL
Autonomous Systems Lab
2020 | Exercise 5 11
Solutions
|
Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASL
Autonomous 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
ASL
Autonomous 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
ASL
Autonomous 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
ASL
Autonomous Systems Lab
2020 | Exercise 5 15
Task 1 - Solution
|
Autonomous Mobile Robots
Roland Siegwart, Margarita Chli, Nick Lawrance
ASL
Autonomous 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
ASL
Autonomous 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 fixed
if (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
ASL
Autonomous Systems Lab
2020 | Exercise 5 18
Task 2 - Solution
