EKF-SLAM: one covariance matrix for the robot and everything it has ever seen

SLAM is a chicken-and-egg problem. To know where the robot is you need a map to place it in; to build the map you need to know where the robot was when it looked. Neither half can be solved first.

EKF-SLAM’s answer is blunt and, once you see it, rather beautiful: stop treating them as two problems. Put the robot’s pose and the position of every landmark it has ever seen into one state vector, keep one joint covariance matrix over all of it, and run the Extended Kalman Filter from three posts ago on the whole thing. The map and the pose then correct each other through the off-diagonal blocks of that matrix (the robot–landmark and landmark–landmark correlations), which is the part of the machinery nobody ever draws. This post draws it.

The system this comes from was written in C++ with Eigen and OpenCV, in November 2018 (the source headers say “Created on: 03 Nov 2018”), for a 6-DoF camera pose and 3-D landmarks found with SURF, and run over RGB-D sequences from the TUM Freiburg1 benchmark. This is what it produced on the desk sequence, the reconstruction rendered from its own estimated trajectory:

A coloured 3-D point cloud of an office desk with two monitors, a keyboard and a chair, reconstructed from RGB-D frames placed along the EKF-SLAM trajectory, seen from above and slightly to one side. — The Freiburg1 desk scene, reconstructed by placing every depth frame on the pose the EKF-SLAM estimated for it. (`docs/pngs/res.png` in the repo.)

None of that runs in a browser: ROS, rosbag, PCL, OpenCV-contrib and half a gigabyte of bags per sequence. What does run is the same filter, reduced to two dimensions, in a room where you are the robot. That is the widget further down, and it is honestly the better teaching artefact. But first the machine, as built.

The pipeline

Figure 1 of the report is the whole algorithm as a flowchart, and it is worth tracing before any equation, because two of the boxes (Matrix Extraction and Diverged?) are where the engineering lives.

Figure 1 of the report, page 5, redrawn. One frame goes round the loop once. A frame without enough matches is skipped; a frame whose refinement diverges is thrown away whole; only a converged frame is allowed to add new landmarks.

Read it as: see, match, predict, extract, refine, check, grow. Each frame’s SURF features are matched against every descriptor the map has ever stored (that is what makes relocalisation possible after getting lost, and it is also what §6 later blames for the map’s decline). If enough matches survive, the filter predicts, pulls only the relevant rows and columns of the joint covariance out into a small matrix, runs an iterated correction on that, and either commits the result and admits new landmarks or discards the entire frame.

One state vector, one covariance

Eq. (2.11) of the report writes the state as the camera’s six degrees of freedom followed by three numbers per landmark:

\mathbf{x} = \begin{bmatrix} x & y & z & \gamma & \alpha & \beta & p^0_x & p^0_y & p^0_z & p^1_x & \cdots \end{bmatrix}^{\mathsf{T}}

with $(\gamma, \alpha, \beta)$ ZYX Euler angles. The report spends a paragraph on why Euler angles and not a rotation matrix or Rodrigues: a $3\times 3$ matrix is nine numbers for a three-degree-of-freedom quantity and the filter would happily drift it off the set of valid rotations, and with Rodrigues the Jacobian about an arbitrary current rotation is awkward. I still think that was the right call for a first implementation, even though the rotational error is exactly where this system loses to the published ones.

The joint covariance is $n\times n$ with $n = 6 + 3N$ . The code keeps it in pieces: PosStateVariance ( $6\times 6$ ), LandMarkStateVariance (up to MAX_LANDMARK_SIZE $\times$ MAX_LANDMARK_SIZE) and two cross blocks, PosLandMarkRowStateVariance and PosLandMarkColStateVariance. But it is one matrix, and the cross blocks are the ones this post is about.

In the widget the pose is $(x, y, \theta)$ and a landmark is $(p_x, p_y)$ , so $n = 3 + 2N$ ; with twenty landmarks that is a $43\times 43$ matrix, small enough to draw cell by cell.

The motion model is the identity

The first engineering decision, page 8 of the report. A constant-velocity model was tried (it adds six velocity states and predicts the camera onward each frame), and it made things worse on the RGB-D sequences. The motion of a hand-held camera is stochastic, and a prediction in the wrong direction on a frame that then fails to match anything is not corrected; it compounds, and the next frame matches even less. So $f$ became the identity and the entire burden moved to the correction step:

f(\mathbf{x}) = \mathbf{x}, \qquad \mathbf{J}_f = \mathbf{I}_n, \qquad \mathbf{x}^f_k = \mathbf{x}^a_{k-1}, \qquad \mathbf{P}^f_k = \mathbf{P}_{k-1} + \mathbf{Q}(\Delta t)

which is why PredictionStep, EKFSlam.cpp lines 234–238, is the shortest function in the file:

void EKFSlam::PredictionStep(double deltaTime){
	double dt_sq = (deltaTime*deltaTime);
	this->PosStateVariance += this->Q*dt_sq;
	this->max_squared_dist_to_travel += dt_sq*DELTA_DIST_THRESH_SQ;
}

Only the pose block of $\mathbf{P}$ grows. Landmarks do not move, so their rows of $\mathbf{Q}$ are zero and an unobserved landmark’s uncertainty is frozen until it is seen again.

Where $\mathbf{Q}$ comes from is the part I like. Rather than tune six variances by hand, the report starts from a physical limit: the fastest the camera plausibly moves, read off the ground truth, is about $0.9\ \text{m s}^{-1}$ . Take that to be the $0.8\sigma$ point of the per-frame displacement, and

0.8\,\sigma_{xyz} = 0.9\,\Delta t \;\Rightarrow\; \sigma^2_{xyz} \approx 1.3\,\Delta t^2, \qquad 0.8\,\sigma_{rpy} = 3.14\,\Delta t \;\Rightarrow\; \sigma^2_{rpy} \approx 16\,\Delta t^2

with $3.14\ \text{rad s}^{-1}$ the assumed rotational limit. The widget does exactly this in 2-D from the robot’s own limits ( $1.2\ \text{m s}^{-1}$ and $1.6\ \text{rad s}^{-1}$ ) at twenty steps a second; the “assumed top speed” slider is $v_{\max}$ , and $\mathbf{Q}$ is derived from it every step rather than set.

Caveat

The report and the code disagree about the final $\mathbf{Q}$ . Eq. (2.14), page 8, says the derived values were inflated and gives $\mathbf{Q} = \operatorname{diag}(0.02, 0.02, 0.02, 0.05, 0.05, 0.05, 0, \ldots)\,\Delta t^2$ . The constructor, lines 20–26, does something else:

this->Q = Eigen::MatrixXd::Identity(POSITION_VECTOR_SIZE, POSITION_VECTOR_SIZE);
//this->Q.block(0,0,3,3) *= 1.5;
//this->Q.block(3,3,3,3) *= 20;
this->Q.block(0,0,3,3) *= 0.9;
this->Q.block(3,3,3,3) *= 16;

That is $0.9$ and $16$ , with $1.5$ and $20$ commented out above them: a tuning history, not the matrix in the report, and the position value is the speed limit itself rather than the variance derived from it. The rotational value matches the derivation; the report’s $0.02$ and $0.05$ are, as far as I can tell, from an earlier run. The widget uses the derivation.

The observation, and its Jacobian

The sensor is a depth camera, so a matched feature comes with a 3-D position in the camera’s frame. Eq. (2.18) writes what the filter expects to see for a landmark $\mathbf{p}_i$ from a pose with translation $\mathbf{T}$ and rotation $\mathbf{R}$ :

h(\mathbf{p}_i) = \mathbf{R}^{-1}(\mathbf{p}_i - \mathbf{T}) = \mathbf{R}^{\mathsf{T}}(\mathbf{p}_i - \mathbf{T}) = \hat{\mathbf{q}}_i

The report considered converting this to range and bearing and chose not to: the Cartesian form is the simplest, and its Jacobian comes apart by inspection. $\partial h / \partial \mathbf{p}_i = \mathbf{R}^{\mathsf{T}}$ , $\partial h / \partial \mathbf{T} = -\mathbf{R}^{\mathsf{T}}$ , and the derivative with respect to the three angles is assembled from three matrices, one per angle, weighted by the offset to the landmark (Eq. 2.20):

\mathbf{J}_R(\mathbf{x})^i = \mathbf{J}_{Rx}\,(p^i_x - x) + \mathbf{J}_{Ry}\,(p^i_y - y) + \mathbf{J}_{Rz}\,(p^i_z - z)

