Sensors, noise and a first Kalman filter

A robot never sees its own state. It sees sensor readings, and every reading is wrong by some amount. This lesson is for readers who can write a numpy loop and want to know what “wrong” looks like for the sensors on a typical robot, and how to combine many wrong readings into an estimate that is better than any one of them. We simulate three sensors, measure their errors, and then build a Kalman filter that tracks a moving robot from noisy position fixes. The filter is about 15 lines of numpy and it is the same algorithm, in more dimensions, that runs inside most robot localisation stacks.

Three sensors you will meet first

An encoder measures how far a shaft has turned. Incremental optical or magnetic encoders count pulses as the shaft rotates. A quadrature encoder has two channels a quarter of a cycle apart, so the controller can tell the direction of rotation as well as the amount. The reading is an integer count, so the measured angle moves in steps. Encoders on drive wheels give odometry, covered in the next lesson. See the glossary entry for encoder.

An IMU (inertial measurement unit) combines a three-axis accelerometer and a three-axis gyroscope, and sometimes a magnetometer. The gyro measures angular rate in rad/s. To get an angle you integrate the rate, and integration is where trouble starts: any constant offset in the rate becomes an angle error that grows linearly with time. MEMS gyros in low-cost IMUs have an offset of this kind (the bias) that changes with temperature and from one power-up to the next. See IMU.

A range sensor measures distance: ultrasonic sensors, infrared time-of-flight sensors and lidar are the common ones. A single range reading has noise that is roughly Gaussian for a well-behaved target, plus occasional outliers from reflections, glass and dark surfaces.

Three kinds of error

Almost every sensor error you will debug falls into one of three groups.

Error	What it looks like	Does averaging help?
Random noise	Each reading scatters around the true value, independently of the last	Yes. The mean of N readings has a standard deviation smaller by a factor of the square root of N
Bias	Every reading is off by the same amount (or a slowly changing amount)	No. The mean converges to the true value plus the bias
Quantisation	Readings come in fixed steps, such as one encoder count	Only if the true value moves or noise dithers it across steps

The block below simulates one minute of a stationary robot. The gyro has an illustrative bias of 0.5 deg/s and white noise of 0.2 deg/s. Real figures depend on the part, so check your IMU’s datasheet. The encoder has 4096 counts per revolution. The range sensor reads a wall 2 m away with 20 mm of Gaussian noise.

import numpy as np

rng = np.random.default_rng(6)
dt = 0.01                      # 100 Hz sample rate
t = np.arange(0, 60, dt)       # one minute, robot standing still

# Gyro: true rate is zero. The reading has a constant bias plus white noise.
gyro_bias = np.deg2rad(0.5)    # 0.5 deg/s, an illustrative low-cost MEMS value
gyro_noise_std = np.deg2rad(0.2)
gyro = gyro_bias + rng.normal(0.0, gyro_noise_std, t.size)
heading = np.cumsum(gyro) * dt             # integrate rate to get heading

# Same gyro after a 5 s calibration at rest: subtract the mean of that window.
bias_estimate = gyro[: int(5 / dt)].mean()
heading_cal = np.cumsum(gyro - bias_estimate) * dt

# Encoder: 4096 counts per revolution, so angle comes in steps of 2*pi/4096.
counts_per_rev = 4096
true_angle = 0.123456
measured = np.round(true_angle / (2 * np.pi / counts_per_rev)) * (2 * np.pi / counts_per_rev)

# Range sensor: true distance 2.000 m, Gaussian noise 20 mm, 50 readings.
ranges = 2.0 + rng.normal(0.0, 0.02, 50)

print(f"gyro heading after 60 s, raw:        {np.rad2deg(heading[-1]):7.2f} deg")
print(f"gyro heading after 60 s, calibrated: {np.rad2deg(heading_cal[-1]):7.2f} deg")
print(f"estimated bias: {np.rad2deg(bias_estimate):.3f} deg/s (true 0.500)")
print(f"encoder step: {np.rad2deg(2 * np.pi / counts_per_rev):.4f} deg, "
      f"quantisation error here: {np.rad2deg(measured - true_angle):+.4f} deg")
