Simultaneous Localization and Mapping

EKF localization needs a map, and correspondence estimation needs a map to match against. A robot placed in an unknown room has neither. Simultaneous localization and mapping (SLAM) estimates the map and the robot’s path together, from the same measurements and actions. Given the actions

a_{1:t}

and the measurements

z_{1:t}

, full SLAM estimates the whole path and the map,

p(s_{1:t}, m \mid z_{1:t}, a_{1:t}),

while online SLAM estimates only the current pose and the map,

p(s_t, m \mid z_{1:t}, a_{1:t}).

The two estimates are coupled. An error in the pose places every landmark seen from that pose in the wrong spot, and when the robot sees a wrongly placed landmark again, the update shifts the pose estimate. This section builds the oldest online SLAM algorithm, EKF SLAM, in the version for unknown correspondences, where the landmark that produced each measurement is not given: section 10.3 of Thrun, Burgard and Fox (Burgard & Thrun, n.d.). It runs it on a TurtleBot with two different landmark front ends, the LIDAR and the D435i depth camera, feeding the same filter.

The EKF SLAM state

EKF SLAM keeps a Gaussian belief over a state that stacks the robot pose and every landmark found so far:

y_t = \begin{bmatrix} s_t \\ m \end{bmatrix} = (x, y, \theta, m_{1,x}, m_{1,y}, \ldots, m_{N,x}, m_{N,y})^\top, \qquad \mathtt{bel}(y_t) = \mathcal N(\mu_t, \Sigma_t).

The state has

3 + 2N

entries and grows as the robot finds landmarks. The covariance

\Sigma_t

holds the uncertainty of the pose and of each landmark, and the cross terms between every pair. The cross terms are nonzero because the map was built from an uncertain pose. Through them, a correction to one landmark also moves the pose and every other landmark. The prediction step moves only the pose, because landmarks do not move. With the velocity motion model

g

from the EKF localization section and its Jacobians

G_t

(state) and

V_t

(wheel speeds),

\bar\mu_t = \begin{bmatrix} g(a_t, \mu_{t-1,s}) \\ \mu_{t-1,m} \end{bmatrix}, \qquad \bar\Sigma_t = \begin{bmatrix} G_t & 0 \\ 0 & I \end{bmatrix} \Sigma_{t-1} \begin{bmatrix} G_t & 0 \\ 0 & I \end{bmatrix}^\top + \begin{bmatrix} Q_t & 0 \\ 0 & 0 \end{bmatrix}, \qquad Q_t = V_t M_t V_t^\top ,

where

M_t

is the covariance of the wheel-speed noise. The pose block, the pose’s cross terms with the map, and nothing else change. The robot model is copied from the EKF localization section.

r, d = 0.033, 0.1435
dt = 0.2  # s, one lidar scan at 5 Hz


def wrap(angle):
    return (angle + np.pi) % (2 * np.pi) - np.pi


def twist(a):
    u_L, u_R = a
    return r / 2 * (u_L + u_R), r / (2 * d) * (u_R - u_L)


def g(a, s, dt=dt):
    v, w = twist(a)
    x, y, th = s
    if abs(w) < 1e-9:
        return np.array([x + v * dt * np.cos(th), y + v * dt * np.sin(th), th])
    return np.array([
        x - v / w * np.sin(th) + v / w * np.sin(th + w * dt),
        y + v / w * np.cos(th) - v / w * np.cos(th + w * dt),
        wrap(th + w * dt),
    ])


def G(a, s, dt=dt):
    v, w = twist(a)
    th = s[2]
    if abs(w) < 1e-9:
        return np.array([[1, 0, -v * dt * np.sin(th)], [0, 1, v * dt * np.cos(th)], [0, 0, 1]])
    return np.array([
        [1, 0, v / w * (np.cos(th + w * dt) - np.cos(th))],
        [0, 1, v / w * (np.sin(th + w * dt) - np.sin(th))],
        [0, 0, 1],
    ])


def V(a, s, dt=dt):
    v, w = twist(a)
    th = s[2]
    s0, c0, s1, c1 = np.sin(th), np.cos(th), np.sin(th + w * dt), np.cos(th + w * dt)
    dvw_du = np.array([[r / 2, r / 2], [-r / (2 * d), r / (2 * d)]])
    if abs(w) < 1e-9:
        dg_dvw = np.array([[c0 * dt, -v * s0 * dt**2 / 2], [s0 * dt, v * c0 * dt**2 / 2], [0, dt]])
        return dg_dvw @ dvw_du
    dg_dvw = np.array([
        [(s1 - s0) / w, v * (s0 - s1) / w**2 + v * c1 * dt / w],
        [(c0 - c1) / w, -v * (c0 - c1) / w**2 + v * s1 * dt / w],
        [0, dt],
    ])
    return dg_dvw @ dvw_du

