Kalman, extended Kalman, or particle filter

October 18, 2026

The extended Kalman filter localised a robot in a loop of corridors by matching each lidar scan to the map. The same problem can be handed to the plain Kalman filter, which it does not quite fit, and to a particle filter, which it fits easily and at a cost. Here all three run side by side, on the same robot, the same wheel commands and the same scans, so whatever separates them is the filter.

The robot, the map and the lidar are the EKF’s: ninety beams, two centimetres of range noise, three metres of reach, a scan every tenth of a second. The top corridor has recesses, so a scan up there pins position and heading. The bottom corridor has none, and for ten metres along its middle nothing in reach faces along it. All three start on the true pose, a few centimetres unsure.

Sensing / state estimation

Three filters, one lidar

One robot drives the loop and three filters localise it from the same commands and the same scans. The Kalman filter and the EKF match each scan to the map and fold in the pose it lands on. The particle filter weighs every particle against the raw scan instead. The chart on the block plots each one's position error, and the readouts its time per scan. Kidnap it to see which of them can find the robot again.

Kalman—

Extended—

Particle—

Particle cost—

—

Particles 300

Beam σ, as the particle filter models it 5 cm

Kalman filter's model

Fresh random particles

One scan, read two ways

The Kalman filters cannot use a scan as it comes. Ninety ranges are a nonlinear function of the pose through every wall they hit, so they match it first: iterative closest point aligns the scan to the map from the filter’s own prediction, and the pose it lands on is folded in as a measurement of the whole pose, $H = I$ , with the information matrix of the fit,

\Lambda = \frac{1}{\sigma^2} \sum_k J_k^{\mathsf T} J_k,

standing in for the inverse of the measurement noise. In the bare corridor $\Lambda$ has nothing along the corridor, and the update leaves that direction to the prediction.

The particle filter never matches anything. Each particle ray casts the whole scan from where it stands and is weighted by how well those ranges agree with the real ones,

w_i \propto \prod_{k=1}^{90} \exp\!\left(-\frac{(z_k - \hat z_k(x_i))^2}{2\sigma_b^2}\right),

and the population is resampled when the weight has piled onto too few of them. A beam that reaches nothing reads as the lidar’s full reach, which is what the particle’s own ray cast reports when it reaches nothing too. In the bare corridor every particle along the corridor predicts the same scan, the weights stay level that way, and the cloud spreads along it on the wheels alone.

What a linear filter cannot hold

The wheels report a speed $v$ along the heading and a turn rate $\omega$ . Moving the pose by them,

p_x' = p_x + v \cos\theta\,\Delta t, \qquad p_y' = p_y + v \sin\theta\,\Delta t,

puts the cosine of a state into the model, and a Kalman filter’s model has to be a matrix. There are two ways to keep it one.

The first gives up the speed. Track position and a world-frame velocity with the constant-velocity model of the Kalman filter post, in both axes, and heading by integrating the turn rate, which is linear:

x = \begin{bmatrix} p_x & p_y & v_x & v_y & \theta \end{bmatrix}^{\mathsf T}, \qquad p' = p + v\,\Delta t, \quad v' = v, \quad \theta' = \theta + \omega\,\Delta t.

Velocity is never measured. The filter infers it from successive fixes, and its process noise $q$ has to be large enough to let the velocity swing through a right angle at every corner, so the velocity it holds is only as good as the last few seconds of fixes. In this building the bare corridor starts about two metres after a corner. The filter enters it carrying a speed that was off by anywhere from under one centimetre a second to twenty-four across six runs, and then nothing corrects it for ten metres. A few centimetres a second becomes a metre by the far end, ICP from a metre out pairs the scan with the wrong walls, and the filter does not come back. It was lost, more than half a metre out, in five runs of six. No setting of $q$ saves it: anywhere from 0.03 to 10, it was lost in at least eight runs of ten.

The second keeps the speed and gives up the heading’s doubt. Take the current heading estimate as a known number for the length of one step, and the displacement $v\,\Delta t\,(\cos\hat\theta, \sin\hat\theta)$ is an input like any other. The mean then moves exactly as the EKF’s does, and the covariance moves with $F = I$ ,

P' = P + L M L^{\mathsf T},

which is the EKF’s $P' = F P F^{\mathsf T} + L M L^{\mathsf T}$ with the third column of $F$ deleted. That column was the whole of the nonlinearity: it is how a doubt in heading becomes a doubt in position across the direction of travel. Pick wheel speed along its own heading to run this one.

