> ## Documentation Index
> Fetch the complete documentation index at: https://aegean.ai/llms.txt
> Use this file to discover all available pages before exploring further.

# Mapping a Room of Pillars with EKF SLAM

> EKF SLAM with LIDAR and D435i landmark front ends.

[![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/pantelis/eng-ai-agents/blob/main/notebooks/rse/ekf-slam/index.ipynb)

In the [theory section](https://aegean.ai/aiml-common/lectures/rse/slam/index.md), we derived the prediction and correction equations for the joint distribution of the robot pose and the landmark positions. We will now use those equations to map a room with a simulated TurtleBot. The robot starts with an empty map and receives range and bearing measurements of pillars, without being told which pillar produced each measurement.

We will run the same estimator twice: first with a scanning lidar and then with a simplified D435i depth-camera model. The difference between the two runs lies in the observations available to the filter and their assumed noise covariances.

## The algorithm

The function `ekf_slam` implements the following cycle, adapted from Table 10.2 of Thrun, Burgard and Fox (2005).

1. Predict the robot pose using $g$ and propagate the covariance using $G_t$, $V_t$ and $M_t$. The landmark means remain unchanged.
2. For each observation $z_t^i$, compute the predicted observation, innovation, Jacobian and innovation covariance for each landmark already in the map. Use the squared Mahalanobis distance to compare candidates.
3. If no candidate passes the gate $\alpha$, initialize a landmark using `add_landmark`. This enlarges the state and covariance without correcting the existing pose estimate.
4. Otherwise, use the candidate with the smallest squared Mahalanobis distance and correct the joint mean and covariance. The correction may affect landmarks that were not observed.

Observations are processed sequentially. Each one uses the belief left by the preceding observation. This implementation uses nearest-neighbor Mahalanobis association. It omits the log-determinant of the innovation covariance, which the full Gaussian likelihood includes. A rejected observation is treated as a new landmark, a simplification that we will examine in the experiments.

## The robot model

We reuse the differential-drive model from [EKF localization](https://aegean.ai/aiml-common/lectures/rse/ekf-localization/index.md). The input is a pair of wheel angular velocities. The function `g` integrates the resulting linear and angular velocities over one sampling interval; `G` and `V` differentiate this motion with respect to the pose and wheel velocities, respectively. The sampling interval is 0.2 s for both simulated sensors.

```python theme={null}
r_w, d = 0.033, 0.1435  # wheel radius and half the wheel separation (m)
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_w / 2 * (u_L + u_R), r_w / (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_w / 2, r_w / 2], [-r_w / (2 * d), r_w / (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
```

## Implementing one filter step

The first two functions below compute the range and bearing to a landmark and the corresponding Jacobians. `innov` wraps the bearing residual into $[-\pi,\pi)$ so that angles on opposite sides of the branch cut are compared correctly.

When a new landmark is added, `add_landmark` computes its world coordinates from the current pose and the observation. Notice that the new rows and columns of the covariance matrix are not initialized to zero: the new landmark inherits uncertainty and cross-covariances from the pose.

For a known landmark, `ekf_slam` uses the covariance update $(I-K_tH_t^k)\bar\Sigma_t$ and symmetrizes the result. In exact arithmetic this is equivalent to the Joseph form for the Kalman gain used here:

$$
\Sigma_t=(I-K_tH_t^k)\bar\Sigma_t(I-K_tH_t^k)^\top+K_tR_tK_t^\top.
$$

The Joseph form is more robust to roundoff. Symmetrization alone does not guarantee positive definiteness; the final covariance eigenvalues in the repeated baseline runs are checked later. The code retains the shorter update for this demonstration.

```python theme={null}
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 unknown correspondences (Thrun, Burgard and Fox, Table 10.2).

    Each measurement goes to the landmark with the smallest Mahalanobis distance, which approximates the
    maximum likelihood correspondence without its ln det Psi term. A measurement whose distance to every
    landmark exceeds ALPHA, a chi-squared gate, starts a new landmark.

    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_d2, 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]))
            d2_k = nu @ np.linalg.solve(Psi, nu)  # Mahalanobis distance D^2_ik
            if d2_k < best_d2:
                best_k, best_d2, best = k, d2_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 room is 8 m by 8 m. Eight cylindrical pillars, each with a radius of 10 cm, lie on a circle of radius 2.8 m. The TurtleBot drives counterclockwise on a concentric circle of radius 2.4 m at 0.2 m/s. We use landmarks within 2.5 m and run the simulation for 603 intervals, approximately 1.6 laps. This gives the robot a chance to revisit landmarks from the beginning of the drive.

The true robot follows the prescribed circle. We corrupt the reported wheel speeds with independent Gaussian noise, using a standard deviation of 5% of the magnitude of each commanded wheel speed plus 0.1 rad/s. Both dead reckoning and the EKF integrate these noisy readings. The simulator knows the pillar locations and true pose, but the estimator does not use them for association. Ground truth is used to generate measurements and evaluate the estimates.

```python theme={null}
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_w, (V_CMD + d * W_CMD) / r_w])
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)))
```

![Output from cell 5](https://mintcdn.com/aegeanaiinc/Nwj-oOUe3UuRN9WC/aiml-common/lectures/rse/ekf-slam/images/cell_5_output_1.svg?fit=max\&auto=format\&n=Nwj-oOUe3UuRN9WC\&q=85\&s=f0b57facdac3e4481eb0e95407ac49b7)

The green dot marks the starting position; the robot initially faces upward, along the positive $y$ axis. We use the blue pose, 30° along the path, to illustrate both sensors. The dashed circle marks the maximum accepted landmark range, and the wedge shows the camera’s approximately 55° horizontal field of view.

## Extracting landmarks from a lidar scan

The lidar reports 360 ranges per scan, one per degree, with independent Gaussian range noise of standard deviation 1 cm. The filter needs a range and bearing to each pillar center. The scan gives only distances to surfaces.

A pillar produces a short run of returns in front of the more distant wall. The code splits the scan at range jumps greater than 15 cm, rejects clusters wider than 25 cm and discards distant detections. It estimates the bearing from the cluster's beam angles and estimates the center range by adding the known pillar radius to the shortest measured range. The minimum is convenient, but selecting the smallest of several noisy values introduces a bias that we will measure below.

On a real robot, a fixed jump threshold is often inadequate because the separation between adjacent beam endpoints increases with distance. Circle fitting can estimate the center and radius of a pole; line fitting can extract walls and corners. [laser\_segmentation](https://github.com/ajtudela/laser_segmentation) is one ROS 2 implementation of scan segmentation.

For an unordered 3D point cloud, another common approach is to remove the ground plane and cluster the remaining points. [Open3D](https://www.open3d.org/) provides `segment_plane` and `cluster_dbscan`; the [Point Cloud Library](https://pointclouds.org/) provides `SACSegmentation` and `EuclideanClusterExtraction`.

Geometry and return intensity can help distinguish types of lidar landmarks. Shape descriptors such as FPFH and VFH are also useful in 3D. None of these guarantees a unique identity for identical pillars, however. In this experiment the observations carry no landmark identifiers.

```python theme={null}
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
```

![Output from cell 7](https://mintcdn.com/aegeanaiinc/Nwj-oOUe3UuRN9WC/aiml-common/lectures/rse/ekf-slam/images/cell_7_output_1.svg?fit=max\&auto=format\&n=Nwj-oOUe3UuRN9WC\&q=85\&s=f347b791465914104cd64c64445d3d82)

The left panel shows the beam endpoints in the room and the estimated pillar centers (crosses). The right panel shows the same scan as a function of bearing. Locate the three nearby pillars in both panels: each produces a dip in the range profile. The front end rejects the more distant pillars. Notice that a pillar occupies several beams, but contributes only one observation to the filter.

## Simulating observations from the D435i

Unlike the lidar, the camera sees only a narrow sector in front of the robot. We use the color-camera intrinsics from the course configuration: $f_x=617$ px, $c_x=321$ px and image width 640 px.

This is an idealized feature-level simulation, not an RGB-D image-processing pipeline. The simulator projects the known pillar centers, adds Gaussian noise of standard deviation one pixel to the horizontal coordinate, and computes the depth of the near surface along the ray to the center. It does not perform image segmentation or resolve occlusions. We assume a depth standard deviation of $0.01\delta^2+0.002$ m, where $\delta$ is depth in meters. This is an assumed model, motivated by stereo depth error, not a calibrated D435i specification.

The camera measures depth along the optical axis. The range lies along the viewing ray. Given the image column $u$ and depth $\delta$, the bearing and center range are

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

The cosine converts optical-axis depth to distance along the ray. Adding the pillar radius then gives the center range in the ideal geometry.

We place the camera at the robot origin, facing forward. For a physical TurtleBot, use the camera-to-base transform discussed in the [D435i section](https://aegean.ai/aiml-common/lectures/sensor-models/d435i-landmark-features/index.md) before expressing the observation in the base frame.

```python theme={null}
# 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
        dist = np.hypot(xc, yc)
        depth = (dist - PILLAR_R) * xc / dist  # depth of the near surface on the ray to the center
        if depth < 0.3 or dist > 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)
        z.append(np.array([depth_meas / np.cos(bearing) + PILLAR_R, 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])}