Measurements, new landmarks and correspondences

A landmark

m_k

is measured as a range and bearing,

z = h(s, m_k)

, the same model as in localization. The difference is that the landmark position is now part of the state, so the Jacobian has two nonzero blocks: one for the pose and one for landmark

k

. For the innovation covariance and the gain the filter builds the full

2 \times (3 + 2N)

matrix

H^k_t = \begin{bmatrix} \dfrac{\partial h}{\partial s} & 0 \cdots 0 & \dfrac{\partial h}{\partial m_k} & 0 \cdots 0 \end{bmatrix}, \qquad \Psi_k = H^k_t \bar\Sigma_t H^{k\top}_t + R_t ,

where

R_t

is the measurement noise. The bearing part of the innovation is wrapped to

[-\pi, \pi)

: a landmark straight behind the robot can be measured at

\pi - \epsilon

and predicted at

-\pi + \epsilon

, and without wrapping, the raw difference of almost

2\pi

would enter the update as a large wrong innovation.

Unknown correspondences

The algorithm of Thrun, Burgard and Fox (Table 10.2) picks, for each measurement

z^i_t

, the landmark with the smallest Mahalanobis distance

\pi_k = (z^i_t - \hat z^k_t)^\top \Psi_k^{-1} (z^i_t - \hat z^k_t),

the quantity

D^2_{ik}

of the correspondence estimation section. It also considers one extra hypothesis, that

z^i_t

comes from a landmark not yet in the map, and gives that hypothesis the fixed score

\alpha

. If every existing landmark scores above

\alpha

, the measurement starts a new landmark. The code uses

\alpha = 13.8

, the 99.9th percentile of the

\chi^2

distribution with 2 degrees of freedom: a measurement of a known landmark scores above it only once in a thousand times.

Adding a landmark

The new landmark’s mean is the measured point placed in the world from the predicted pose,

m_{N+1} = \begin{bmatrix} \bar\mu_x + r \cos(\phi + \bar\mu_\theta) \\ \bar\mu_y + r \sin(\phi + \bar\mu_\theta) \end{bmatrix}.

Its covariance must contain both the measurement noise and the pose uncertainty, and it must be correlated with the pose. Thrun, Burgard and Fox start every landmark with infinite variance (their equation 10.10), which a computer cannot store. The code instead extends

\Sigma

with the two Jacobians of this placement function,

G_s

with respect to the pose and

G_z

with respect to the measurement, as Corke does in sections 6.3 and 6.4 (Corke, 2023):

\Sigma_{m_{N+1}} = G_s \Sigma_{ss} G_s^\top + G_z R_t G_z^\top, \qquad \Sigma_{m_{N+1}, \cdot} = G_s \Sigma_{s, \cdot} .

With

N

landmarks in the map, the quantities above have these sizes:

Symbol	Meaning	Size
$y_t$ , $\mu_t$ , $\bar\mu_t$	full state (pose and $N$ landmarks), its mean and predicted mean	$(3 + 2N) \times 1$
$\Sigma_t$ , $\bar\Sigma_t$	covariance of the full state	$(3 + 2N) \times (3 + 2N)$
$s_t$	robot pose $(x, y, \theta)$	$3 \times 1$
$a_t$	wheel speeds	$2 \times 1$
$G_t$	motion Jacobian with respect to the pose	$3 \times 3$
$V_t$	motion Jacobian with respect to the wheel speeds	$3 \times 2$
$M_t$	wheel-speed noise covariance	$2 \times 2$
$Q_t = V_t M_t V_t^\top$	motion noise of the pose	$3 \times 3$
$z^i_t$ , $\hat z^k_t$	one range-bearing measurement and its prediction for landmark $k$	$2 \times 1$
$H^k_t$	measurement Jacobian for landmark $k$	$2 \times (3 + 2N)$
$R_t$	measurement noise covariance	$2 \times 2$
$\Psi_k$	innovation covariance	$2 \times 2$
$K_t = \bar\Sigma_t H^{k\top}_t \Psi_k^{-1}$	Kalman gain for one measurement	$(3 + 2N) \times 2$
$G_s$ , $G_z$	placement Jacobians with respect to the pose and the measurement	$2 \times 3$ , $2 \times 2$
$\Sigma_{m_{N+1}}$	covariance of a new landmark	$2 \times 2$
$\Sigma_{m_{N+1}, \cdot}$	its cross-covariance with the existing state	$2 \times (3 + 2N)$

