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 (Thrun et al., 2005). 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).

A subscript selects part of a vector or a matrix.

\mu_{t,s}

is the pose part of

\mu_t

, its first three entries, and

\mu_{t,m}

is the landmark part.

\mu_{t,x}

\mu_{t,y}

and

\mu_{t,\theta}

are single entries of the pose. In the covariance,

\Sigma_{ss}

is the

3 \times 3

pose block and

\Sigma_{s,\cdot}

is the three pose rows, which hold the covariance of the pose with every entry of the state. Thrun, Burgard and Fox introduce this component notation in chapter 7, right after equation 7.8. Their state also stores a signature for each landmark, so it has

3N + 3

entries (their equation 10.7). The pillars in this section all look alike, so the signature row is dropped, and the state has

3 + 2N

entries. It 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_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

Correlation from a single update

A one-dimensional example shows where the cross terms come from. A robot and a landmark lie on a line. The robot’s position

x

has a prior mean of 1 m and a variance of 0.04 m², the landmark’s position

m

has a prior mean of 4 m and a variance of 0.01 m², and the two are uncorrelated. The robot measures the range

z = m - x

, so

H = [-1, \; 1]

, with noise variance

R = 0.01

m². The predicted range is 3 m and the measured range is 3.1 m, so the innovation is 0.1 m.

mu_1d = np.array([1.0, 4.0])  # robot x, landmark x (m)
Sigma_1d = np.diag([0.04, 0.01])
H_1d = np.array([[-1.0, 1.0]])  # z = m - x
R_1d = 0.01
nu_1d = np.array([3.1]) - H_1d @ mu_1d
S_1d = H_1d @ Sigma_1d @ H_1d.T + R_1d
K_1d = Sigma_1d @ H_1d.T @ np.linalg.inv(S_1d)
mu_1d_post = mu_1d + K_1d @ nu_1d
Sigma_1d_post = (np.eye(2) - K_1d @ H_1d) @ Sigma_1d
print("innovation variance S =", S_1d.round(3).item())
print("gain K =", K_1d.ravel().round(3))
print("posterior mean (x, m) =", mu_1d_post.round(3))
print("posterior covariance =\n", Sigma_1d_post.round(4))
print("correlation of x and m =", round(Sigma_1d_post[0, 1] / np.sqrt(Sigma_1d_post[0, 0] * Sigma_1d_post[1, 1]), 2))

innovation variance S = 0.06
gain K = [-0.667  0.167]
posterior mean (x, m) = [0.933 4.017]
posterior covariance =
 [[0.0133 0.0067]
 [0.0067 0.0083]]
correlation of x and m = 0.63

The innovation variance is

S = 0.06

m² and the gain is

K = (-0.667, 0.167)

. The update moves the robot back by 6.7 cm and the landmark forward by 1.7 cm, in proportion to their prior variances. The prior covariance had no off-diagonal entry, but the posterior has 0.0067 m², a correlation of 0.63. The measurement constrains only the difference

m - x

, so the errors that remain in

x

and

m

tend to have the same sign. EKF SLAM builds up the same structure between the pose and every landmark.

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 filter evaluates

h

and its Jacobian at the estimated pose and landmarks. The linearization is accurate only while the uncertainty of the posterior stays small, so that

h

is close to linear over the region where the true state is likely to be (Thrun, Burgard and Fox, section 10.2.1). 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

D^2_{ik} = (z^i_t - \hat z^k_t)^\top \Psi_k^{-1} (z^i_t - \hat z^k_t),

which Thrun, Burgard and Fox call

\pi_k

. The correspondence estimation section derived the maximum likelihood correspondence, which minimizes

D^2_{ik} + \ln\det\Psi_k

. Table 10.2 drops the

\ln\det\Psi_k

term and approximates the maximum likelihood choice by the Mahalanobis distance alone. The two agree when the candidate landmarks have similar innovation covariances. The algorithm 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, so

\alpha

acts as a

\chi^2

gate. The code uses

\alpha = 13.8

, the 99.9th percentile of the

