Click here to load reader

Exercise 5 | EKF Simultaneous Localization And Mapping (SLAM) · PDF file Autonomous Mobile Robots Roland Siegwart, Margarita Chli, Nick Lawrance ASL Autonomous Systems Lab 2020 |

  • View
    2

  • Download
    0

Embed Size (px)

Text of Exercise 5 | EKF Simultaneous Localization And Mapping (SLAM) · PDF file Autonomous Mobile...

  • | 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