print(f"range: one reading {ranges[0]:.4f} m, mean of 50 {ranges.mean():.4f} m, "
      f"std of one {ranges.std(ddof=1) * 1000:.1f} mm, std of mean {ranges.std(ddof=1) / np.sqrt(50) * 1000:.1f} mm")

gyro heading after 60 s, raw:          30.22 deg
gyro heading after 60 s, calibrated:    0.19 deg
estimated bias: 0.500 deg/s (true 0.500)
encoder step: 0.0879 deg, quantisation error here: -0.0423 deg
range: one reading 2.0301 m, mean of 50 1.9981 m, std of one 19.7 mm, std of mean 2.8 mm

The numbers tell the story of each error type.

The raw gyro heading has drifted by 30 degrees in a minute while the robot sat still. That is the 0.5 deg/s bias times 60 s. The white noise contributes almost nothing by comparison because it averages out under integration. Averaging the first 5 s of readings, while the robot is known to be still, estimates the bias to within a few thousandths of a degree per second. Subtracting it leaves 0.19 degrees of drift after a minute. This is why many robots ask you to keep them still for a few seconds after power-up. It does not fix bias that changes with temperature, which is one reason robots combine a gyro with something that measures heading directly.

The encoder’s step is 0.088 degrees. No amount of averaging a stationary encoder will recover the true angle between two steps, because every reading is identical. The quantisation error here is 0.042 degrees, a little under half a step, which is the worst case.

The range sensor shows averaging at its best. One reading is 30 mm off. The mean of 50 readings is 1.9 mm off, and the predicted standard deviation of that mean is 19.7 divided by the square root of 50, or 2.8 mm. The catch is that averaging 50 readings only works because the wall did not move. For a moving robot, we need something better.

Averaging is not filtering

Suppose the robot drives down a corridor and a sensor reports its position 5 times a second with 0.3 m of noise. The obvious fix is a moving average over the last few readings. It reduces noise, but it reports where the robot was, on average, over the window. If the window covers 0.8 s and the robot moves at 1.2 m/s, the average sits about half a metre behind. On a moving robot a moving average trades noise for lag.

A filter avoids that trade by using a model of how the state changes. If you know the robot was at 3.0 m moving at 1.2 m/s, then 0.2 s later you expect it near 3.24 m before you look at the sensor. The new reading then corrects that prediction. How much you trust the prediction versus the reading depends on how uncertain each one is. The Kalman filter does this weighting optimally when the model is linear and the noise is Gaussian. R. E. Kalman published it in 1960.

The Kalman filter in two steps

The state is a vector x with an uncertainty, the covariance matrix P. For a robot on a line we use position and velocity, so x has two entries and P is 2 by 2. Every time step runs two stages.

Predict. Push the state through the motion model F. For constant velocity over a time step dt, position gains velocity times dt and velocity stays the same. The covariance grows by Q, the process noise, which says how far the real robot can depart from the model in one step. Here the departure is an unknown acceleration with standard deviation q, and Q follows from how that acceleration feeds into position and velocity.

Update. The measurement model H picks out what the sensor sees, here position only. The innovation z - H x is the difference between the reading and the prediction. The Kalman gain K decides what fraction of the innovation to apply. It is large when the prediction is uncertain (big P) and small when the sensor is noisy (big R). After the update the covariance shrinks, because the estimate now uses one more piece of information.

The filter never measures velocity. It infers velocity from how the positions change, and the covariance tracks how well it knows it.

Tracking a robot from noisy positions

The block simulates a robot driving along a corridor for 10 s. Its velocity starts at 1.0 m/s and wanders because of random acceleration (standard deviation 0.5 m/s²). A position sensor reports every 0.2 s with 0.3 m of noise. We run a 5-sample moving average and the Kalman filter on the same measurements.

