exercise 5 | ekf simultaneous localization and mapping (slam) › content › dam › ethz ›...
TRANSCRIPT
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
Florian Tschopp and Andrei Cramariuc
2019 | Exercise 5 1
Exercise 5 | EKF Simultaneous Localization And Mapping (SLAM)
Spring 2019
|Autonomous Mobile RobotsRoland 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)
l Detect lines in lidar scan
l Associate lines with previous detectionsl Evaluate innovation term for measurements
§ New: No ground truth map of environment → map is in EKF state
2019 | Exercise 5 2
Goal …
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2019 | Exercise 5 3
EKF recap
PredictionState Propagation
Covariance Propagation
UpdateMeasurement
Update
Innovation
Optimal gain
|Autonomous Mobile RobotsRoland 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)
2019 | Exercise 5 4
Assumptions
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
Task 1
PredictionState Propagation
Covariance Propagation
UpdateMeasurement
Update
Innovation
Optimal gain
What is new since Exercise 4?
Suggestion:Practice derivation of Jacobians by hand!
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2019 | Exercise 5 6
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 RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2019 | Exercise 5 7
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 RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2019 | Exercise 5 8
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 RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2019 | Exercise 5 9
Task 1 - Solution
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2019 | Exercise 5 10
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 RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2019 | Exercise 5 11
Task 2 - Solution
§
§ h = [...m(1) - x(3)m(2) - (x(1)*cos(m(1)) + x(2)*sin(m(1)))];
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2019 | Exercise 5 12
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 RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
2019 | Exercise 5 13
Task 2 - Solution
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASLAutonomous Systems Lab
§ Try it out in V-REP!
2019 | Exercise 5 14
Task 3