FixTheRobot Explore Robotics / Localization & SLAM / M3 / M3-L5

This lab works on phones, but writing code is much easier on a laptop.

Localization & SLAM · Mapping and State Estimation · M3-L5 · Advanced

Implement a Simplified EKF-SLAM

Filter SLAM is a short algorithm and a long list of ways to get it almost right. The covariance has a block structure, and each step touches a different part of it.

You complete the prediction and the update, including how a new landmark enters the state, then run the whole filter through a loop and test whether it tells the truth about its own uncertainty.

Your task

    Simulation
    solution.pyLoading Python…
    Ctrl+Enter runs
    What Python can I use?

    Plain Python with math and, for matrices, slamkit (matmul, transpose, solve, inverse, identity). The data arrive as the dictionary D. There is no NumPy, random, json or copy in the browser runtime. Your code runs locally and never leaves this page.

    Hints

    What this lab teaches

    Stages 1 and 2 are the two steps of M3-T7, graded against a reference filter on states with real correlations. Each starter has the characteristic mistake: forgetting the cross-covariances in prediction, and an update that touches only the observed landmark. Stage 3 runs the reference filter for a loop with the two outcomes that matter in practice: loop closure on or off, and an overconfident or honest noise model (NEES). The true landmark positions stay in the simulator.

    Theory for this lab

    Mapping and State Estimation labs

    1. M3-L1: Build a 2D Occupancy Grid
    2. M3-L2: Fix a Misaligned Map
    3. M3-L3: Diagnose Scan-Matching Failure
    4. M3-L4: Explore Particle-Filter Localization
    5. M3-L5: Implement a Simplified EKF-SLAM: this lab.

    Educational simplification: known landmark identities (no data association), one range-bearing observation per update, a dense full-matrix implementation, a few landmarks. Real EKF-SLAM uses sparse updates, robust association (M2-T7) and consistency fixes; most modern systems use graph optimisation instead (Module 4).

    Get the next lab

    Get notified when new robotics labs are released. One email per new lab, nothing else.