\chi^2

distribution with 2 degrees of freedom. If the filter is consistent, a measurement of the correct 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_{t,x} + r \cos(\phi + \bar\mu_{t,\theta}) \\ \bar\mu_{t,y} + r \sin(\phi + \bar\mu_{t,\theta}) \end{bmatrix},

where

r

and

\phi

are the measured range and bearing. This works because a range and a bearing fix a point in the plane: the measurement function can be inverted. A camera that measures only bearing cannot place a landmark from a single view. It has to triangulate the landmark from two views taken from different poses, which is where the visual SLAM section starts. 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.9), 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 filter also uses only positive sightings. A pillar that should be in view but is not detected changes nothing, because a Gaussian belief cannot represent the absence of a landmark (Thrun, Burgard and Fox, section 10.2.1). The function ekf_slam below performs one prediction and then processes the measurements one at a time. Each measurement is gated and associated against the belief already updated by the measurements before it, as in Table 10.2. The covariance update is

(I - K_t H^k_t)\bar\Sigma_t

, also as in Table 10.2, followed by averaging the matrix with its transpose. The Joseph form

(I - K_t H^k_t)\bar\Sigma_t(I - K_t H^k_t)^\top + K_t R_t K_t^\top

gives the same result for this gain and protects positive definiteness against rounding errors, at about twice the cost. The short form is enough here: the final covariance of every run below is positive definite.

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 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_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)))

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. At that column the depth image gives the depth

\delta

of the pillar’s near surface, its distance measured along the camera’s optical axis. Stereo depth error grows with the square of the distance, so the code uses a standard deviation of

0.01 \delta^2 + 0.002

m, an assumed noise model. 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{\delta}{\cos\phi} + r_{\text{pillar}}.

The ray through the pillar’s center travels

\delta / \cos\phi

to the near surface, and the center lies one pillar radius beyond it. 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
        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])}

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)


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")

 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

 lidar without beam noise: range error mean +0.4 mm

The LIDAR’s range is 7 mm short on average, and its errors are 8 mm and 0.2° (one standard deviation). The short bias comes from the front end, which takes the shortest range in the cluster. The minimum of several noisy ranges is biased low. Without beam noise the bias is +0.4 mm, slightly long, because the beam closest to the pillar’s center does not hit its exact front. The camera is four times worse in range, 3.1 cm, because of the depth noise, but better in bearing, 0.08°, 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 and the gate becomes too tight. The repeated runs at the end of this section show what follows: measurements of known pillars fall outside the gate and start duplicate landmarks.

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, 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")

 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

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 for three reasons. 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. The camera’s range noise is larger, 5 cm against 2 cm in

R_t

. And the camera’s pose has drifted further by the time it first sees each pillar, so each landmark starts from a less certain pose.

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.34 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.149 m, and the following updates briefly bring it below 1 cm. The error then grows again on the second lap and ends at 0.133 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, and every landmark is also correlated with the pose. The code below finds which motion of the map these correlations leave uncertain.

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")

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

Almost all of the landmark uncertainty is one motion: a rotation of the whole map about the start pose. The distances between neighboring pillars are known to a few millimeters with the LIDAR and to one or two centimeters with the camera, while a pillar 5 m from the start moves by several centimeters under the rotation. The map is well known in relative terms, and much less well known in how it is turned. The rotation comes from the start. Dissanayake et al. (Dissanayake et al., 2001) proved that in EKF SLAM the covariance of every landmark can only shrink, and that in the limit it is bounded below by the covariance of the robot at the time the first landmark was observed. Here the start pose is known to 1 mm, but the first pillars are mapped after one prediction step, when the heading is already uncertain by 0.75°. The LIDAR map’s rotation uncertainty has already reached this floor. The camera map’s is about twice the floor after 1.6 laps, and further laps could only bring it closer.

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),

with the heading difference wrapped, 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.1. 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. These fractions alone do not show that the filter is consistent. The error at one step is almost the same as at the next, so the 603 steps of a run carry far fewer independent samples than 603. A test needs independent runs. The code below repeats both runs for