import numpy as np

rng = np.random.default_rng(42)
dt = 0.2                 # s between measurements
n = 50                   # 10 s of data
q = 0.5                  # std of the random acceleration, m/s^2 (process noise)
r = 0.3                  # std of a position measurement, m

# Truth: a robot driving along a corridor. Velocity wanders because of random acceleration.
x_true = np.zeros(n)
v_true = np.zeros(n)
v_true[0] = 1.0
for k in range(1, n):
    a = rng.normal(0.0, q)
    x_true[k] = x_true[k - 1] + v_true[k - 1] * dt + 0.5 * a * dt**2
    v_true[k] = v_true[k - 1] + a * dt
z = x_true + rng.normal(0.0, r, n)          # noisy position measurements

# Constant-velocity model: state [position, velocity].
F = np.array([[1.0, dt], [0.0, 1.0]])       # state transition
H = np.array([[1.0, 0.0]])                  # we measure position only
G = np.array([[0.5 * dt**2], [dt]])         # how an acceleration enters the state
Q = G @ G.T * q**2                          # process noise covariance
R = np.array([[r**2]])                      # measurement noise covariance

x_hat = np.array([z[0], 0.0])               # start at the first measurement, velocity unknown
P = np.diag([r**2, 4.0])                    # large uncertainty on velocity
est = np.zeros(n)
vel = np.zeros(n)
for k in range(n):
    if k > 0:                               # predict
        x_hat = F @ x_hat
        P = F @ P @ F.T + Q
    y = z[k] - H @ x_hat                    # innovation
    S = H @ P @ H.T + R                     # innovation covariance
    K = P @ H.T @ np.linalg.inv(S)          # Kalman gain
    x_hat = x_hat + (K @ y).ravel()         # update
    P = (np.eye(2) - K @ H) @ P
    est[k], vel[k] = x_hat

# A moving average of the last 5 measurements for comparison.
ma = np.array([z[max(0, k - 4): k + 1].mean() for k in range(n)])

def rms(e):
    return float(np.sqrt(np.mean(e**2)))

print(f"RMS error, raw measurements:   {rms(z - x_true):.3f} m")
print(f"RMS error, 5-sample average:   {rms(ma - x_true):.3f} m")
print(f"RMS error, Kalman filter:      {rms(est - x_true):.3f} m")
print(f"final velocity: true {v_true[-1]:.3f} m/s, estimated {vel[-1]:.3f} m/s")
print(f"final position std from P: {np.sqrt(P[0, 0]):.3f} m, Kalman gain {K.ravel().round(3)}")
print("t, truth, measured, estimate (every second sample)")
for k in range(0, n, 2):
    print(f"{k * dt:.1f}, {x_true[k]:.3f}, {z[k]:.3f}, {est[k]:.3f}")

RMS error, raw measurements:   0.232 m
RMS error, 5-sample average:   0.465 m
RMS error, Kalman filter:      0.145 m
final velocity: true 1.449 m/s, estimated 1.191 m/s
final position std from P: 0.166 m, Kalman gain [0.306 0.278]
t, truth, measured, estimate (every second sample)
0.0, 0.000, 0.020, 0.020
0.4, 0.399, 0.588, 0.523
0.8, 0.801, 0.705, 0.596
1.2, 1.168, 0.976, 0.955
1.6, 1.477, 1.925, 1.548
2.0, 1.768, 2.059, 1.885
2.4, 2.068, 1.968, 2.001
2.8, 2.413, 2.589, 2.428
3.2, 2.799, 3.037, 2.911
3.6, 3.164, 3.026, 3.161
4.0, 3.531, 3.473, 3.563
4.4, 3.892, 3.552, 3.707
4.8, 4.266, 4.415, 4.111
5.2, 4.631, 4.839, 4.619
5.6, 5.002, 5.049, 4.990
6.0, 5.405, 5.312, 5.408
6.4, 5.885, 5.687, 5.809
6.8, 6.351, 6.237, 6.215
7.2, 6.818, 6.964, 6.671
7.6, 7.312, 7.316, 7.173
8.0, 7.762, 7.896, 7.778
8.4, 8.251, 8.221, 8.304
8.8, 8.746, 8.722, 8.738
9.2, 9.247, 8.813, 8.974
9.6, 9.784, 9.485, 9.395

