From a grid to two numbers
The histogram localiser of the previous lesson stored one probability per cell, which is fine for a rail of 40 cells and hopeless for a drone whose state has position, velocity, orientation and sensor biases: fifteen numbers or more. The way out is to assume the belief has a fixed shape and keep only the numbers that describe it. If the shape is a Gaussian (a bell curve), the whole belief is a mean (the best guess) and a covariance (how unsure, and in which directions), and if the robot's models are linear, the Bayes filter's sums become matrix products. That is the Kalman filter.
It was published by Rudolf Kálmán in 1960, and within a few years an extended version, worked out largely by Stanley Schmidt's group at NASA's Ames Research Center, was navigating the Apollo spacecraft to the Moon. Today Kalman filters run in phones (fusing GNSS with motion sensors), drone autopilots, cars, aircraft and nearly every mobile robot. It is the same predict–correct loop as the observer of State-space models and LQR, , with one difference: the Kalman filter computes the gain from the noise, step by step, and carries an honest measure of its own uncertainty.
Fusing two noisy numbers
Two sensors measure the distance to a wall. A lidar says 2.0 m and has a standard deviation of 0.8 m (it is a cheap one, and the wall is dark); an ultrasonic sensor says 3.0 m with a standard deviation of 0.5 m. What should the robot believe?
Neither average nor "trust the better one" is right. The best combination weights each reading by its precision, the inverse of its variance: the more certain reading pulls harder. Writing and ,
Rearranged, this is the form the Kalman filter uses: start from the first estimate and move towards the second by a fraction of the disagreement,
Here , so m and m: closer to the better sensor, and more certain than either. If the first number is a prediction rather than a reading, this is exactly one correct step of a Kalman filter, and is the Kalman gain.
Gaussians, linear maps and the filter
Definition— Multivariate Gaussian
A random vector is Gaussian with mean and covariance (symmetric, positive definite), written , if its density is
The diagonal of holds the variances of the components; the off-diagonal entries say how errors in different components move together. Its level sets are ellipses (ellipsoids), whose axes are the eigenvectors of .
The quantity in the exponent is the squared Mahalanobis distance: distance measured in standard deviations, along each direction. It will return as the consistency test.
Theorem— Linear maps preserve Gaussians
If and , then . If, moreover, is independent of , then .
Proof
The mean is linear: . The covariance: . Covariances of independent terms add. That the result is again Gaussian follows from a change of variables in the density (or from characteristic functions); Lumenary Math's Probability theory has the details.
∎
This theorem is the reason the Kalman filter exists. Suppose the state evolves and is observed linearly, with Gaussian noise:
is the process noise covariance (how much the world departs from the motion model in one step) and the measurement noise covariance (how noisy the sensor is). Then a Gaussian belief stays Gaussian forever, and the Bayes filter only has to track its mean and covariance .
Definition— The Kalman filter
Predict:
Correct: with the reading ,
is the innovation (the surprise: what the sensor said minus what the filter expected), its predicted covariance, and the gain.
Compare the one-dimensional fusion above: there , , , and . The matrix gain is the same ratio, "my uncertainty divided by the total uncertainty of the disagreement", with carrying the correction from measurement space back to state space. It also spreads a correction to quantities the sensor never sees: a position-only reading corrects velocity through the off-diagonal terms of , which remember that a velocity error causes position errors.
Theorem— Optimality of the Kalman filter
For the linear–Gaussian model above, with a Gaussian prior, the Kalman filter's is exactly the Bayes filter's belief . If the noises are not Gaussian but have the stated means and covariances, is still the best estimate among all linear functions of the data, in the sense of minimum mean squared error.
The proof multiplies two Gaussian densities (prediction and likelihood) and completes the square in the exponent; the one-dimensional fusion above is the scalar case, and Särkkä and Svensson's book in the references gives the general one in a page.
Tracking at constant velocity, and tuning Q and R
A delivery robot waits to cross a cycle path and must predict when a cyclist will pass. Its lidar's person-and-cyclist detector reports the cyclist's position along the path ten times a second, with a standard deviation of 0.3 m. The state is position and velocity, ; the motion model is "velocity stays the same, apart from random accelerations":
This is what a random acceleration of intensity (in ), integrated over one step, does to position and velocity; it has one number to tune instead of three. The simulation generates a cyclist with , then runs filters with five different values of to see what mistuning does.
import numpy as np
dt, steps = 0.1, 400 # 10 Hz for 40 s
F = np.array([[1.0, dt], [0.0, 1.0]]) # constant velocity: p <- p + v dt
H = np.array([[1.0, 0.0]]) # we measure position only
def Q_of(q): # random acceleration of intensity q (m^2/s^3)
return q * np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]])
# Simulate a person strolling along a station platform, seen by a lidar person-detector.
rng = np.random.default_rng(1)
q_true, sigma_z = 0.1, 0.30
truth = np.zeros((steps, 2))
x = np.array([0.0, 1.2])
for k in range(steps):
x = F @ x + rng.multivariate_normal([0, 0], Q_of(q_true))
truth[k] = x
z = truth[:, 0] + rng.normal(0, sigma_z, steps)
def kalman(q, r):
x, P = np.array([z[0], 0.0]), np.diag([r, 4.0]) # start at the first reading, unsure of speed
Q, R = Q_of(q), np.array([[r]])
est, nis = np.zeros((steps, 2)), []
for k in range(steps):
x, P = F @ x, F @ P @ F.T + Q # predict
y = z[k] - H @ x # innovation
S = H @ P @ H.T + R # its predicted covariance
K = P @ H.T @ np.linalg.inv(S) # Kalman gain
x, P = x + K @ y, (np.eye(2) - K @ H) @ P # correct
est[k] = x
nis.append(float(y @ np.linalg.inv(S) @ y))
return est, np.mean(nis[50:])
rms = lambda e: float(np.sqrt(np.mean(e[50:] ** 2))) # skip the first 5 s while the filter settles
print(f"raw readings: position RMS error {rms(z - truth[:, 0]):.3f} m")
print(f"{'q (filter)':>10} {'pos RMS (m)':>12} {'vel RMS (m/s)':>14} {'mean NIS':>9}")
for q in [0.001, 0.01, 0.1, 1.0, 10.0]:
est, nis = kalman(q, sigma_z**2)
print(f"{q:10.3f} {rms(est[:, 0] - truth[:, 0]):12.3f} {rms(est[:, 1] - truth[:, 1]):14.3f} {nis:9.2f}")
est, nis = kalman(0.1, 0.1**2)
print(f"right q, but R assumes sigma 0.1 m: pos RMS {rms(est[:, 0] - truth[:, 0]):.3f} m, mean NIS {nis:.2f}")The raw readings are off by 0.31 m (RMS). The filter tuned with the true is off by 0.16 m, half as much, and it also estimates the speed, which no sensor measured, to within 0.3 m/s.
- too small (0.001): the filter believes cyclists hardly change speed, trusts its prediction, and lags behind every acceleration. Its error, 0.60 m, is worse than the raw readings.
- too large (10): the filter believes anything can happen, trusts each reading, and passes its noise straight through; the velocity estimate, a difference of noisy positions, is worst of all.
- too small: claiming the sensor is three times better than it is makes the filter chase noise; the position error rises only slightly (0.163 m), but the last column gives it away.
That last column, the mean normalised innovation squared (NIS), is the filter checking itself. If the model is right, each innovation is Gaussian with covariance , so follows a distribution with as many degrees of freedom as the measurement has components, here one, and averages 1. A filter whose NIS averages 5.4 is constantly more surprised than it expects to be (it is overconfident); one averaging 0.87 is a little underconfident. With the true , the mean NIS is 1.09. Unlike the RMS error, the NIS needs no ground truth, so it can be monitored on a real robot.
The extended Kalman filter: landmarks by range and bearing
Real robots are not linear. A wheeled robot's motion involves and (Wheeled robots and odometry), and a sensor that measures the range and bearing to a landmark involves a square root and an arctangent. The extended Kalman filter (EKF) keeps the Gaussian belief and linearises the models around the current estimate, using their Jacobians.
Definition— Extended Kalman filter
For and : predict with and , where at ; correct with and the Kalman equations using at .
With , and ,
The robot below drives a 5 m circle for 90 s using wheel odometry, which has a 3% scale error and a small turn bias, as real odometry does. Once a second it measures range (standard deviation 0.1 m) and bearing (2°) to every one of five landmarks within 8 m. The odometry readings are the EKF's control input ; their noise enters the state through the Jacobian , giving . The filter is then run a second time believing the sensor is five times better than it is.
import numpy as np
dt, steps = 0.1, 900 # 90 s at 10 Hz
landmarks = np.array([[-5.0, 0.0], [1.0, 4.0], [-11.0, -3.0], [-4.0, 7.0], [-2.0, -6.0]])
sig_v, sig_w = 0.05, 0.03 # odometry noise per step (m/s, rad/s)
sig_r, sig_b = 0.10, np.radians(2.0) # range-bearing sensor noise
wrap = lambda a: (a + np.pi) % (2 * np.pi) - np.pi
def move(s, v, w): # unicycle motion model
x, y, th = s
return np.array([x + v * dt * np.cos(th), y + v * dt * np.sin(th), wrap(th + w * dt)])
def measure(s, lm): # range and bearing to one landmark
dx, dy = lm[0] - s[0], lm[1] - s[1]
return np.array([np.hypot(dx, dy), wrap(np.arctan2(dy, dx) - s[2])])
def run_ekf(R_assumed):
true = np.array([0.0, 0.0, np.pi / 2]); mu, P = true.copy(), np.diag([0.01, 0.01, 0.001])
dead = true.copy()
rng = np.random.default_rng(5) # the same noise for every run
err_dr, err_ekf, nis = [], [], []
for k in range(steps):
v, w = 0.5, 0.1 # commanded: a 5 m circle
true = move(true, v, w)
vo = 1.03 * v + rng.normal(0, sig_v) # odometry: 3% scale error...
wo = w + 0.004 + rng.normal(0, sig_w) # ...and a small turn bias
dead = move(dead, vo, wo)
# --- EKF predict ---
th = mu[2]
Fx = np.array([[1, 0, -vo * dt * np.sin(th)], [0, 1, vo * dt * np.cos(th)], [0, 0, 1]])
Fu = np.array([[dt * np.cos(th), 0], [dt * np.sin(th), 0], [0, dt]])
mu = move(mu, vo, wo)
P = Fx @ P @ Fx.T + Fu @ np.diag([sig_v**2, sig_w**2]) @ Fu.T
# --- EKF correct: once a second, every landmark within 8 m ---
if k % 10 == 9:
for lm in landmarks:
z = measure(true, lm) + rng.normal(0, [sig_r, sig_b])
if z[0] > 8.0:
continue
zhat = measure(mu, lm)
dx, dy = lm[0] - mu[0], lm[1] - mu[1]; q = dx**2 + dy**2
H = np.array([[-dx / np.sqrt(q), -dy / np.sqrt(q), 0], [dy / q, -dx / q, -1]])
y = z - zhat; y[1] = wrap(y[1])
S = H @ P @ H.T + R_assumed
K = P @ H.T @ np.linalg.inv(S)
mu = mu + K @ y; mu[2] = wrap(mu[2])
P = (np.eye(3) - K @ H) @ P
nis.append(float(y @ np.linalg.inv(S) @ y))
err_dr.append(np.hypot(*(dead[:2] - true[:2])))
err_ekf.append(np.hypot(*(mu[:2] - true[:2])))
return np.array(err_dr), np.array(err_ekf), np.array(nis), P
R_true = np.diag([sig_r**2, sig_b**2])
dr, ekf, nis, P = run_ekf(R_true)
print(f"after 90 s: dead reckoning error {dr[-1]:.2f} m (max {dr.max():.2f}),"
f" EKF error {ekf[-1]:.3f} m (max after 10 s {ekf[100:].max():.3f})")
print(f"EKF's own 1-sigma position uncertainty at the end: {np.sqrt(P[0, 0]):.3f} m (x), {np.sqrt(P[1, 1]):.3f} m (y)")
print(f"honest R: mean NIS {nis.mean():.2f} (ideal 2.00), fraction below 5.99: {np.mean(nis < 5.99):.2f} (ideal 0.95)")
_, ekf_bad, nis_bad, _ = run_ekf(np.diag([0.02**2, np.radians(0.4)**2]))
print(f"overconfident R: mean NIS {nis_bad.mean():.2f}, fraction below 5.99: {np.mean(nis_bad < 5.99):.2f},"
f" final error {ekf_bad[-1]:.3f} m")Dead reckoning has drifted 2.16 m after 90 s; the EKF is within 4 cm, and its own covariance claims about 3 cm, so it is not fooling itself. For a two-component measurement, a consistent filter's NIS averages 2 and stays below 5.99 (the 95% point of ) 95% of the time. Here it averages 2.25 and passes 92% of the time: close, with the small excess honestly reporting that the motion model ignores the odometry's systematic errors. The overconfident filter's error looks just as good (3.7 cm), but its NIS averages 37 and passes only 13% of the time. Its covariance is a lie, and any decision built on it (a planner choosing how close to pass a pallet, a data-association step deciding which landmark it saw) would be wrong. In simulation, with the truth known, one also checks the normalised estimation error squared (NEES), , which should average the state dimension.
Two details in the code are easy to forget and cause real bugs. The bearing innovation must be wrapped into : a reading of against a prediction of is a 2° surprise, not 358°. And the Jacobians must be evaluated at the current estimate every time.
The unscented transform
Linearisation fails when the function curves noticeably within the spread of the belief. The classic case is exactly the range–bearing sensor: a landmark seen 10 m away with a precise range (5 cm) but a vague bearing (15°) lies somewhere on a curved arc, a "banana", whose centre of mass is inside the arc, closer than 10 m. The EKF puts the mean on the arc and believes the depth uncertainty is just the range noise.
The unscented transform (Julier and Uhlmann, 1990s) instead picks sigma points: the mean and points one column of a matrix square root of along each axis. It pushes each through the exact nonlinear function and recomputes a weighted mean and covariance. No Jacobians are needed, and the result is accurate to second order where linearisation is accurate to first. The program compares both against 200,000 Monte Carlo samples.
import numpy as np
# A range-bearing reading of a landmark: range 10 m (sd 0.05 m), bearing 90 deg (sd 15 deg).
mu = np.array([10.0, np.radians(90)])
C = np.diag([0.05**2, np.radians(15) ** 2])
f = lambda s: np.array([s[0] * np.cos(s[1]), s[0] * np.sin(s[1])]) # polar -> Cartesian
# 1. Monte Carlo: push 200,000 samples through f (the reference answer).
rng = np.random.default_rng(0)
S = rng.multivariate_normal(mu, C, 200_000)
Y = np.stack([S[:, 0] * np.cos(S[:, 1]), S[:, 0] * np.sin(S[:, 1])], axis=1)
mc_mean, mc_cov = Y.mean(axis=0), np.cov(Y.T)
# 2. Linearisation (what the EKF does): mean f(mu), covariance J C J^T.
J = np.array([[np.cos(mu[1]), -mu[0] * np.sin(mu[1])],
[np.sin(mu[1]), mu[0] * np.cos(mu[1])]])
lin_mean, lin_cov = f(mu), J @ C @ J.T
# 3. Unscented transform: 2n + 1 = 5 sigma points, weights with kappa = 3 - n.
n, kappa = 2, 1.0
L = np.linalg.cholesky((n + kappa) * C)
pts = [mu] + [mu + L[:, i] for i in range(n)] + [mu - L[:, i] for i in range(n)]
w = np.array([kappa / (n + kappa)] + [1 / (2 * (n + kappa))] * (2 * n))
fy = np.array([f(p) for p in pts])
ut_mean = w @ fy
ut_cov = sum(wi * np.outer(d, d) for wi, d in zip(w, fy - ut_mean))
for name, m, c in [("Monte Carlo", mc_mean, mc_cov), ("linearised", lin_mean, lin_cov), ("unscented", ut_mean, ut_cov)]:
sd = np.sqrt(np.diag(c))
print(f"{name:12s} mean = ({m[0]:+.3f}, {m[1]:.3f}) m sd = ({sd[0]:.3f}, {sd[1]:.3f}) m")The linearised estimate puts the mean at 10.000 m ahead with a depth spread of 5 cm. The samples say the mean is 9.662 m ahead and the depth spread is 47 cm, nearly ten times larger. The unscented transform, with only five function evaluations, gives 9.663 m and 48 cm. An unscented Kalman filter (UKF) uses this transform in both the predict and correct steps. It costs a few times more than an EKF, needs no derivatives (handy when is a simulator or a lookup table), and is noticeably better when the uncertainty is large compared with the curvature; when the belief is tight, the two agree.
Fusing an IMU with wheel odometry
A gyroscope measures turning rate precisely and quickly, but every MEMS gyro has a bias: it reads, say, rad/s (about per second) when perfectly still, and the bias wanders with temperature. As Rotations and quaternions warned, integrating it alone drifts without bound. Wheel odometry measures turning rate from the difference of the wheel speeds: unbiased on average but noisier, and badly wrong whenever a wheel slips. Each sensor is good exactly where the other is bad, which is the ideal case for fusion.
Put the bias in the state, , with heading , true yaw rate and gyro bias . The model is linear: integrates ; changes freely (a random walk); drifts very slowly. The gyro measures and the odometry measures , so and . Neither measures the bias, but their difference does, and the filter discovers this through the covariance. Halfway through the two-minute run, one wheel spins on a wet patch for two seconds. The last filter rejects any odometry reading whose NIS exceeds 9, a three-standard-deviation surprise.
import numpy as np
rng = np.random.default_rng(3)
dt, T = 0.02, 120.0 # 50 Hz for two minutes
n = int(T / dt)
t = np.arange(1, n + 1) * dt
omega = np.where((t % 20) < 5, 0.4, 0.0) # true yaw rate: a 5 s turn every 20 s
bias = 0.01 # gyro bias, rad/s (about 0.6 deg/s)
gyro = omega + bias + rng.normal(0, 0.01, n) # precise but biased
odo = omega + rng.normal(0, 0.05, n) # unbiased but noisy (encoder steps)
slip = (t > 60) & (t < 62)
odo[slip] += 0.5 # a wheel spins on a wet patch for 2 s
theta_true = np.cumsum(omega) * dt
# State x = [heading, yaw rate, gyro bias]. Both sensors measure rates: gyro = w + b, odometry = w.
F = np.array([[1, dt, 0], [0, 1, 0], [0, 0, 1]])
Q = np.diag([0.0, 0.5**2 * dt, 1e-5**2 * dt]) # yaw rate may change fast; bias drifts very slowly
Hg, Ho = np.array([[0.0, 1, 1]]), np.array([[0.0, 1, 0]])
Rg, Ro = 0.01**2, 0.05**2
def fuse(gate):
x, P = np.zeros(3), np.diag([0.0, 0.1, 0.05**2])
out, rejected = np.zeros(n), 0
for k in range(n):
x, P = F @ x, F @ P @ F.T + Q
for H, r, z, is_odo in ((Hg, Rg, gyro[k], False), (Ho, Ro, odo[k], True)):
y = z - (H @ x)[0]
s = (H @ P @ H.T)[0, 0] + r
if gate and is_odo and y * y / s > 9.0: # NIS above 9: a 3-sigma surprise, reject
rejected += 1
continue
K = (P @ H.T / s).ravel()
x, P = x + K * y, P - np.outer(K, H @ P)
out[k] = x[0]
return out, x[2], rejected
err = lambda th: np.degrees(th - theta_true)
gyro_only, odo_only = np.cumsum(gyro) * dt, np.cumsum(odo) * dt
fused, b1, _ = fuse(gate=False)
gated, b2, nrej = fuse(gate=True)
print("heading error (deg) at 30 s 60 s 90 s 120 s")
for name, th in [("gyro only", gyro_only), ("odometry only", odo_only), ("KF, no gate", fused), ("KF, NIS gate", gated)]:
e = err(th)
print(f"{name:14s} " + " ".join(f"{e[int(s / dt) - 1]:6.2f}" for s in (30, 60, 90, 120)))
print(f"estimated gyro bias: {b1:.4f} rad/s (no gate), {b2:.4f} rad/s (gated); true {bias}")
print(f"odometry readings rejected by the gate: {nrej} of {n}")The gyro alone drifts steadily, a second, to after two minutes. Odometry alone is good until the slip, which adds of phantom rotation that never goes away. The plain Kalman filter does well for the first minute, and has learned the bias, but then it faithfully fuses the slip: the model says odometry is merely noisy, so a 2-second disagreement must be partly a change of bias (its estimate collapses from 0.010 to 0.0016 rad/s) and partly real rotation. The gated filter recognises the slip as impossible under its model, rejects those readings (99 of them during the slip, plus 11 ordinary readings beyond three standard deviations, close to the 0.3% expected by chance), keeps the bias at 0.0099 rad/s, and ends within one degree.
The professional view
Mobile robots. The ROS package robot_localization provides EKF and UKF nodes with a 15-dimensional state (position, orientation, their velocities and linear acceleration) that fuse any number of odometry, IMU, pose and GNSS inputs, each with a configuration saying which components it measures. A typical setup runs one instance in the smooth odom frame (wheels plus IMU) and another in the map frame that also takes GNSS or a localiser, exactly the two-frame structure of Frames and rigid transforms.
Drones and inertial navigation. Orientation is not a vector, so IMU-based filters use an error-state Kalman filter: the nominal orientation is integrated as a quaternion, and the filter estimates a small 3-vector error in its tangent space, where the Kalman equations are valid (Solà's notes in the references explain this with care). The open-source autopilots PX4 and ArduPilot both run EKFs of this kind with more than twenty states, including sensor biases, wind and the magnetic field. Invariant EKFs, which exploit the Lie-group structure of poses, have better consistency guarantees and are used on legged robots.
Cameras plus IMU. Visual-inertial odometry in phones, headsets and drones is often a filter: the MSCKF of Mourikis and Roumeliotis keeps a sliding window of past camera poses in the state, and open implementations such as OpenVINS descend from it. The alternative is to optimise over a window of poses, which leads to the factor graphs of SLAM. A filter is a smoother that has decided never to revisit the past; when revisiting is affordable, optimisation is usually more accurate.
Learned components. Research systems learn parts of a Kalman filter from data: noise covariances that depend on the situation, measurement models that read raw images, or the gain itself (KalmanNet, 2021, replaces the gain computation with a recurrent network while keeping the filter's structure). Lumenary CS's neural networks lesson covers the networks involved. In deployed robots, as of 2026, the filter equations are still almost always the classical ones; learning more often supplies the measurements that feed them.
Common misconceptions
Misconception
" is the noise of the motion sensors and the noise of the other sensors." is how far the true state departs from the motion model in one step: unmodelled accelerations, slip, wind. It includes, but is not limited to, the noise of whatever drives the prediction. is the noise of the measurement as the model describes it, which must also cover modelling errors such as a landmark that is not quite where the map says.
Misconception
" tells you how big the error is." Only if the filter is consistent. is computed from and , not from the data, so a filter with an overconfident reports a tiny whatever its real error. The NIS test is how you find out.
Misconception
"The EKF is optimal." It is the optimal filter for a linear model that happens to match the real model's slope at the current estimate. With large uncertainty or strong curvature it is biased and overconfident, as the unscented-transform example showed.
Misconception
"The Kalman filter needs Gaussian noise to work at all." Its equations need only means and covariances, and for any noise with those statistics it is the best linear estimator. Gaussian noise makes it the best estimator of all. Heavy-tailed noise (outliers) is the real danger, which is why gating matters.
Misconception
"A Kalman filter can track several hypotheses." A Gaussian has one peak. If the robot might be in either of two identical corridors, a Kalman filter must pick one or average them into the wall between. That is the job of the particle filter.
A short history
Carl Friedrich Gauss used least squares in 1801 to predict where the newly found dwarf planet Ceres would reappear from behind the Sun, from a few weeks of observations. Norbert Wiener and Andrey Kolmogorov developed optimal filtering of stationary signals in the 1940s. Rudolf Kálmán's 1960 paper recast the problem in state-space form, recursive and suited to digital computers, and with Richard Bucy he gave the continuous-time version in 1961. Stanley Schmidt's team at NASA Ames extended it to nonlinear systems for Apollo's navigation, which flew with a Kalman filter on board. The unscented transform was introduced by Simon Julier and Jeffrey Uhlmann in the mid-1990s. The filter has since moved from spacecraft to pockets: smartphones run them to fuse satellite positioning with their motion sensors.
Exercises
1. Kalman filters
Warm-upHumanA robot measures the distance to a doorway with two independent sensors: a time-of-flight sensor says m with standard deviation m, and a stereo camera says m with standard deviation m. Fusing them optimally, what is the standard deviation of the fused estimate, in centimetres?
2. Kalman filters
CoreHumanA constant-velocity filter has independent position and velocity errors with standard deviations cm and cm/s. With no process noise (), it predicts s ahead using . What is the predicted variance of the position, in cm²?
3. Kalman filters
CoreHumanA tracker following a forklift lags badly behind whenever the forklift speeds up, and its mean NIS is about 6 for a one-dimensional position measurement. Which change is most likely to help?
4. Kalman filters
CoreHumanAn EKF localises a robot with range–bearing readings (two components each). Over an hour its mean NIS is 0.4. Which explanations fit? Select all that apply.
5. Kalman filters
Warm-upHumanAn unscented Kalman filter for a drone has a 15-dimensional state (position, velocity, orientation error, gyro bias and accelerometer bias, three numbers each). With the standard sigma-point set of the lesson, how many times must it evaluate the motion model in each predict step?
6. Kalman filters
CoreComputationalWrite kf_step(x, P, z, F, Q, H, R), one full cycle of the Kalman filter: predict with and , then correct with the measurement z using and . All arguments are NumPy arrays (x and z 1-D, the rest 2-D). Return the new (x, P).
7. Kalman filters
StretchComputationalThe EKF needs the Jacobian of the range–bearing model. The function h(state, landmark) returns range and bearing from a robot at state = (x, y, theta) to landmark = (lx, ly). Write jacobian(state, landmark), returning the matrix . The tests compare it with numerical derivatives at random poses.
Further reading and resources
- A New Approach to Linear Filtering and Prediction Problems · Rudolf E. Kálmán · 1960The original paper.
- Unscented Filtering and Nonlinear Estimation · Simon J. Julier and Jeffrey K. Uhlmann · 2004The unscented transform and the UKF, explained by their inventors.
- Quaternion kinematics for the error-state Kalman filter · Joan Solà · 2017How IMU-based estimators handle orientation inside a Kalman filter in practice.
- A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation · Anastasios I. Mourikis and Stergios I. Roumeliotis · 2007The MSCKF, a filter for camera plus IMU that underlies many visual-inertial odometry systems.
- robot_localizationThe ROS package with EKF and UKF nodes that fuse wheel odometry, IMUs and GNSS.
- Using PX4's Navigation Filter (EKF2)The open-source drone autopilot's estimator, including its innovation consistency checks.
- Bayesian Filtering and Smoothing (2nd edition, free pre-publication PDF) · Simo Särkkä and Lennart Svensson · 2023