The block matrix that multiplies

\Sigma_{t-1}

in the prediction is

(3 + 2N) \times (3 + 2N)

, with

G_t

in the pose block and the identity on the landmarks. Adding a landmark grows the state by 2 and the covariance by two rows and two columns. A measurement that creates a landmark is not used to update the pose: it carries no information about the pose until the landmark is seen again. The function ekf_slam below performs one prediction and then processes the measurements one at a time, updating the belief after each.

def h_lm(s, m_k):
    """Range and bearing from pose s to a landmark at m_k."""
    dx, dy = m_k - s[:2]
    return np.array([np.hypot(dx, dy), wrap(np.arctan2(dy, dx) - s[2])])


def H_lm(s, m_k):
    """Jacobians of h_lm with respect to the pose (2x3) and to the landmark (2x2)."""
    dx, dy = m_k - s[:2]
    q = dx**2 + dy**2
    sq = np.sqrt(q)
    Hs = np.array([[-sq * dx, -sq * dy, 0], [dy, -dx, -q]]) / q
    Hm = np.array([[sq * dx, sq * dy], [-dy, dx]]) / q
    return Hs, Hm


def innov(z, zhat):
    nu = z - zhat
    nu[1] = wrap(nu[1])
    return nu


def add_landmark(mu, Sigma, z, R):
    """Append a landmark initialized from measurement z, with the insertion Jacobians."""
    x, y, th = mu[:3]
    rng_, phi = z
    c, s_ = np.cos(phi + th), np.sin(phi + th)
    m_new = np.array([x + rng_ * c, y + rng_ * s_])
    Gs = np.array([[1, 0, -rng_ * s_], [0, 1, rng_ * c]])
    Gz = np.array([[c, -rng_ * s_], [s_, rng_ * c]])
    n = len(mu)
    Sigma_new = np.zeros((n + 2, n + 2))
    Sigma_new[:n, :n] = Sigma
    cross = Gs @ Sigma[:3, :]
    Sigma_new[n:, :n] = cross
    Sigma_new[:n, n:] = cross.T
    Sigma_new[n:, n:] = Gs @ Sigma[:3, :3] @ Gs.T + Gz @ R @ Gz.T
    return np.r_[mu, m_new], Sigma_new


ALPHA = chi2.ppf(0.999, 2)  # new-landmark threshold on the Mahalanobis distance


def ekf_slam(mu, Sigma, a, z, M, R, dt=dt):
    """One step of EKF SLAM with maximum likelihood correspondences.

    Returns the new mean and covariance, and for each measurement the landmark index it was assigned to.
    """
    n = len(mu)
    G_t, V_t = G(a, mu[:3], dt), V(a, mu[:3], dt)
    mu_bar = mu.copy()
    mu_bar[:3] = g(a, mu[:3], dt)
    Sigma_bar = Sigma.copy()
    Sigma_bar[:3, :] = G_t @ Sigma[:3, :]
    Sigma_bar[:, :3] = Sigma_bar[:, :3] @ G_t.T
    Sigma_bar[:3, :3] += V_t @ M @ V_t.T  # Q_t, the motion noise
    assigned = []
    for z_i in z:
        N = (len(mu_bar) - 3) // 2
        best_k, best_pi, best = None, ALPHA, None
        for k in range(N):
            j = 3 + 2 * k
            Hs, Hm = H_lm(mu_bar[:3], mu_bar[j:j + 2])
            H_full = np.zeros((2, len(mu_bar)))
            H_full[:, :3], H_full[:, j:j + 2] = Hs, Hm
            Psi = H_full @ Sigma_bar @ H_full.T + R
            nu = innov(z_i, h_lm(mu_bar[:3], mu_bar[j:j + 2]))
            pi_k = nu @ np.linalg.solve(Psi, nu)
            if pi_k < best_pi:
                best_k, best_pi, best = k, pi_k, (H_full, Psi, nu)
        if best_k is None:  # no landmark explains z_i better than alpha: a new landmark
            mu_bar, Sigma_bar = add_landmark(mu_bar, Sigma_bar, z_i, R)
            assigned.append(N)
            continue
        H_full, Psi, nu = best
        K = Sigma_bar @ H_full.T @ np.linalg.inv(Psi)
        mu_bar = mu_bar + K @ nu
        mu_bar[2] = wrap(mu_bar[2])
        Sigma_bar = (np.eye(len(mu_bar)) - K @ H_full) @ Sigma_bar
        Sigma_bar = (Sigma_bar + Sigma_bar.T) / 2
        assigned.append(best_k)
    return mu_bar, Sigma_bar, assigned

