LX: Localization - Extended Kalman Filter (EKF)#

What you will need

What you will get

  • Running the Localization - Extended Kalman Filter learning experience.

This learning experience is about how we should use the data streaming through the sensors, together with the knowledge of our surroundings, to estimate our state. The so-called optimal approach to this is the Bayes filter, however, this approach is computationally intractable in all but the simplest settings. We will explore several approximations to the Bayes filter. Namely, the Kalman filter, the particle filter, and the histogram filter. Each has its own assumptions and conditions under which it is most applicable. Finally, you will program an extended Kalman filter, or EKF, to localize your Duckiebot using the data from the wheel encoders and the AprilTag fiducial markers that you observe at known locations.

Duckietown histogram filter representation for Duckiebot pose estimation

Fig. 161 Welcome to the EKF - Localization LX!#

Intended Learning Outcomes

After this learning experience, you will:

  • learn about Kalman filters: from the theory to implementation details

  • learn about Particle filters: from the theory to implementation details

  • learn about Histogram filters: from the theory to implementation details

  • be able to evaluate advantages and disadvantages of each approach given different scenarios

  • be able to design, tune and test an extended Kalman Filter for achieving map-based localization, fusing wheel encoder and AprilTag-based pose measurement.

Warning

If using a Duckietown Workspace instead of a native Ubuntu setup, some steps vary slightly. Read this before proceeding: DT Workspaces - Duckiematrix for LXs

About these learning activities#

For guided setup instructions, lecture content, and more related to this LX, see our Self-Driving Cars with Duckietown MOOC on EdX.

Note

This exercise can be run on a virtual Duckiebot in the Duckiematrix, and on a real Duckiebot with off-board agent workflow. On-board agent workflow is work in progress.

Forking the repository#

1. Create a fork#

Navigate to the lx-ekf-localization repository.

Find and press the “Fork” button on the top right:

how to fork a Duckietown LX repository

Fig. 162 Fork the LX to be able to make local changes while still being able to receive updates.#

This will create a new repository at: <your_github_username>/lx-ekf-localization.

2. Clone the fork#

Clone the fork on your computer, replacing your GitHub username in the command below, and navigate to the new folder:

git clone [email protected]:<your_github_username>/lx-ekf-localization
cd lx-ekf-localization

3. Configure the upstream repository#

Configure the Duckietown version of this repository as the upstream repository to synchronize with your fork.

List the current remote repository for your fork,

git remote -v

Specify a new remote upstream repository,

git remote add upstream https://github.com/duckietown/lx-ekf-localization

Confirm that the new upstream repository was added to the list,

git remote -v

You can now push your work to your own repository using the standard GitHub workflow, and the beginning of every exercise will prompt you to pull from the upstream repository, updating your exercises to the latest version (if available).

Keeping your System Up To Date#

  • 💻 These instructions are for ente learning experiences. Ensure your Duckietown Shell is set to an ente profile (and not a daffy one). You can check your current profile with:

    dts profile list

    To switch to an ente profile, follow the Duckietown Manual DTS installation instructions.

  • 💻 Pull from the upstream remote to synch your fork with the upstream repo:

    git pull upstream ente

  • 💻 Make sure your Duckietown Shell is updated to the latest version:

    pipx upgrade duckietown-shell

  • 💻 Update the shell commands:

    dts update

  • 💻 Update your laptop/desktop:

    dts desktop update

  • 🚙 Update your Duckiebot (even if it is a virtual one):

    dts duckiebot update ROBOTNAME

    (where ROBOTNAME is the name of your Duckiebot - real or virtual.)

Launching the Code Editor#

Important

All dts code commands should be executed inside the root directory of the learning experience.

Making sure you are inside the path of this learning experience (cd ./path-to-lxs-in-your-workstation/lx-ekf-localization), then open the code editor:

dts code editor

Wait for a URL to appear on the terminal, then click on it or copy-paste it in the address bar of your browser to access the code editor.

The first thing you will see in the code editor are a version of these instructions. At this point you can start following the LX-specific indications shown in your code editor.

Walkthrough of Notebooks#

Inside the code editor, use the navigator sidebar on the left-hand side to navigate to the notebooks directory and open the first notebook.

Follow the instructions on the notebook and work through them in sequence.

In many cases the last notebook will instruct you to write some code inside the learning experience directory.

Once you have done that you will need to build your code before testing it.

Testing with the Duckiematrix#

To test your code in the Duckiematrix you will need a virtual robot attached to an ongoing session.