where $\mathbf{J}_{Rx}$ , $\mathbf{J}_{Ry}$ , $\mathbf{J}_{Rz}$ (Eqs 2.21–2.23) are the element-wise derivatives of $\mathbf{R}^{\mathsf{T}}$ ‘s columns, written out in full in GetJRxyz, lines 80–88. I will spare you the nine-term trigonometric entries; the point is that they are constant for a frame and the per-landmark Jacobian is three of them scaled and summed. Stacked, one row-block per observed landmark, Eq. (2.24) is very sparse:

\mathbf{J}_h = \begin{bmatrix} -\mathbf{R}^{\mathsf{T}} & \mathbf{J}^0_R(\mathbf{x}) & \mathbf{R}^{\mathsf{T}} & \mathbf{0} & \mathbf{0} & \cdots\\ -\mathbf{R}^{\mathsf{T}} & \mathbf{J}^1_R(\mathbf{x}) & \mathbf{0} & \mathbf{R}^{\mathsf{T}} & \mathbf{0} & \cdots\\ \vdots & \vdots & \vdots & \vdots & \ddots & \\ -\mathbf{R}^{\mathsf{T}} & \mathbf{J}^m_R(\mathbf{x}) & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{R}^{\mathsf{T}} \end{bmatrix}

Every row-block touches the pose columns; each touches exactly one landmark’s columns. That shape is what makes the extraction step in the next section legitimate.

In two dimensions all of this collapses to one angle. With $\mathbf{R}(\theta) = \begin{bmatrix} c & -s \\ s & c \end{bmatrix}$ and $\mathbf{d} = \mathbf{p}_i - \mathbf{T}$ , the widget’s row-block for landmark $i$ is

\begin{bmatrix} -\mathbf{R}^{\mathsf{T}} & \begin{bmatrix} -s\,d_x + c\,d_y \\ -c\,d_x - s\,d_y \end{bmatrix} & \cdots & \mathbf{R}^{\mathsf{T}} & \cdots \end{bmatrix}

and the middle column is $\mathbf{J}_R(\mathbf{x})^i$ with only one term left in it.

Where $\mathbf{R}$ (the measurement noise) comes from is less principled, and the report says so. reprojectPointWithSigmaInterval, lines 100–114, back-projects the pixel at its depth and again at depth plus one raw unit, and calls half the difference a standard deviation:

void EKFSlam::reprojectPointWithSigmaInterval(double depth, const cv::Point2f &pt, Eigen::Vector3d &pt3d, Eigen::Vector3d &var3d){
	double Z = depth / SCALING_FACTOR;
	double Zp1 = (depth + 1.0) / SCALING_FACTOR;

	double n_x = (pt.x-CAMERA_C_X)/ CAMERA_FOCAL_LENGTH;
	double n_y = (pt.y-CAMERA_C_Y)/ CAMERA_FOCAL_LENGTH;
	double X = n_x*Z;
	double Y = n_y*Z;

	double sigma_x = std::abs(n_x*Zp1-X)/2.0;
	double sigma_y = std::abs(n_y*Zp1-Y)/2.0;
	double sigma_z = std::abs(Zp1-Z)/2.0;
	pt3d << X, Y, Z;
	var3d << sigma_x*sigma_x, sigma_y*sigma_y, sigma_z*sigma_z;
}

One raw depth unit is $1/5000$ m, so this gives a $\sigma_z$ of a tenth of a millimetre: a “2σ interval” on the quantisation of the depth image and nothing else. Page 9 admits that it ignores pixel quantisation entirely and that a mis-matched feature with a variance that small drags the pose after it. So getSmallerStateVectors adds a term the report describes as “experimentally set to $1\times10^{-1}$ times a feature’s match distance”: line 273, match_distance[loop]*1e-1+2e-6. A worse descriptor match is a less trusted position. In practice that term is the whole of $\mathbf{R}$ .

The matrix-extraction step