The room and the drive

The TurtleBot drives a counterclockwise circle of radius 2.4 m at 0.2 m/s in a room with walls at

\pm 4

m. Eight pillars of radius 10 cm stand on a ring of radius 2.8 m, just outside the path. Only pillars within 2.5 m are used. The drive lasts 1.6 laps, 603 steps of 0.2 s, so the robot passes its first landmarks again. The robot follows the circle exactly. The noise is in the wheel encoder readings: each step the reported wheel speeds differ from the true ones by the noise used in the EKF localization section, a standard deviation of 5% of each wheel speed plus 0.1 rad/s. Dead reckoning integrates these reports, and so does the filter’s prediction.

ROOM = 4.0  # walls at x, y = +-4 m
PILLAR_R = 0.1
PILLARS = 2.8 * np.array([[np.cos(t), np.sin(t)] for t in np.deg2rad(np.arange(0, 360, 45) + 20)])
MAX_RANGE = 2.5  # m, landmarks farther than this are not used


def cast(s, pillars, angles):
    """Lidar ranges along the given world-frame ray angles from position s[:2]."""
    p = s[:2]
    out = np.full(len(angles), np.inf)
    for i, a in enumerate(angles):
        u = np.array([np.cos(a), np.sin(a)])
        best = np.inf
        for ax in (0, 1):  # walls
            if abs(u[ax]) > 1e-12:
                for wall in (-ROOM, ROOM):
                    t = (wall - p[ax]) / u[ax]
                    if t > 0:
                        best = min(best, t)
        for c in pillars:  # pillars
            oc = p - c
            b = oc @ u
            disc = b * b - (oc @ oc - PILLAR_R**2)
            if disc >= 0:
                t = -b - np.sqrt(disc)
                if t > 0:
                    best = min(best, t)
        out[i] = best
    return out


V_CMD, RADIUS = 0.2, 2.4
W_CMD = V_CMD / RADIUS
A_CMD = np.array([(V_CMD - d * W_CMD) / r, (V_CMD + d * W_CMD) / r])
SIGMA_U = 0.05 * np.abs(A_CMD) + 0.1
M = np.diag(SIGMA_U**2)
N_STEPS = int(1.6 * 2 * np.pi * RADIUS / V_CMD / dt)  # 1.6 laps


def true_id(s_true, z_i, pillars):
    """Which pillar a measurement came from, for scoring only."""
    p = s_true[:2] + z_i[0] * np.array([np.cos(z_i[1] + s_true[2]), np.sin(z_i[1] + s_true[2])])
    return int(np.argmin(np.linalg.norm(pillars - p, axis=1)))

The figure shows the room from above, drawn to scale. The robot starts at the green dot on the right, heading in the

+y

direction, and drives counterclockwise. The blue pose, 30° around the circle, is the example used in the next two figures. The dashed circle around it marks the 2.5 m range within which pillars count as landmarks, and the shaded wedge is the D435i’s field of view, about 55° wide.

Pillars from a LIDAR scan

The simulated LIDAR matches the one on the course simulator’s TurtleBot3: 360 beams, one per degree, five scans a second, range noise of 1 cm. A scan is a list of ranges, so the front end has to find the pillars in it. It splits the scan wherever the range jumps by more than 15 cm between neighboring beams. A pillar in front of a wall shows up as a short run of beams with a jump on both sides. A cluster wider than 25 cm is a stretch of wall and is dropped, and so is a cluster farther than 2.5 m. The pillar’s bearing is the mean bearing of its beams, and its range is the shortest range in the cluster plus the pillar radius, because the beams hit the near surface. The same steps apply to a real scan, with three refinements. The jump threshold should grow with range, because neighboring beams land farther apart on distant surfaces. A pole of unknown size is better handled by fitting a circle to the cluster’s points and reporting the range and bearing of its center. Walls and corners, the other common landmarks, are found by fitting lines to the clusters. For ROS 2, laser_segmentation segments laser scans this way. A 3D point cloud is usually treated as an unordered set of points instead. The common pipeline removes the ground plane with RANSAC and groups the remaining points by their Euclidean distance. Open3D provides the two steps as segment_plane and cluster_dbscan, and the Point Cloud Library as SACSegmentation and EuclideanClusterExtraction. A LIDAR feature carries a weaker signature than a camera’s. The cluster’s width and shape separate a pillar from a wall, and the intensity of the returns separates a retroreflector from paint or concrete. The Point Cloud Library also computes shape descriptors that serve as a 3D signature, such as FPFH for the neighborhood of a point and VFH for a whole cluster. These attributes identify the kind of landmark that was hit, but not the individual landmark. Identical pillars still look alike, which is why this section has to estimate the correspondences.