```

![Output from cell 9](https://mintcdn.com/aegeanaiinc/Nwj-oOUe3UuRN9WC/aiml-common/lectures/rse/ekf-slam/images/cell_9_output_1.svg?fit=max\&auto=format\&n=Nwj-oOUe3UuRN9WC\&q=85\&s=788bc41e02186e316548a5edef5c5883)

At the example pose, only pillar 1 falls inside the camera field of view and accepted range. Its projected width is about 80 pixels. The simulated front end supplies its center column and near-surface depth. During this drive the camera sees at most one pillar at a time, whereas the lidar generally sees two or three. This difference in visibility will matter when the robot revisits previously mapped pillars.

## Choosing the measurement covariance

The EKF requires a covariance $R_t$ for the errors in the extracted range and bearing, not the raw sensor readings. To estimate the error statistics, we sample 400 random robot poses and compare detections with the corresponding simulated pillars using the known pose. Correspondences here are used for evaluation, not supplied to the SLAM filter.

```python theme={null}
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)


fe_err = {}
for name, sense in SENSORS.items():
    e = fe_err[name] = 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")
SIGMA_LIDAR = 0.0  # the same scans without beam noise
e0 = front_end_errors(lidar_landmarks)
SIGMA_LIDAR = 0.01
print(f" lidar without beam noise: range error mean {1000 * e0[:, 0].mean():+.1f} mm")
```

```output theme={null}
 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: 134 detections, range error mean -0.003 m, std 0.031 m; bearing error std 0.08 deg
        R_t uses std 0.050 m and 0.50 deg