Page 10: the matrix inversion required to calculate the Kalman gain is an expensive operation, so the relevant matrix elements are extracted from the global matrices before processing. The gain (Eq. 2.9) needs $(\mathbf{J}_h \mathbf{P} \mathbf{J}_h^{\mathsf{T}} + \mathbf{R})^{-1}$ , and because $\mathbf{J}_h$ has non-zero columns only for the pose and the landmarks in this frame, that product depends only on the corresponding rows and columns of $\mathbf{P}$ . With $m$ landmarks matched, the matrix to invert is $3m\times 3m$ whatever the size of the map. getSmallerStateVectors, lines 256–267, gathers those blocks into Pk:

	for (int loop = 0; loop < number_of_observations; loop++){
		int state_offset_obs_pt_ind = obs_pt_ind+POSITION_VECTOR_SIZE;
		int startPointInState=offset_rw_obs_ind[loop];

		Pk.block(0, state_offset_obs_pt_ind, POSITION_VECTOR_SIZE, LANDMARK_LENGTH) << this->PosLandMarkRowStateVariance.block(0,startPointInState,POSITION_VECTOR_SIZE,LANDMARK_LENGTH);
		Pk.block(state_offset_obs_pt_ind, 0,LANDMARK_LENGTH, POSITION_VECTOR_SIZE)  << this->PosLandMarkColStateVariance.block(startPointInState,0,LANDMARK_LENGTH,POSITION_VECTOR_SIZE);

		int current_i2=POSITION_VECTOR_SIZE;
		for (int loop2 = 0; loop2 < number_of_observations; loop2++){
			Pk.block(state_offset_obs_pt_ind, current_i2,LANDMARK_LENGTH,LANDMARK_LENGTH) << this->LandMarkStateVariance.block(startPointInState,offset_rw_obs_ind[loop2],LANDMARK_LENGTH,LANDMARK_LENGTH);
			current_i2+=LANDMARK_LENGTH;
		}

That is exactly right for the inversion. It is not the whole update, and here is the thing I did not see in 2018. The full correction is $\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k\mathbf{J}_h)\mathbf{P}^f_k$ with $\mathbf{K}_k = \mathbf{P}^f_k\mathbf{J}_h^{\mathsf{T}}\mathbf{S}^{-1}$ , and $\mathbf{P}^f_k\mathbf{J}_h^{\mathsf{T}}$ has a non-zero row for every state that is correlated with an observed one, which after a few frames is every landmark in the map. Observing landmark 3 moves landmark 17, if the two were mapped from the same uncertain pose, and shrinks its covariance too. That is not a side effect of EKF-SLAM. It is EKF-SLAM: the mechanism by which re-seeing an old landmark tightens the whole map is precisely those rows.

UpdateStateAndVariance, lines 317–332, writes back the pose, the observed landmarks and the blocks between them, and nothing else:

	this->PosState = StateUpdate.segment(0,POSITION_VECTOR_SIZE);
	this->PosStateVariance << VarianceUpdate.block(0, 0, POSITION_VECTOR_SIZE, POSITION_VECTOR_SIZE);
	for (int loop = 0; loop < number_of_observations; loop++){
		int state_offset_obs_pt_ind = obs_pt_ind+POSITION_VECTOR_SIZE;
		int startPointInState=offset_rw_obs_ind[loop];

		this->PosLandMarkRowStateVariance.block(0,startPointInState,POSITION_VECTOR_SIZE,LANDMARK_LENGTH) << VarianceUpdate.block(0, state_offset_obs_pt_ind, POSITION_VECTOR_SIZE, LANDMARK_LENGTH);
		this->PosLandMarkColStateVariance.block(startPointInState,0,LANDMARK_LENGTH,POSITION_VECTOR_SIZE) << VarianceUpdate.block(state_offset_obs_pt_ind, 0,LANDMARK_LENGTH, POSITION_VECTOR_SIZE);
		int current_i2=POSITION_VECTOR_SIZE;
		for (int loop2 = 0; loop2 < number_of_observations; loop2++){
			this->LandMarkStateVariance.block(startPointInState,offset_rw_obs_ind[loop2],LANDMARK_LENGTH,LANDMARK_LENGTH) << VarianceUpdate.block(state_offset_obs_pt_ind, current_i2,LANDMARK_LENGTH,LANDMARK_LENGTH);
			current_i2+=LANDMARK_LENGTH;
		}
		this->LandMarkStateVector.segment(startPointInState,3) = StateUpdate.segment(state_offset_obs_pt_ind, 3);
		obs_pt_ind+=LANDMARK_LENGTH;
	}

Caveat