SIGMA_LIDAR = 0.01  # m, range noise of the simulated lidar
N_BEAMS = 360


def lidar_landmarks(s, pillars, rng):
    """Range and bearing of each pillar found in one simulated 360-beam scan."""
    bearings = np.arange(N_BEAMS) * 2 * np.pi / N_BEAMS
    ranges = cast(s, pillars, s[2] + bearings) + rng.normal(0, SIGMA_LIDAR, N_BEAMS)
    # split the scan into clusters wherever the range jumps
    breaks = np.flatnonzero(np.abs(np.diff(np.r_[ranges, ranges[0]])) > 0.15)
    if len(breaks) == 0:
        return []
    z = []
    starts = breaks + 1
    for a, b in zip(starts, np.r_[starts[1:], starts[0] + N_BEAMS]):
        idx = np.arange(a, b) % N_BEAMS
        if len(idx) < 2:
            continue
        rr, bb = ranges[idx], bearings[idx]
        pts = rr * np.array([np.cos(bb), np.sin(bb)])
        if np.linalg.norm(pts[:, -1] - pts[:, 0]) > 2.5 * PILLAR_R or rr.mean() > MAX_RANGE:
            continue  # too wide for a pillar (a wall), or too far
        k = np.argmin(rr)
        bearing = np.arctan2(np.sin(bb).mean(), np.cos(bb).mean())
        z.append(np.array([rr[k] + PILLAR_R, wrap(bearing)]))
    return z

On the left, each dot is where one beam of the scan from the example pose stopped. Most beams end on the walls. The colored dots are returns from the three pillars within 2.5 m, and the crosses are the centers the front end computed for them. The right panel is the same scan in the form the front end receives it: one range per degree, measured from the robot’s heading. Each pillar is a short dip with a steep jump on both sides. The three dips below the dashed line become landmark measurements. The dips above it are pillars farther than 2.5 m, which the front end drops.

Pillars from the D435i

The D435i sees only what is in front of it, within its field of view. The front end projects each pillar’s center into the color image with the intrinsics from the course’s D435i configuration (

f_x = 617

px,

c_x = 321

px, 640 pixels wide) and adds one pixel of noise to the column. The depth image gives the distance to the pillar’s near surface. Stereo depth error grows with the square of the distance, so the code uses a standard deviation of

0.01 z^2 + 0.002

m, an assumed model rather than a datasheet value. From the column

u

and the depth, the bearing and range in the camera frame are

\phi = \arctan\frac{c_x - u}{f_x}, \qquad r = \frac{z + r_{\text{pillar}}}{\cos\phi}.

The camera sits at the robot’s origin, looking forward. On a real robot the camera-to-base transform from the D435i section moves the measurement into the base frame first.

# D435i color camera, intrinsics from the turtlebot-maze config
FX, CX, WIDTH = 617.0, 321.0, 640
SIGMA_PX = 1.0


def camera_landmarks(s, pillars, rng):
    """Range and bearing of each pillar center seen by a forward-looking depth camera."""
    z = []
    for c in pillars:
        dx, dy = c - s[:2]
        xc = np.cos(s[2]) * dx + np.sin(s[2]) * dy  # forward
        yc = -np.sin(s[2]) * dx + np.cos(s[2]) * dy  # left
        depth = xc - PILLAR_R  # the camera sees the near surface
        if depth < 0.3 or np.hypot(xc, yc) > MAX_RANGE:
            continue
        u = CX - FX * yc / xc + rng.normal(0, SIGMA_PX)
        if not 0 <= u < WIDTH:
            continue
        depth_meas = depth + rng.normal(0, 0.01 * depth**2 + 0.002)
        bearing = np.arctan((CX - u) / FX)
        x_c = depth_meas + PILLAR_R
        z.append(np.array([x_c / np.cos(bearing), bearing]))
    return z

SENSORS ={"lidar": lidar_landmarks, "camera": camera_landmarks}
R_MEAS = {"lidar": np.diag([0.02**2, np.deg2rad(0.75) ** 2]), "camera": np.diag([0.05**2, np.deg2rad(0.5) ** 2])}

