|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

|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance

ASLAutonomous Systems Lab

2019 | Exercise 5 15

Real application