So the 2018 system extracted the sub-matrix for the inversion, which is exact, and then also confined the update to that sub-matrix, which is not. Landmarks out of view keep their old mean, their old variance and their old cross-covariances with a pose that has since moved on. It is a consistent-looking filter ( $\mathbf{P}$ stays symmetric and positive), but it cannot close a loop: revisiting the start of a sequence corrects the landmarks in view and leaves the rest of the map where drift put it. The correct version costs an $n \times 3m$ product per iteration and no extra inversion, and it is what the widget does by default. The covariance bookkeeping radio switches to the shipped behaviour so you can see the difference, and the fourth case of the widget’s self-check (checkSlam in slam.ts) is that exact experiment: a landmark mapped from the certain origin, a drift, two more landmarks mapped from the drifted pose, and then the first landmark re-seen. In exact mode the two unobserved landmarks tighten; in shipped mode they do not move at all.

A related shortcut: addPointToCurrentWorld, lines 124–129, inserts a new landmark with a variance of the pose’s own plus the reprojection variance plus $3\times$ the pose variance again plus $2\times10^{-4}$ , and zero cross-covariance to the pose that saw it. The landmark is born uncorrelated with the only thing that knows where it is. The exact augmentation uses the Jacobian of $\mathbf{p} = \mathbf{T} + \mathbf{R}\mathbf{z}$ with respect to the pose; the widget does that in exact mode and reproduces the inflated, uncorrelated insertion in the shipped mode.

The IEKF loop and its guard rails

The correction is iterated, as in post 18: re-linearise about the current answer, correct again, until the error stops improving. This is processImage, lines 372–408, with the debug prints and commented-out lines removed:

	for (int iteration =0; iteration < MAX_REFINEMENT_ITERS; iteration++){
		this->recalculateRelevantV(relevantX, ObservedValues, ek, Jh);
		Eigen::MatrixXd PfJht = Pk*Jh.transpose();
		Eigen::MatrixXd mtxToInvert = Jh*PfJht + Rk;
		Eigen::MatrixXd KkT = (mtxToInvert.transpose().householderQr().solve(PfJht.transpose())).transpose();
		double solution_error = (KkT*mtxToInvert - PfJht).norm() / PfJht.norm();
		if (solution_error > 3e-4){
			return;
		}

		StateUpdate = KkT*ek;
		VarianceUpdate = (IdentityMat - (KkT*Jh))*Pk;
		relevantX += StateUpdate;
		Pk = VarianceUpdate;

		curr_err_norm = ek.norm();
		if (prev_error_norm<1.02*curr_err_norm){break;}
		prev_error_norm = curr_err_norm;
	}
	curr_err_norm /=eq_obs_size;
	if (curr_err_norm>0.05){
		std::cout << "Error too high to add new features \n\n";
		return;
	}
	double deltadist = (this->PosState.segment(0,3) - relevantX.segment(0,3)).squaredNorm();
	if (deltadist>this->max_squared_dist_to_travel){
		std::cout << "I see the machine tried to warp through space. I will not allow this. \n\n";
		return;
	}
	this->max_squared_dist_to_travel = 0;
	this->UpdateStateAndVariance(relevantX, Pk, offset_rw_obs_ind);

Four things in there I would still do today.

No explicit inverse. The gain is found by solving $\mathbf{S}\mathbf{K}^{\mathsf{T}} = (\mathbf{P}\mathbf{J}_h^{\mathsf{T}})^{\mathsf{T}}$ with a Householder QR, the same idea post 18 recommended over np.linalg.inv.
Then the solve is checked. solution_error is the relative residual of that linear system. If it is above $3\times10^{-4}$ the frame is abandoned, because a gain that does not satisfy its own equation is a gain computed from an ill-conditioned $\mathbf{S}$ , and that means a bad match somewhere. It costs one matrix product.
The stopping rule is a plateau, not a tolerance. The loop breaks when the previous error is not at least 2% larger than the current one. Post 18 found that iterating the correction to convergence runs Newton’s method on the measurement equation and forgets the prior; stopping on a plateau after a pass or two is a pragmatic hedge against that, and it also means the covariance (note Pk = VarianceUpdate inside the loop, unlike the Python version, which held $\mathbf{P}^f$ fixed) is only shrunk a few times.
The warp gate. After the loop, the proposed pose is compared with the previous one. If it moved further than the speed limit allows since the last accepted frame, the frame is discarded with the message quoted above. A single confidently-wrong match can move the pose by metres; the physics says that cannot have happened, so the filter is not allowed to believe it.

Caveat