From the example pose only pillar 1 is inside the field of view and within 2.5 m. In the image it is a vertical band about 80 pixels wide. The front end reports the column of the band’s center and the depth to the pillar’s near surface. Along the whole path the camera never has more than one pillar in view, while the LIDAR sees two or three. The results below show how the number of pillars in view affects the estimate.

Measurement noise of the front ends

The filter needs

R_t

, the covariance of the range and bearing a front end reports. The code below places the robot at 400 random poses and compares each detection with the true pillar.

def front_end_errors(sense, n_poses=400, seed=1):
    """Errors of a front end's range and bearing against the true pillar, over random poses."""
    rng = np.random.default_rng(seed)
    errs = []
    for _ in range(n_poses):
        s = np.r_[rng.uniform(-3, 3, 2), rng.uniform(-np.pi, np.pi)]
        if np.min(np.linalg.norm(PILLARS - s[:2], axis=1)) < 0.4:
            continue  # standing inside or against a pillar
        for z in sense(s, PILLARS, rng):
            errs.append(innov(z, h_lm(s, PILLARS[true_id(s, z, PILLARS)])))
    return np.array(errs)


for name, sense in SENSORS.items():
    e = front_end_errors(sense)
    print(f"{name:>6}: {len(e)} detections, range error mean {e[:, 0].mean():+.3f} m, std {e[:, 0].std():.3f} m; "
          f"bearing error std {np.degrees(e[:, 1].std()):.2f} deg")
    print(f"        R_t uses std {np.sqrt(R_MEAS[name][0, 0]):.3f} m and {np.degrees(np.sqrt(R_MEAS[name][1, 1])):.2f} deg")

 lidar: 889 detections, range error mean -0.007 m, std 0.008 m; bearing error std 0.20 deg
        R_t uses std 0.020 m and 0.75 deg
camera: 143 detections, range error mean -0.005 m, std 0.032 m; bearing error std 0.09 deg
        R_t uses std 0.050 m and 0.50 deg

The LIDAR’s range is 7 mm short on average, because the nearest beam rarely hits the exact front of the pillar, and its errors are 8 mm and 0.2° (one standard deviation). The camera is four times worse in range, 3.2 cm, because of the depth noise, but better in bearing, 0.09°, because a pixel is narrower than a LIDAR beam.

R_t

is set above these figures, to 2 cm and 0.75° for the LIDAR and 5 cm and 0.5° for the camera. The filter is also affected by errors that these statistics do not measure. It evaluates

h

and its Jacobian at the estimated pose and landmarks, and the LIDAR’s 7 mm range bias repeats in every scan, while the filter assumes the noise is independent from scan to scan. If

R_t

is set to the raw statistics, the filter gives each measurement too much weight, the gate becomes too tight, and a measurement of a known pillar that falls just outside the gate starts a duplicate landmark.

Running the filter

The code below drives the loop once with each front end. Both runs use the same wheel noise, so dead reckoning is the same in both.

def simulate(sensor, seed=0, on_step=None):
    """Drive the loop once with one front end; return the histories.

    on_step(t, s, mu, Sigma, mu_dr, z) is called after every filter step, for the recording below.
    """
    wheel_rng, sensor_rng = np.random.default_rng(seed), np.random.default_rng(seed + 100)
    sense, R = SENSORS[sensor], R_MEAS[sensor]
    s = np.array([RADIUS, 0.0, np.pi / 2])
    mu, Sigma = s.copy(), np.diag([1e-6, 1e-6, 1e-6])  # the map frame is the start pose
    mu_dr = s.copy()
    lm_truth, first_seen, last_seen, wrong, closure_step = [], {}, {}, 0, None
    true_path, ekf_path, dr_path, covs = [s], [mu[:3]], [mu_dr], [Sigma[:3, :3]]
    for t in range(N_STEPS):
        s = g(A_CMD, s)  # the robot drives the circle
        a_t = A_CMD + wheel_rng.normal(0, SIGMA_U)  # what the wheel encoders report
        mu_dr = g(a_t, mu_dr)
        z = sense(s, PILLARS, sensor_rng)
        mu, Sigma, assigned = ekf_slam(mu, Sigma, a_t, z, M, R)
        for z_i, k in zip(z, assigned):  # score the associations against the truth
            tid = true_id(s, z_i, PILLARS)
            if k == len(lm_truth):
                lm_truth.append(tid)
                first_seen[k] = t
            elif lm_truth[k] != tid:
                wrong += 1
            if closure_step is None and first_seen[k] < 0.1 * N_STEPS and t - last_seen.get(k, t) > 100:
                closure_step = t  # a landmark from the start, seen again after 20 s out of view
            last_seen[k] = t
        if on_step is not None:
            on_step(t, s, mu, Sigma, mu_dr, z)
        true_path.append(s); ekf_path.append(mu[:3].copy()); dr_path.append(mu_dr); covs.append(Sigma[:3, :3].copy())
    true_path, ekf_path, dr_path, covs = map(np.array, (true_path, ekf_path, dr_path, covs))
    err = ekf_path - true_path
    err[:, 2] = wrap(err[:, 2])
    nees = np.einsum("ti,tij,tj->t", err[1:], np.linalg.inv(covs[1:]), err[1:])
    return dict(mu=mu, Sigma=Sigma, lm_truth=lm_truth, wrong=wrong, closure_step=closure_step,
                true_path=true_path, ekf_path=ekf_path, dr_path=dr_path, covs=covs, err=err, nees=nees)