1. Creating and starting virtual Duckiebot#

If you have not done so already (e.g., for a different LX), you can create a virtual Duckiebot with the command:

dts duckiebot virtual create --type duckiebot --configuration DB21J [VBOT]

where [VBOT] is the hostname. It can be anything you like, subject to the same naming constraints of physical Duckiebots.

Then you can start your virtual robot with the command:

dts duckiebot virtual start [VBOT]

You should see it with a status Booting and finally Ready if you look at dts fleet discover:

     | Hardware |   Type    | Model |  Status  | Hostname 
---  | -------- | --------- | ----- | -------- | ---------
[VBOT] |  virtual | duckiebot | DB21J |  Ready   | [VBOT].local

Once you are done for the day, do not forget to stop your virtual robot:

dts duckiebot virtual stop [VBOT]

If in doubt if any of your virtual Duckiebots in running or not, you can check the status of your virtual scuderia at any time with:

dts duckiebot virtual list

2. Starting the Duckiematrix with the virtual Duckiebot#

Now that your virtual robot is ready, you can start the Duckiematrix. From this LX directory:

dts code start_matrix [--no-renderer]

Where the --no-renderer argument is used only if running this LX in a Duckietown Workspace.

You will see the Unity-based Duckiematrix simulator start up. The startup screen will look like:

Duckiematrix splash screen for the EKF Localization learning experience.

Fig. 163 In this LX you will be greeted by a slightly more complex Duckietown than in previous ones.#

From here you can click anywhere on the window and click ENTER to make it become active, and then move the duckie towards the Duckiebot with the w, a, s, and d keys, and you can move the camera angle to view the Duckiebot with the mouse.

If you are close enough to your Duckiebot, you can board it with the E key, which should look like

Duckiematrix splash screen for the EKF Localization learning experience.

Fig. 164 After boarding your Duckiebot you will be able to move it around manually with WASD.#

You can then you can drive the Duckiebot around with the ‘w’, ‘a’, ‘s’, and ‘d’ keys. You will notice that this map includes traffic signs with fiducial markers (AprilTags) that we are going to use in this LX to help localize your robot.

If you get very lost from the road and you want to come back, you can do so with the R key.

Note

You should reset your Duckiebot’s position with R before each test since the initial state estimate coincides with the reset position.

To run the WebGL (browser) version of the Duckiematrix, add the --browser flag.

Note

For the WebGL (browser) version of the Duckiematrix, if the colors look desaturated, try a different browser.

Building the Code#

From inside the learning experience root directory, you can build your code with:

dts code build -R ROBOT_NAME

where ROBOT_NAME can be either a physical or virtual robot.

Testing on a Duckiebot or in the Duckiematrix#

🚙 To test your code on your real Duckiebot you can do:

dts code workbench -R ROBOT_NAME

💻 To test your code on a virtual robot in the Duckiematrix:

dts code workbench -m -R VIRTUAL_ROBOT_NAME

(note the -m flag which means that we are running in the matrix.)

In another terminal, you can launch the noVNC viewer for this exercise and open RViz.

dts code vnc -R ROBOT_NAME

where ROBOT_NAME could be the real or the virtual robot (use whichever you ran the dts code workbench and dts code build command with).

This will show you your published pose estimate (blue arrow with a covariance ellipse in purple) as well the ground truth pose of the robot (red arrow which should be inside the ellipse if your implementation is correct).

You will also see markers that correspond to the AprilTag traffic signs in the map. As each one is detected by your camera you will see it change color from green to blue. At initialization, it should like this:

rviz initialization of Duckietown localization LX

You can also look at an image that shows the tags that are being detected. It is published on the topic /ROBOT_NAME/detections/image/compressed (for example you can view with rqt_image_viewer).

This output should look like this:

Duckietown EKF localization LX AprilTag detections

Fig. 165 AprilTag detections.#

You may also use the keyboard controller to pilot your robot and test that the localization performance of your Duckiebot as it moves.

Troubleshooting#

Troubleshooting

SYMPTOM

When running dts code editor I get an error: dts :  No valid DTProject found at '/workspaces/dt-env-developer/lx'

RESOLUTION

Make sure your are executing the commands from inside a learning experience folder (e.g., */lx-ekf-localization/)

Troubleshooting

SYMPTOM

My virtual robot (named, e.g., VBOT) hangs indefinitely when trying to update it.

RESOLUTION

Try to restart it with: dts duckiebot virtual restart VBOT