Three honest footnotes on that block. The report, page 10, says the refinement runs “up to a maximum of 8 times” and stops when “the previous error is not more than 1.1 times the current error”; the header says MAX_REFINEMENT_ITERS 10 and the loop says 1.02. The print before the second return says “too high to add new features”, but the return comes before UpdateStateAndVariance, so the whole frame is dropped, not just the new features. And the gate’s allowance is accumulated in PredictionStep as $\sum \Delta t^2 \cdot 18$ rather than $18 \cdot (\sum \Delta t)^2$ , so after $n$ skipped frames it grows like $\sqrt{n}$ while the robot’s possible displacement grows like $n$ . Past about twenty consecutive rejected frames a camera moving at full speed can never satisfy the gate again until it comes back. The widget accumulates elapsed time and squares it.

Data association, where SLAM actually lives

Everything above assumes each measurement arrives labelled with the landmark it belongs to. The labelling is the hard part. The header, EKFSlam.hpp lines 15–44, is the system’s entire tuning surface, and all of it is about that:

#define CAMERA_FOCAL_LENGTH 525
#define CAMERA_C_X 319.5
#define CAMERA_C_Y 239.5
#define SCALING_FACTOR 5000.0
#define MIN_KEYPOINTS 12

#define LANDMARK_LENGTH 3
#define POSITION_VECTOR_SIZE 6
#define MAX_LANDMARK_SIZE 30000

#define CLOSEST_MATCH_THRESH 0.15
#define CLOSEST_MATCH_THRESH_RATIO 0.68
#define MAX_MATCH_THRESH 0.24

#define MAX_LANDMARKS_IN_FRAME 40

#define CLOSEST_CANDIDATE_THRESH_FF 0.4
#define CLOSEST_CANDIDATE_THRESH 0.4
#define MIN_CANDIDATE_THRESH_FF 300
#define MIN_CANDIDATE_THRESH 250

#define CURRENT_STATE_VARIANCE_MULTIPLE_ADD 3
#define CURRENT_STATE_VARIANCE_ADDITION_ADD 2e-4

#define MAX_REFINEMENT_ITERS 10
#define DELTA_DIST_THRESH_SQ 18