```

```output theme={null}
 lidar without beam noise: range error mean +0.4 mm
```

The lidar range estimates are short by 7 mm on average, with standard deviations of 8 mm in range and 0.2° in bearing. Selecting the minimum range accounts for much of the negative bias: when we repeat the calculation without beam noise, the mean error becomes +0.4 mm. The camera has a larger range standard deviation (3.1 cm) but a smaller bearing standard deviation (0.08°).

For the initial SLAM runs we deliberately use larger standard deviations in $R_t$:

| Front end | Range | Bearing |
| - | -: | -: |
| Lidar | 2 cm | 0.75° |
| Camera | 5 cm | 0.5° |

These values are tuning choices, not new sensor calibration results. The filter uses a zero-mean additive observation-noise model and a local linearization. The measured range bias and range-dependent camera noise do not fit that model exactly. Increasing $R_t$ reduces the weight of each observation and widens the association gate, but does not remove the bias. We will compare this choice with the raw empirical standard deviations at the end.

## Running the filter

We now run the estimator with each sensor using identical wheel-noise realizations. Both start with the same pose estimate and empty map. The simulator records the true, dead-reckoned and estimated trajectories, as well as covariance and association diagnostics.

```python theme={null}
def simulate(sensor, seed=0, on_step=None, sense=None, R=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.
    sense and R replace the front end and the measurement noise of the named sensor, for the experiments at the end.
    """
    wheel_rng, sensor_rng = np.random.default_rng(seed), np.random.default_rng(seed + 100)
    sense = SENSORS[sensor] if sense is None else sense
    R = R_MEAS[sensor] if R is None else R
    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")
```

```output theme={null}
 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.133 m (dead reckoning 1.21 m), mean NEES 3.1, steps inside the 95% NEES bounds 99%
        loop closed at 67.8 s: position error 0.289 m before, 0.149 m after
```

![Output from cell 12](https://mintcdn.com/aegeanaiinc/Nwj-oOUe3UuRN9WC/aiml-common/lectures/rse/ekf-slam/images/cell_12_output_1.svg?fit=max\&auto=format\&n=Nwj-oOUe3UuRN9WC\&q=85\&s=741ee30ee6ceaf260204fc2c29d2d770)

Dead reckoning spirals inward and finishes 1.21 m from the true position. Both EKF runs stay much closer to the circular path. Each produces eight landmarks, with no wrong associations according to the simulator’s nearest-ground-truth scoring rule.

The lidar landmark ellipses are smaller in this run. The lidar observes pillars for longer and from more viewpoints, and the filter assigns it lower range noise. Differences in pose uncertainty at landmark initialization also contribute. This experiment changes several sensor characteristics together; it does not isolate the effect of field of view.

## Watching the estimation process

The recording places the lidar run on the left and the camera run on the right. The green arrow denotes the true pose and the orange arrow the estimate. Blue dots and ellipses show the estimated landmarks. Gray traces show dead reckoning, gold lines show current measurements, and the camera wedge marks its field of view. The lower plots track position error and map size.

The ellipses use a radius of two in Mahalanobis units. For a bivariate Gaussian this encloses about 86.5% of its probability mass, not 95%. Watch what happens to the pose and landmark ellipses when a previously mapped pillar becomes visible again.

[https://artifacts.aegeanai.com/rerun/viewer/0.41.0/index.html?url=https%3A%2F%2Fartifacts.aegeanai.com%2Frerun%2Fslam%2Fekf-slam-0.41.rrd](https://artifacts.aegeanai.com/rerun/viewer/0.41.0/index.html?url=https%3A%2F%2Fartifacts.aegeanai.com%2Frerun%2Fslam%2Fekf-slam-0.41.rrd)

*Press play or drag the time marker. Scroll to zoom; double-click to reset the view. The panel control at the upper right opens a panel full size.*

![Output from cell 15](https://mintcdn.com/aegeanaiinc/Nwj-oOUe3UuRN9WC/aiml-common/lectures/rse/ekf-slam/images/cell_15_output_1.svg?fit=max\&auto=format\&n=Nwj-oOUe3UuRN9WC\&q=85\&s=b5dc6e79f6dba6716166734c5799b786)

## Interpreting the trajectory error

In the camera run the position error grows to about 0.34 m during the first lap. A pillar leaves the field of view soon after it is observed; as the robot continues, new landmarks inherit uncertainty from its estimated pose. At 67.8 s a landmark from early in the drive is reobserved after a long absence. The position error drops from 0.289 m to 0.149 m in that step and briefly falls below 1 cm in subsequent steps. This is an example of loop closure. The joint covariance allows the update to correct other landmark estimates as well, although the magnitude of each correction depends on its cross-covariances and the innovation. The final camera position error is 0.133 m.

The lidar run closes the loop earlier, at 57.8 s; its position error falls from 0.085 m to 0.026 m. It has more simultaneous observations and stays below roughly 10 cm in this run.

The shaded region is $2\sqrt{\Sigma_{xx}+\Sigma_{yy}}$, twice the root-mean-square position uncertainty implied by the covariance. It is a useful scale for comparison with the Euclidean position error, but it is not a calibrated 95% confidence interval. A statistical consistency test follows below.

![Output from cell 16](https://mintcdn.com/aegeanaiinc/Nwj-oOUe3UuRN9WC/aiml-common/lectures/rse/ekf-slam/images/cell_16_output_1.svg?fit=max\&auto=format\&n=Nwj-oOUe3UuRN9WC\&q=85\&s=e1100d201100fa5d0a87317f5763cafc)

## What has the filter learned about the map?

The heatmap shows the final camera run's correlation matrix. The first three rows and columns represent the robot pose; each subsequent pair corresponds to a landmark's two coordinates. Many coefficients are near $+1$ or $-1$. This tells us that coordinate errors tend to vary together, but it does not identify the geometric motion responsible. To find that motion we examine the dominant eigenvector of the landmark covariance block.

```python theme={null}
def map_rotation_std(o, center):
    """Standard deviation (degrees) of a rotation of the whole map about center, from the landmark block of Sigma."""
    lm = o["mu"][3:].reshape(-1, 2)
    rot = np.column_stack([-(lm[:, 1] - center[1]), lm[:, 0] - center[0]]).ravel()  # landmark motion per radian
    return np.degrees(np.sqrt(rot @ o["Sigma"][3:, 3:] @ rot) / (rot @ rot))


s_0 = np.array([RADIUS, 0.0, np.pi / 2])
Sigma_1 = G(A_CMD, s_0) @ np.diag([1e-6] * 3) @ G(A_CMD, s_0).T + V(A_CMD, s_0) @ M @ V(A_CMD, s_0).T
print(f"heading std after the first prediction, when the first pillars are mapped: {np.degrees(np.sqrt(Sigma_1[2, 2])):.2f} deg")
for name, o in runs.items():
    P_m = o["Sigma"][3:, 3:]
    vals, vecs = np.linalg.eigh(P_m)
    lm = o["mu"][3:].reshape(-1, 2)
    rot = np.column_stack([-(lm[:, 1] - s_0[1]), lm[:, 0] - s_0[0]]).ravel()
    n = len(lm)
    gap = []  # standard deviation of the distance between neighboring landmarks
    for i in range(n):
        j = (i + 1) % n
        u = (lm[j] - lm[i]) / np.linalg.norm(lm[j] - lm[i])
        idx = [2 * i, 2 * i + 1, 2 * j, 2 * j + 1]
        gap.append(np.sqrt(np.r_[-u, u] @ P_m[np.ix_(idx, idx)] @ np.r_[-u, u]))
    print(f"{name:>6}: largest eigenvector holds {vals[-1] / vals.sum():.0%} of the landmark variance, "
          f"|cos| with a rotation about the start {abs(vecs[:, -1] @ rot) / np.linalg.norm(rot):.2f}; "
          f"map rotation std {map_rotation_std(o, s_0):.2f} deg; "
          f"neighbor distances std {1000 * min(gap):.0f} to {1000 * max(gap):.0f} mm")
```

```output theme={null}
heading std after the first prediction, when the first pillars are mapped: 0.75 deg
 lidar: largest eigenvector holds 99% of the landmark variance, |cos| with a rotation about the start 1.00; map rotation std 0.78 deg; neighbor distances std 2 to 3 mm
camera: largest eigenvector holds 89% of the landmark variance, |cos| with a rotation about the start 0.97; map rotation std 1.48 deg; neighbor distances std 11 to 17 mm
```

The largest eigenvector of the landmark covariance is closely aligned with a small rotation about the starting position. The projected rotation uncertainty is large compared with the uncertainty in distances between successive mapped landmarks. In these runs those distance standard deviations are a few millimeters for lidar and one or two centimeters for the camera. Thus the filter knows the shape of the map much better than its orientation in the chosen world frame.

Relative observations cannot by themselves determine the global translation and rotation of an unanchored map. The initial pose prior supplies a reference here, but the first prediction introduces heading uncertainty before any pillar is mapped. The heading standard deviation at that point is approximately 0.75°. The rotation uncertainty computed for the lidar map is close to that value, and the camera's is larger.

This illustrates the connection with the convergence results of Dissanayake et al. (Dissanayake et al., 2001). Those results rely on model assumptions; they do not guarantee that a nonlinear EKF will preserve this apparent uncertainty floor or that every future lap will improve the map consistently. Linearization can introduce spurious information in globally unobservable directions.

![Output from cell 18](https://mintcdn.com/aegeanaiinc/Nwj-oOUe3UuRN9WC/aiml-common/lectures/rse/ekf-slam/images/cell_18_output_1.svg?fit=max\&auto=format\&n=Nwj-oOUe3UuRN9WC\&q=85\&s=59b3085da882e57234a973757bc1b05d)

## Testing estimator consistency

A small position error is not enough to establish that the filter is working well. We also need to know whether its reported covariance is commensurate with its errors. Because this is a simulation, we can compute the normalized estimation error squared (NEES) of the pose:

$$
\epsilon_t=(\mu_{t,s}-s_t)^\top\Sigma_{t,ss}^{-1}(\mu_{t,s}-s_t),
$$

where the heading difference is wrapped. Under a correctly specified Gaussian error model, pose NEES follows a chi-squared distribution with three degrees of freedom and has expectation 3. The dashed lines are its central 95% interval. In the two example runs, 97% (lidar) and 99% (camera) of the values fall inside it; both have mean NEES about 3.1.

These are not 603 independent statistical tests: errors at successive times are correlated. Instead, run the simulation with $M=10$ independent noise seeds per sensor and at each time average the NEES across runs. Under the same Gaussian assumptions this average has distribution $\chi^2_{3M}/M$, with central 95% interval approximately $[1.68,4.70]$. The time series of this average remains correlated, so its fraction of excursions is descriptive. It is not an independent-sample hypothesis test.

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


def mc_nees(nees_runs):
    """Average NEES over runs at each step: its time mean and the shares of steps inside and above the 95% bounds."""
    lo_mc, hi_mc = chi2.ppf([0.025, 0.975], 3 * len(nees_runs)) / len(nees_runs)
    avg = np.mean(nees_runs, axis=0)
    return avg.mean(), np.mean((avg > lo_mc) & (avg < hi_mc)), np.mean(avg > hi_mc)


print()
for name in SENSORS:
    print("{:>6}: average NEES {:.2f}, steps inside the bounds {:.0%}, above {:.0%}".format(
        name, *mc_nees([o["nees"] for o in mc[name]])))
o = mc["lidar"][3]
print(f" lidar seed 3: heading error after the first step {np.degrees(o['err'][1, 2]):+.2f} deg, "
      f"filter std {np.degrees(np.sqrt(o['covs'][1, 2, 2])):.2f} deg")
print(" lidar without seed 3: average NEES {:.2f}, steps inside the bounds {:.0%}, above {:.0%}".format(
    *mc_nees([o["nees"] for k, o in enumerate(mc["lidar"]) if k != 3])))
bias = fe_err["lidar"][:, 0].mean()
debiased = [simulate("lidar", seed, sense=lambda s, p, rng: [z - np.array([bias, 0.0]) for z in lidar_landmarks(s, p, rng)])
            for seed in range(N_MC) if seed != 3]
print(" lidar without seed 3, range bias subtracted: average NEES {:.2f}, steps inside the bounds {:.0%}, above {:.0%}".format(
    *mc_nees([o["nees"] for o in debiased])))
print(" smallest covariance eigenvalue over all final beliefs: {:.1e}".format(
    min(np.linalg.eigvalsh(o["Sigma"]).min() for runs_ in mc.values() for o in runs_)))
```

```output theme={null}
seed  lidar: landmarks, wrong, final error (m), mean NEES | camera: landmarks, wrong, final error (m), mean NEES
   0   8,  0, 0.031,   3.1                     |  8,  0, 0.133,   3.1
   1   8,  0, 0.009,   3.2                     |  8,  0, 0.072,   1.7
   2   8,  0, 0.037,   4.2                     |  8,  0, 0.238,   5.6
   3   8,  0, 0.215,  14.1                     |  8,  0, 0.182,   4.8
   4   8,  0, 0.024,   4.1                     |  8,  0, 0.209,   7.9
   5   8,  0, 0.010,   4.0                     |  8,  0, 0.026,   1.5
   6   8,  0, 0.064,   3.3                     |  8,  0, 0.169,   3.0
   7   8,  0, 0.010,   4.6                     |  8,  0, 0.197,   2.8
   8   8,  0, 0.040,   4.2                     |  8,  0, 0.041,   2.4
   9   8,  0, 0.061,   3.8                     |  9,  0, 0.212,   3.0

 lidar: average NEES 4.88, steps inside the bounds 55%, above 44%
camera: average NEES 3.58, steps inside the bounds 98%, above 2%
 lidar seed 3: heading error after the first step -2.45 deg, filter std 0.75 deg
 lidar without seed 3: average NEES 3.85, steps inside the bounds 43%, above 34%
```

```output theme={null}
 lidar without seed 3, range bias subtracted: average NEES 1.71, steps inside the bounds 32%, above 0%
 smallest covariance eigenvalue over all final beliefs: 1.1e-06
```

## Results across noise realizations

There are no wrong associations under the scoring rule in the twenty baseline runs. Nineteen recover exactly eight landmarks; the camera run with seed 9 creates a duplicate of pillar 6. Under the assumed two-dimensional innovation model, a 99.9% gate rejects a correct association with probability 0.001 per candidate test. This does not predict the duplicate count exactly, but explains why occasional false rejections are possible even with a well-calibrated gate. All final position errors in these runs are below 0.24 m.

The camera's average NEES is 3.58, with 98% of time samples inside the pointwise bounds. The results are broadly compatible with the assumed uncertainty model for this experiment, not proof of consistency in general. For lidar the corresponding mean is 4.88, with 44% of time samples above the upper bound, indicating overconfidence.

Two diagnostic experiments help investigate the lidar result. Seed 3 has an unusually large heading error after the first prediction (about 2.5°, compared with a predicted standard deviation of 0.75°). The initial map inherits that error, which relative observations cannot directly distinguish from a rotation of the global frame. Excluding this seed reduces the average NEES to 3.85. After also correcting the measured range bias in the remaining nine seeds, it falls to 1.71. These are exploratory comparisons: dropping an outlier is not a valid substitute for evaluating the full ensemble. The bias correction and conservative $R_t$ may both contribute to the low NEES.

Over longer trajectories, ordinary EKF linearization can also create artificial information about the global heading (Julier & Uhlmann, 2002; Huang et al., 2010). This short, conservatively tuned experiment does not establish whether that effect is present or absent.

## Experiment: using the raw front-end noise

Finally, we replace $R_t$ with the diagonal covariance obtained from the empirical range and bearing standard deviations, without inflation, and repeat three seeds per sensor. The association rule is unchanged. Compare the number of mapped landmarks and the NEES with the baseline.

```python theme={null}
for name in SENSORS:
    R_raw = np.diag(fe_err[name].std(axis=0) ** 2)
    for seed in range(3):
        o = simulate(name, seed, R=R_raw)
        print(f"{name:>6}, seed {seed}: {(len(o['mu']) - 3) // 2} landmarks, {o['wrong']} wrong associations, mean NEES {o['nees'].mean():.1f}")
```

```output theme={null}
 lidar, seed 0: 18 landmarks, 0 wrong associations, mean NEES 38.0
```

```output theme={null}
 lidar, seed 1: 16 landmarks, 0 wrong associations, mean NEES 37.5
```

```output theme={null}
 lidar, seed 2: 17 landmarks, 0 wrong associations, mean NEES 53.0
camera, seed 0: 9 landmarks, 0 wrong associations, mean NEES 4.7
```

```output theme={null}
camera, seed 1: 12 landmarks, 0 wrong associations, mean NEES 3.0
camera, seed 2: 11 landmarks, 0 wrong associations, mean NEES 5.1
```

With the smaller measurement covariance, the lidar runs produce 16 to 18 landmarks and the camera runs 9 to 12. The room still contains only eight pillars. Under the scoring rule none of the measurements is assigned to a different physical pillar, but several pillars acquire duplicate map entries. The narrower innovation gate can reject a measurement from an existing pillar; the algorithm then initializes a second estimate of it.

The lidar mean NEES rises to 38 to 53, indicating severe overconfidence in these runs. The camera values remain between 3 and 5. Bias, state-dependent noise, linearization and gating can all affect these results; the experiment does not establish a single cause for the camera duplicates.

The practical lesson is that a landmark front end and a data-association rule must be evaluated together with the filter. A covariance measured from isolated sensor detections need not yield a well-calibrated estimator when used in a simplified observation model. Increasing $R_t$ can help, but modeling systematic errors and handling rejected observations explicitly are preferable to relying on inflation alone.

Key references: (Thrun et al., 2005; Dissanayake et al., 2001; Julier & Uhlmann, 2002; Huang et al., 2010)

## References

* Dissanayake, M., Newman, P., Clark, S., Durrant-Whyte, H., Csorba, M. (2001). *A solution to the simultaneous localization and map building (SLAM) problem*. IEEE Trans. Rob. Autom.
* Huang, G., Mourikis, A., Roumeliotis, S. (2010). *Observability-based rules for designing consistent EKF SLAM estimators*. Int. J. Rob. Res.
* Julier, S., Uhlmann, J. (2002). *A counter example to the theory of simultaneous localization and map building*. Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164).
* Thrun, S., Burgard, W., Fox, D. (2005). *Probabilistic Robotics*.

***

Found an error on this page? [Report an issue](https://github.com/aegean-ai/eaia-feedback/issues/new?template=page-issue.yml\&page=/aiml-common/lectures/rse/ekf-slam).


This documentation is built and hosted on [Mintlify](https://mintlify.com), a developer documentation platform.