M = 10

seeds of the wheel and sensor noise and averages the NEES over the runs at each step. For a consistent filter this average follows

\chi^2_{3M} / M

, so 95% of the steps should fall between 1.68 and 4.70.

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_)))

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%

 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

The filter makes no wrong association in any of the twenty runs, and in nineteen of them it finds exactly eight landmarks. In the camera run with seed 9 it maps pillar 6 twice. A 99.9% gate rejects one in a thousand measurements of the correct pillar even when the filter is consistent, and the ten camera runs make thousands of detections, so an occasional duplicate is expected. The final error stays below 0.24 m, against more than a meter of dead reckoning in the run above. The camera filter is consistent: its average NEES is 3.58 and stays inside the bounds at 98% of the steps. The LIDAR filter is overconfident. Its average NEES is 4.88, above the upper bound at 44% of the steps, for two reasons. Seed 3 alone has a mean NEES of 14: its first wheel-noise draw turns the heading by 2.5°, more than three times the filter’s standard deviation, before any pillar is in the map. The whole map is then built in that rotated frame, and no later measurement can reveal the error. Without seed 3 the average drops to 3.85, but it is still above the bound at a third of the steps. The rest comes from the 7 mm range bias, which repeats in every scan. Subtracting it from every LIDAR range brings the average down to 1.71, below 3 because

R_t

is inflated. Longer runs show a further source of overconfidence. The EKF linearizes at its estimate instead of at the true state, and over many laps this lets it gain information about the global heading, which no measurement provides. Its heading covariance then shrinks below the true error (Julier & Uhlmann, 2002; Huang et al., 2010). These runs are 1.6 laps long and

R_t

is inflated, so they would not show the effect. The last experiment sets

R_t

to the raw front-end statistics from the start of this section, with no inflation, and runs three seeds per front end.

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}")

 lidar, seed 0: 18 landmarks, 0 wrong associations, mean NEES 38.0

 lidar, seed 1: 16 landmarks, 0 wrong associations, mean NEES 37.5

 lidar, seed 2: 17 landmarks, 0 wrong associations, mean NEES 53.0
camera, seed 0: 9 landmarks, 0 wrong associations, mean NEES 4.7

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 raw statistics the LIDAR filter maps most of the eight pillars twice, 16 to 18 landmarks in all, and the camera filter finds 9 to 12. No measurement goes to the wrong pillar. A measurement of a known pillar falls outside the tightened gate and starts a second copy of it, and from then on the two copies split the measurements. The LIDAR’s mean NEES rises to between 38 and 53, so its filter is far more confident than its error allows. The camera’s stays between 3 and 5, probably because its range bias is small next to its noise, so its duplicates come from the tighter gate alone.

Limits of EKF SLAM

The covariance has

(3 + 2N)^2

entries, and every update changes all of them. The Jacobian

H^k_t

has only five nonzero columns. An implementation that uses this computes

\Psi_k

for each candidate landmark in constant time and the update

\bar\Sigma_t - K_t \Psi_k K_t^\top

O(N^2)

, so a step with a bounded number of measurements costs

O(N^2)

. The code in this section ignores the sparsity. It forms the full

H^k_t

and multiplies dense matrices, which costs

O(N^3)

per measurement and is fine for eight landmarks. Even with sparsity, a few hundred landmarks is the practical limit. Sparse extended information filters (SEIF), FastSLAM, which gives each particle its own small landmark filters, and submaps, which split the map into small local maps, are the common ways past it (Thrun, Burgard and Fox, chapters 12 and 13). 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 relaxes the

O(N^2)

limit.

Lectures

Key references: (Thrun et al., 2005; Corke, 2023; Dissanayake et al., 2001; Huang et al., 2010)

References

Corke, P. (2023). Robotics, vision and control: Fundamental algorithms in python.
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.

State Estimation & Mapping

State Estimation

SLAM

Simultaneous Localization and Mapping

The EKF SLAM state

Correlation from a single update

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

​Correlation from a single update

​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

Correlation from a single update

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