runs = {name: simulate(name) for name in SENSORS}
for name, o in runs.items():
    pos_err = np.hypot(o["err"][:, 0], o["err"][:, 1])
    dr_err = np.hypot(*(o["dr_path"][-1, :2] - o["true_path"][-1, :2]))
    inside = np.mean((o["nees"] > chi2.ppf(0.025, 3)) & (o["nees"] < chi2.ppf(0.975, 3)))
    print(f"{name:>6}: {(len(o['mu']) - 3) // 2} landmarks, {o['wrong']} wrong associations, "
          f"final position error {pos_err[-1]:.3f} m (dead reckoning {dr_err:.2f} m), "
          f"mean NEES {o['nees'].mean():.1f}, steps inside the 95% NEES bounds {inside:.0%}")
    c = o["closure_step"] + 1
    print(f"        loop closed at {c * dt:.1f} s: position error {pos_err[c - 1]:.3f} m before, {pos_err[c]:.3f} m after")

 lidar: 8 landmarks, 0 wrong associations, final position error 0.031 m (dead reckoning 1.21 m), mean NEES 3.1, steps inside the 95% NEES bounds 97%
        loop closed at 57.8 s: position error 0.085 m before, 0.026 m after
camera: 8 landmarks, 0 wrong associations, final position error 0.135 m (dead reckoning 1.21 m), mean NEES 3.0, steps inside the 95% NEES bounds 99%
        loop closed at 67.8 s: position error 0.289 m before, 0.199 m after

Dead reckoning (dotted) spirals inward and ends 1.21 m from the truth. Both filters stay on the circle. Each found exactly the eight pillars and assigned every measurement to the right one. The LIDAR’s landmark ellipses are smaller than the camera’s: the LIDAR keeps each pillar in view for longer and from more directions as the robot goes round, while the camera sees it only on the approach.

Watching a run step by step

The recording below replays both runs, the LIDAR run on the left and the camera run on the right. In each map the green arrow is the true pose and the orange arrow is the filter’s estimate, with its

2\sigma

ellipse. The gray line is dead reckoning. Blue dots are the landmarks in the map, each with its own

2\sigma

ellipse, and the gold lines are the current step’s measurements. The gray dots in the LIDAR map are the current scan, and the gold wedge in the camera map is the field of view. The plots underneath show the position error and the number of landmarks in the map for both runs.

Press play or drag the time marker. Scroll to zoom a map and double-click to reset it. The button at the top right of each panel opens it full size.

The error plots show how the two runs differ. With the camera, the error grows steadily to about 0.33 m: a pillar leaves the narrow field of view after a few seconds, so each landmark constrains the pose only briefly, and the drift of the pose carries over into the landmark positions. At 67.8 s the camera sees its first landmark again, initialized at the start when the pose was known exactly. That one update reduces the pose error from 0.289 m to 0.199 m, and the following updates briefly bring it below 1 cm. The error then grows again on the second lap and ends at 0.135 m. This is loop closure. Through the cross terms of

\Sigma

, the correction also moves every landmark mapped during the lap. The LIDAR sees about three pillars at all times, so its error stays below 10 cm. It closes the loop too, at 57.8 s, and its error drops from 0.085 m to 0.026 m. In both runs the shaded

2\sigma

band follows the error: it grows while the robot explores and shrinks at the closure.

The correlation matrix of the camera run’s final belief shows the structure Thrun, Burgard and Fox describe in section 10.3.2. Rows and columns are the pose followed by the

x

and