The Kalman filter’s RMS error is 0.145 m, against 0.232 m for the raw readings. The moving average is worse than doing nothing, at 0.465 m, because of the lag described above: its window spans 0.8 s, so it sits about 0.4 s behind a robot moving at over 1 m/s.

The position gain settles at 0.306. Each new reading moves the estimate about 31 % of the way from the prediction towards the measurement. That number is not tuned by hand. It falls out of Q and R once the filter reaches steady state.

At the scale of the whole run the three lines lie almost on top of each other, so the second chart plots the errors: the measured and estimated positions minus the truth, computed from the same printed table.

The last second is worth a look. The true velocity has climbed to 1.45 m/s, but the filter’s velocity estimate is 1.19 m/s, so its predictions fall short and the error grows to 0.39 m. The model says velocity is constant apart from random kicks of size q. If the real robot accelerates harder or longer than that, the filter is slow to follow. Raising q makes the filter trust the model less and the sensor more, which follows changes faster and lets more noise through. Lowering r has a similar effect. Choosing Q and R is the main tuning job when you deploy a Kalman filter, and the right values come from the sensor datasheet and from logs of the real robot, not from guesswork.

From one dimension to a robot

A real mobile robot’s state includes x, y and heading at least, and the motion model involves sines and cosines of the heading. That model is non-linear, so the plain Kalman filter does not apply directly. The extended Kalman filter linearises the model around the current estimate at each step and otherwise runs the same predict and update. When the uncertainty is too large or too oddly shaped for one Gaussian, a particle filter represents it with samples instead.

Bias can also join the state. If you add the gyro bias as a third state with a very small process noise, the filter estimates it continuously from the disagreement between the gyro and another heading source. This is how many IMU fusion filters keep the drift seen in the first block under control while the robot moves. The Kalman filter cheat sheet lists the equations for these variants, and the state estimation hub links to the rest of the material on this site.

Exercise

1. In the second block, set q = 2.0 and run it again. Report the new RMS error and the final Kalman gain, and explain the change in terms of trust in the model versus the sensor.
2. Set r = 0.05 in both the simulation and the filter. What happens to the gain and to the gap between the filter and the raw measurements?
3. Add a constant bias of 0.2 m to every measurement. Does the filter remove it? Then extend the state to [position, velocity, bias] with H = [1, 0, 1] and explain why the filter still cannot tell the bias from the position without another sensor.

Summary

• Sensor errors come as random noise, bias and quantisation. Averaging shrinks random noise by the square root of the number of samples and does nothing to bias.
• Integrating a gyro turns a constant rate bias into an angle error that grows linearly with time. A short stationary calibration removes most of it.
• A moving average on a moving robot trades noise for lag. A filter uses a motion model to predict, then corrects the prediction with each measurement.
• The Kalman filter alternates predict (state through F, covariance grows by Q) and update (gain K from P and R, covariance shrinks). It estimates quantities it never measures, such as velocity.
• Q and R set the balance between the model and the sensor. Too small a Q makes the filter lag real changes.

Further reading

• Probabilistic Robotics, Thrun, Burgard and Fox, MIT Press, 2005: chapter 3 derives the Kalman and extended Kalman filters, and chapter 6 covers sensor models for range finders.
• A New Approach to Linear Filtering and Prediction Problems, R. E. Kalman, Journal of Basic Engineering, 1960: the original paper.
• Modern Robotics, Lynch and Park, Cambridge University Press, 2017 (updated first edition, 2019 preprint): chapter 13 covers wheeled mobile robots and odometry, the motion model you will need when the filter moves to two dimensions.