It tracks the EKF to under a millimetre everywhere the scan pins the heading. Through the bare corridor the two drift apart by up to three centimetres, which is well inside the error both of them carry there, and neither is the better for it. The reason is in the EKF post’s own table. The linearisation costs nothing measurable at a fraction of a degree of heading doubt, and the scan holds the heading to 0.08°, so the term the extended filter adds is multiplying something that is almost zero. Here the extension buys nothing. It would on a robot whose heading the sensor sees badly, or rarely: a doubt of several degrees carried over a few metres is the case its third column exists for.

Precision

Root mean square position and heading error over six runs of ninety seconds, a little over a lap each, split by where the robot was, with the particle filter’s beam σ at its default of five centimetres. Ranges are across the six runs. The fresh particles of the kidnapping section below make no measurable difference to these.

Filter	Top corridor, scan pins everything	Bare corridor, blind along it
Kalman, constant velocity	lost in 5 of 6 runs	lost in 5 of 6 runs
Kalman, heading taken as known	5.8 to 6.5 mm, 0.08°	4 to 10 cm
Extended Kalman	5.8 to 6.5 mm, 0.08°	4 to 10 cm
Particle, 30	12 to 17 mm, 0.19°	5 to 10 cm
Particle, 100	7 to 9 mm, 0.11°	4 to 7 cm
Particle, 300	6 to 8 mm, 0.09°	4 to 11 cm
Particle, 1000	6 to 7.5 mm, 0.08°	2 to 7 cm

Where the scan sees everything, every filter that is not lost converges on the same few millimetres. That floor belongs to the lidar, not to any filter: ninety beams with two centimetres of noise each say where the robot is to about that much, and a filter can only fail to reach it. The particle filter falls short of it with too few particles, because its estimate is a weighted average of a handful of guesses, each scattered by a centimetre a step of motion noise, and a handful of guesses does not average out a centimetre. A hundred gets within a couple of millimetres of the floor. Past three hundred, more buys nothing.

In the bare corridor the scan says nothing about distance along it, every filter is on its wheels that way, and the wheels are the same wheels. The errors are the same size for all three, and which one does best in a run is a matter of that run’s draws.

Runtime

Milliseconds per scan on the machine these were measured on, and the same as a multiple of the EKF. The readouts on the tool measure them again in the browser you are reading this in; the ratios move less than the milliseconds.

Filter	Per scan	Against the EKF
Kalman, either model	0.46 ms	1
Extended Kalman	0.46 ms	1
Particle, 30	0.61 ms	1.3
Particle, 100	1.8 ms	4
Particle, 300	5.6 ms	12
Particle, 1000	18.7 ms	41

The Kalman filters all cost the same because none of them is where the time goes. A predict and an update on a $3 \times 3$ covariance take about ten microseconds. ICP takes the other 98 percent: two or three iterations, each pairing ninety returns with the nearest of twenty walls. The constant-velocity filter’s $5 \times 5$ algebra is lost in the same rounding.

The particle filter’s time is ray casting, ninety beams against twenty walls for every particle, every scan, and it is linear in the particle count: about nineteen microseconds a particle here. Thirty particles cost about what one ICP match does. To match the EKF’s precision takes a hundred or more, and four to twelve times its time.

The beam noise it assumes

The particle filter’s $\sigma_b$ is the sensor as the filter models it, the beam σ slider. The real noise is two centimetres and the default is five, and the reason is in the width of the cloud rather than the error.

Beam σ, 100 particles	Top corridor error	Cloud’s own spread	Weight on
2 cm, the real noise	6 to 9 mm	4 mm	6% of the particles
5 cm	7 to 9 mm	8 mm	19%
30 cm, 300 particles	15 to 17 mm	18 mm	54%

The last column is the effective sample size after weighing, as a fraction of the population. Modelled at the real noise, ninety beams make the likelihood so sharp that each scan leaves the weight on six particles in a hundred, and the next resample rebuilds the population from those few. It still tracks, and its error is if anything a millimetre lower. But the cloud is copies of a few poses, and its spread is half the error it actually has: drawn on the map, it claims to know the pose twice as well as it does. At five centimetres the spread and the error agree, and a cloud that is as wide as it is wrong is worth more downstream than a millimetre.