y

coordinates of each landmark. Many entries are close to

+1

-1

: the landmark estimates have become strongly correlated with each other. The map is well known in relative terms, how far each pillar is from the others, while the whole map can still shift together by an amount that no measurement can remove. Every landmark is also correlated with the pose.

The normalized estimation error squared

(\mu_t - s_t)^\top \Sigma_{t,ss}^{-1} (\mu_t - s_t)

of the pose checks whether the filter’s covariance matches its actual error. For a consistent filter it follows a

\chi^2

distribution with 3 degrees of freedom, so 95% of the steps should fall between the dashed lines. The LIDAR run has 97% of its steps inside with a mean of 3.1, and the camera run 99% with a mean of 3.0. The dips of the LIDAR curve below the lower line mark the stretches right after its loop closure, when the error is smaller than the covariance implies. A single run may not be representative, so the code below repeats both runs for ten seeds of the wheel and sensor noise.

print("seed  lidar: landmarks, wrong, final error (m) | camera: landmarks, wrong, final error (m)")
for seed in range(10):
    rows = []
    for name in SENSORS:
        o = simulate(name, seed)
        rows.append(f"{(len(o['mu']) - 3) // 2:2d}, {o['wrong']:2d}, {np.hypot(*o['err'][-1, :2]):.3f}")
    print(f"{seed:4d}  {rows[0]}          | {rows[1]}")

seed  lidar: landmarks, wrong, final error (m) | camera: landmarks, wrong, final error (m)

   0   8,  0, 0.031          |  8,  0, 0.135

   1   8,  0, 0.009          |  8,  0, 0.056

   2   8,  0, 0.037          |  8,  0, 0.263

   3   8,  0, 0.215          |  8,  0, 0.205

   4   8,  0, 0.024          |  8,  0, 0.202

   5   8,  0, 0.010          |  8,  0, 0.036

   6   8,  0, 0.064          |  8,  0, 0.174

   7   8,  0, 0.010          |  8,  0, 0.154

   8   8,  0, 0.040          |  8,  0, 0.041

   9   8,  0, 0.061          |  8,  0, 0.184

In all twenty runs the filter finds exactly eight landmarks and makes no wrong association. The final error stays below 0.27 m, against more than a meter of dead reckoning in the run above.

Limits of EKF SLAM

The covariance has

(3 + 2N)^2

entries, and every update touches all of them, so each step costs

O(N^2)

. A few hundred landmarks is the practical limit. Maximum likelihood correspondence has a second weakness: it selects one assignment and never reconsiders it, so a single wrong association stays in the map permanently. Thrun, Burgard and Fox (section 10.3.3) list the usual defenses: a provisional list where new landmarks wait until they have been seen several times, a counter that removes landmarks the robot should see but does not, and landmarks spaced far enough apart that they cannot be confused. The pillars here are 2.1 m apart and are seen within 2.5 m, so the association is easy. In this section the LIDAR and the camera only produce landmarks for the same filter. A camera can also estimate its own motion without any depth, from how image features move, and that is what the visual-inertial odometry and visual SLAM sections build on. Visual SLAM systems also replace the single growing filter with a graph of keyframes optimized in batches, which removes the

O(N^2)

limit.

Lectures

Key references: (Burgard & Thrun, n.d.; Corke, 2023)

References

Burgard, W., Thrun, S. (n.d.). Probabilistic Robotics.
Corke, P. (2023). Robotics, vision and control: Fundamental algorithms in python.

Found an error on this page? Report an issue.

State Estimation & Mapping

State Estimation

SLAM

Simultaneous Localization and Mapping

The EKF SLAM state

Measurements, new landmarks and correspondences

Unknown correspondences

Adding a landmark

The room and the drive

Pillars from a LIDAR scan

Pillars from the D435i

Measurement noise of the front ends

Running the filter

Watching a run step by step

Limits of EKF SLAM

Lectures

References

​The EKF SLAM state

​Measurements, new landmarks and correspondences

​Unknown correspondences

​Adding a landmark

​The room and the drive

​Pillars from a LIDAR scan

​Pillars from the D435i

​Measurement noise of the front ends

​Running the filter

​Watching a run step by step

​Limits of EKF SLAM

​Lectures

​References

The EKF SLAM state

Measurements, new landmarks and correspondences

Unknown correspondences

Adding a landmark

The room and the drive

Pillars from a LIDAR scan

Pillars from the D435i

Measurement noise of the front ends

Running the filter

Watching a run step by step

Limits of EKF SLAM

Lectures

References