SURF with the extended 128-element descriptor, brute-force matched against the whole map’s descriptor matrix with the two nearest neighbours returned. A match is accepted only if all three hold, DetectAndSeperateKeypoints lines 197–199:

		if (match[0].distance < MAX_MATCH_THRESH &&
			match[0].distance<match[1].distance*CLOSEST_MATCH_THRESH_RATIO &&
			match[0].distance+CLOSEST_MATCH_THRESH<match[1].distance) {

That is an absolute ceiling, Lowe’s ratio test, and an absolute gap to the runner-up. Then a geometric sanity check that the matched landmark is roughly the right distance away (line 184: a threshold of $24\,\operatorname{tr}\mathbf{P}_{xyz} + 0.04$ , loosening as the pose becomes less certain). A feature that matches nothing well (CLOSEST_CANDIDATE_THRESH) and has a strong detector response becomes a candidate; after a converged frame, candidates that are also dissimilar from each other are added, strongest response first, up to MAX_LANDMARKS_IN_FRAME minus the number matched. The report’s reason for the cap is exactly right: a map cluttered with weak, redundant points dilutes the certain ones.

A wrong match is worse than no match. No match costs a frame; a wrong match is a confident measurement of the wrong thing, applied through a gain that was computed believing it. That is what the report’s §6 blames for most of the system’s error, and it is why two of the four guard rails above exist.

Watch a landmark converge

Figure 36 is the report’s best conceptual figure, and it is what the widget’s ghost trail is built to reproduce. A fork of the code (EKFSlamGenCov/) dumps the landmark block of $\mathbf{P}$ to CSV every frame from frame 8 to 30, and a 36-line script eigen-decomposes one landmark’s $2\times 2$ block and draws its $1\sigma$ , $2\sigma$ and $3\sigma$ ellipses.

Six panels of three concentric ellipses each, for one landmark at t = 0, 5, 10, 15, 20 and 22. In the first panel the axes span about ±0.4 m; in every later panel they span about ±0.01 m and the ellipses have stopped shrinking. — Figure 36, page 29: the $1\sigma$ , $2\sigma$ , $3\sigma$ ellipses of one landmark at $t = 0, 5, 10, 15, 20, 22$ . Read the axes: the first panel spans roughly $\pm 0.4$ m, the rest about $\pm 0.01$ m. By $t = 5$ the outer ellipse is about 5 mm across and from then on it barely moves.

The shape of that curve is the running-mean law from post 17: the first observation halves the variance, the tenth removes a tenth of what is left, and the measurement noise floor is reached in a handful of frames. §6 puts the tension plainly: raise a landmark’s confidence too fast and one outlier match wrenches it; too slowly and the robot is forever navigating by landmarks it does not trust.

Play with it

A room, ten by eight metres, two dozen landmarks, and a robot with a sensor that reports the position of any landmark within its range and field of view, in its own frame, with Gaussian noise: the 2-D version of a depth camera with a perfect descriptor matcher. The filter never sees the controls; it sees measurements and ids.

A top-down room with twenty numbered landmarks. A robot's true path and the filter's estimated path run round the room; each mapped landmark carries a 95% ellipse with a trail of fainter, larger ghost ellipses behind it; a wedge shows the sensor's field of view with rays to the landmarks in it. Beside the room, the joint covariance matrix is drawn as a heatmap with a robot block top-left and a 2×2 block per landmark down the diagonal, with the off-diagonal correlations filled in. — InteractiveThe SLAM sandbox

Drag on the room to steer, or click it and use the arrow keys or WASD; the autopilot tours the room on a loop when you leave it alone. Things to try, roughly in order:

Watch the robot’s ellipse breathe. It grows every step by $\mathbf{Q}$ (the identity model knows nothing) and snaps shut the moment a mapped landmark is in view. Drive into a corner facing the wall and it inflates; turn round and it collapses in one frame. That is the whole of Eqs (2.16) and (2.10) as a picture.
Watch the ghosts. Each landmark leaves a trail of its past 95% ellipses, big and pale on first sight, then shrinking fast, then not at all: Figure 36, for every landmark at once. Switch the trail off if it gets busy.
Watch $\mathbf{P}$ . The heatmap is the joint covariance, log-scaled, with the robot’s $3\times 3$ block top-left and each landmark’s $2\times 2$ block down the diagonal. Everything off the diagonal is a correlation. When a landmark is first seen its row lights up against the robot block; as the robot moves on and sees others, the landmark–landmark blocks fill in too. Hover a cell to see which pair it couples and the correlation coefficient. The outlined blocks are the sub-matrix that entered this frame’s inversion: the matrix-extraction step, live.
Close a loop. Let the autopilot run the first lap. By the time it returns to the start, the map’s far corner was built on a pose that had drifted; when it re-sees a landmark from the beginning the status line reports a loop closure and the mean landmark $\sigma$ drops. Now switch the bookkeeping to as shipped in 2018, reset, and watch the same lap: the landmark in view tightens, the ones out of view do not, the correction never reaches the far side of the room, and in a room this sparse the map comes apart within a lap, with the error gate rejecting frame after frame. On the desk sequences, where almost every frame re-sees most of the map, the same shortcut held to a few centimetres (Table 1 below); it is the sparse, forward-looking case that exposes it.
Lose the robot. Two seconds of no measurements: the ellipse balloons, the estimate sits still while the real robot keeps going, and the first frame back has to close the gap. With the warp gate on, that first frame is sometimes rejected as impossible (the status line says so), and the second one, with a larger allowance, is accepted.
Mis-associate. The two mapped landmarks in view that lie closest together swap ids for a second. This is what a confused descriptor matcher does, and the response is the failure §6 describes: the map lurches, because the filter is confidently told that landmark 7 is where landmark 12 is. If the pair is far apart the warp gate catches it; if they are close, it goes straight through. Try it with the gate off.
IEKF passes at 1 is a plain EKF, and in this world it often cannot cope: with the identity model the heading innovation each step is large, a single linearisation about the prior does not fit it within the error gate, and the readout fills with rejected frames until the pose has drifted off. Two or three passes fix it. That is page 34’s “the linear approximation holds for small rotational movements only”, live, and the reason the C++ iterates at all.
Edit the map. Toggle Edit landmarks and click to add one, click one to remove it, or drag one to move it. The filter is not told, and you can watch a landmark’s estimate get dragged toward its new position by measurements that no longer agree with the map.
The score at sixty seconds is the mean distance between mapped landmarks and where they really are. Different driving strategies give different scores; a slow sweep that keeps a few known landmarks in view beats a fast dash every time.

Caveat

What the widget changes from the C++, beyond the dimension. The sensor’s noise is isotropic and constant, and $\mathbf{R}$ is set from the same slider rather than from a match distance. Data association is perfect unless you sabotage it. A correction runs with one matched landmark rather than twelve (MIN_KEYPOINTS), because a room has far fewer features than an image, and for the same reason a frame that sees only unmapped landmarks is allowed to add them, which the C++ never needed to do. The first frame that sees anything initialises the map before any prediction, exactly as initialise does, and it matters more than it looks: those first landmarks are born from a pose with zero covariance, and they are what pins the map’s frame down. Without that anchor the whole map and robot together drift in rotation while every residual stays small, which is how I lost a day to a filter that was “consistent” and 9° wrong. The error gate is $0.05 + 3\sigma$ rather than a fixed 0.05 m so it tracks the noise slider, and it measures the residual at the state the loop ended on. The C++ measures it at the start of its last pass, which with a single pass is the raw innovation and would reject every frame of a plain EKF. The C++ shrinks $\mathbf{P}$ on every pass of the loop (Pk = VarianceUpdate); the widget applies the covariance update once, at the final linearisation, which is the textbook IEKF and post 18’s lesson. The toggle Shrink P every pass restores the C++ behaviour, and the effect is easy to see: the reported pose $\sigma$ drops to a couple of centimetres while the error stays at tens. Over a lap my harness has the error at three to four times the reported $\sigma$ that way, against about twice with the single shrink. Neither is honest, because the identity model is not honest, but one is a lot less dishonest. And as described above, the exact-update mode and the Jacobian initialisation are what the C++ should have done, not what it did; the shipped mode is the port. The ellipses are 95% contours ( $2.45\sigma$ in 2-D), as in the last three posts.

Why not WASM

With twenty landmarks the joint state has 43 entries, the matrix that is actually inverted is $2m\times 2m$ for $m$ landmarks in view (typically $4\times 4$ to $12\times 12$ ), and the exact update is an $n\times 2m$ product. Measured in Node on the widget’s own scenario, three IEKF passes, exact mode: about 0.35 ms a frame at 20 landmarks, 0.7 ms at 30 and 5 ms at 60 (where a 120° wedge sees a dozen at a time and the inverted matrix is $24\times 24$ ); shipped mode, which never touches the rest of $\mathbf{P}$ , is a quarter of that. In headless Chrome, with a full map and the widget’s own drawing left out of the number, it reads 0.33 ms at 20 landmarks and 5.3 ms at 60; the readout prints the live figure.

The cost of the exact update grows as $n^2 m$ , so it is the $\mathbf{P}$ size that bites. Extrapolating, two hundred landmarks is a $403\times 403$ matrix and something like 25 ms a frame in JavaScript: the point at which a compiled kernel would start to matter, and a sentence about why rather than a build. It is also, in miniature, why the field moved on from EKF-SLAM. $\mathbf{P}$ is dense and quadratic in the map, and for a map of thousands of features it is not the inversion that kills you but the fact that every frame touches every entry. Sparse information filters and graph-based back-ends keep the same probabilistic model and throw away the dense matrix; that is the direction the “what I’d fix” list below is pointing at.

What I’d fix

§6 of the report, page 34, rephrased and with what I think now added.

Two descriptor sets, not one. Matching every frame against the entire map’s descriptors is what makes relocalisation possible, but the matrix grows every frame and so does the rate of false matches. Keep a recent set for tracking and fall back to the global one only when lost.
RANSAC on the matches. Geometric outlier rejection before the filter ever sees a measurement, instead of after the fact through the warp gate.
Loop closure and bundle adjustment. The report lists these as smoothing steps. In hindsight, the first thing to do is the exact covariance update: the loop closure the filter is supposed to give you for free was the very thing the extraction shortcut switched off.
A coarse motion estimate before the EKF. Rotation is the weak point: Table 2 on page 33 has the EKF at 10.6° RMSE on the rotation-heavy sequence against 2.2° for ORB-SLAM2, because the first-order expansion of $h$ holds for small rotations only. Feeding the correction a rough pose from somewhere else would put the linearisation point near the answer and cut the IEKF passes.
A particle filter for the multi-modality. When two features look alike the matcher switches between them, the posterior over that landmark is two bumps, and a Gaussian can only ever sit between them. §6 points at the particle filter from three months earlier as the tool for that job, and Table 3 of the report is a seven-row comparison of the two. The next post is the honest benchmark against RGB-D SLAM and ORB-SLAM2, and what those numbers say about all of the above.