Modelled much more cautious than that, every particle within a few centimetres scores about the same, the scan stops tightening the cloud, and the estimate is the average of a cloud the size of the error: 15 to 17 mm at thirty centimetres, against six to eight at five. More particles do not bring it down. At twenty centimetres it was 12 to 13 mm with a thousand particles and the same with three thousand.

Kidnapping

Everything so far started the filters on the truth, which is the EKF’s home ground. Kidnap it picks the robot up and puts it down somewhere else on the loop, still driving, and tells no filter. Ten kidnaps, thirty seconds into a run, each a jump of between 1.5 and 13.6 metres, then five minutes to find the robot again, counted as five straight seconds within twenty centimetres:

Filter	Found again	How long it took
Extended Kalman, either Kalman with it	2 of 10	32 and 33 s
Particle, no fresh particles	4 of 10	79 to 143 s
Particle, 1% fresh every scan	7 of 10	12 to 78 s
Particle, 5% fresh every scan	9 of 10	half inside 18 s, slowest 266 s
Particle, 5% fresh, beam σ 30 cm	10 of 10	half inside 26 s, slowest 233 s

The EKF has nowhere to look but where it was. Its next scan is matched from a prediction metres from the robot, ICP pairs the returns with whatever walls are near that prediction, and the filter carries on tracking a place that looks roughly like where the robot is. Its two recoveries are the same accident twice: a jump of under four metres straight up the right-hand corridor. The EKF went on tracking the robot at that offset, round the corner and along the top corridor, whose recesses are four metres apart and so nearly agree with a scan shifted by that much, until the far corner of the loop disagreed and pulled the match into the right basin half a minute later. The Kalman filter with its heading taken as known did the same within two seconds, since it reads the scan the same way.

The particle filter’s population is where it looks, and after a resample every particle is near the old pose. With nothing added it is in the EKF’s position with a wider net: it recovered only from four jumps of three to four metres, and only after a minute or two of motion noise had spread the cloud far enough to touch the robot. The fix is the one from the particle filter post: replace a few particles every scan with draws from anywhere on the map, the fresh random particles setting, five percent by default. While the filter is right they score nothing and vanish at the next resample, and they cost nothing measurable here, 7.3 mm in the top corridor against 7.1 without them and the same time per scan. Once it is wrong, the first fresh particle to land near the robot outscores the whole cloud and the next resample copies it everywhere. Five percent found the robot in nine kidnaps of ten, half of them within twenty seconds.

The one it missed shows what a sharp likelihood costs. The building is symmetric under a half turn except for the recesses, and the particle filter settled on the mirror image of the robot, diagonally across the loop and facing the other way. That pose is wrong only in the few beams that reach a recess. A fresh particle near the truth beats it only if it is close enough for all ninety beams to agree within five centimetres, and one that lands thirty centimetres off loses on every beam at once. Modelled at thirty centimetres instead, a near miss scores well enough to be copied and then refined, and all ten kidnaps were found. That is the same σ that cost a factor of two in tracking precision above. A real localiser wants both: the sharp model to track with and the forgiving one to search with, which is why practical ones raise the injection rate when the scans start scoring badly and lower it again once found.

So the usual pairing in a real localisation stack is both: a particle filter, or something like it, to find the robot, and a scan matcher with a Kalman filter behind it to follow it cheaply once found.

Explore further

Watch the chart through the shaded stretch, the bare corridor. All three error lines climb together, then fall together as the far wall comes into reach. Unless the constant-velocity Kalman filter went in with the wrong speed, in which case its line leaves the top of the chart and stays there.
Switch the Kalman filter to wheel speed along its own heading. Its marker sits on the EKF’s for the rest of the run, and so does its error.
Drag particles down to 10. The cloud thins to a few specks, the particle filter’s error in the top corridor goes to two centimetres or so, and its time per scan drops below the EKF’s.
Drag particles to 2000. The cloud is a smear the width of the robot, the error is no better than at 300, and the cost readout climbs past fifty times the EKF. Depending on the machine, the whole simulation may start to run slower than the clock.
Press kidnap it, and watch the cloud. Fresh particles speckle the whole plan every scan, and when one lands close enough to the robot a second cluster grows there and takes the weight at the next resample. The EKF’s marker carries on along the corridor it thought it was in.
Set fresh random particles to none and kidnap it again. The particle filter is now as stuck as the EKF.
Drag beam σ to 30 cm with 300 particles. The cloud swells in every corridor, not only the bare one, and the particle filter’s error stays a centimetre or more where the other two are at a few millimetres.