Tracking position and velocity

A single number is rarely all a robot cares about. A mobile robot needs its position and how fast it is moving; a drone needs height and climb rate. The remarkable thing about the Kalman filter is that it can estimate a quantity you never measure. Give it noisy position readings and it will also give you a velocity estimate, with no velocity sensor at all. The trick is that position and velocity are tied together by physics, and the filter exploits that link. To do it we need to upgrade our two scalars to a vector and a matrix.

The state vector and the model

We stack the unknowns into a state vector:

x = [position, velocity]

Our model is constant velocity: over a short time step dt, the robot keeps moving at the same speed, so

position_new = position + velocity * dt

velocity_new = velocity

Written as a matrix equation, x_new = F * x, where the state transition matrix is:

F = [[1, dt], [0, 1]]

The first row says new position equals one times old position plus dt times old velocity. The second row says velocity carries over unchanged. The model is not exactly true, since real robots speed up and slow down, so we admit that with process noise. If the unknown acceleration is random with standard deviation sigma_a, then over one step it changes position by about a * dt^2 / 2 and velocity by a * dt. Working out the variances and their link gives the process noise matrix:

Q = sigma_a^2 * [[dt^4 / 4, dt^3 / 2], [dt^3 / 2, dt^2]]

You do not need to memorise it. Notice that the same random acceleration disturbs position and velocity together, which is why the matrix is not diagonal.

Covariance: uncertainty with relationships

With two states, a single variance is no longer enough. The covariance matrix P is 2 by 2:

P = [[var_pos, cov_pos_vel], [cov_pos_vel, var_vel]]

The diagonal holds the variance of each state. The off-diagonal entry, the covariance, says how the errors of the two states move together. If velocity is higher than we think, then position is also probably higher than we think a moment later. That link is what lets a position measurement correct the velocity estimate. The predict step becomes:

x = F * x

P = F * P * F^T + Q

The F P F^T form is the matrix version of "variance grows when you multiply by a factor": uncertainty about velocity leaks into uncertainty about position. We can see this by predicting with no measurements and no process noise, starting from variance 1 on both states.

covariance_growth.py

ResetRun

import numpy as np dt = 0.1 F = np.array([[1.0, dt], [0.0, 1.0]]) P = np.diag([1.0, 1.0]) # position var 1 m^2, velocity var 1 (m/s)^2 print("step var_pos cov_pos_vel var_vel") for k in range(0, 31): if k in (0, 1, 5, 10, 20, 30): print("%4d %7.3f %11.3f %7.3f" % (k, P[0, 0], P[0, 1], P[1, 1])) P = F @ P @ F.T # predict only, no measurements, no process noise

After 1 second (step 10) the position variance has doubled from 1.000 to 2.000, and after 3 seconds (step 30) it is 10.000. The pattern is var_pos = 1 + t^2: with a velocity uncertain by 1 m/s, the position uncertainty grows with time. Meanwhile the velocity variance stays at 1.000, because without process noise nothing changes the velocity, and the covariance grows steadily to 3.000, linking the two errors more tightly.

The update with a position-only sensor

The sensor sees position, not velocity. The measurement matrix H picks position out of the state:

H = [1, 0]

The update formulas are the scalar ones in matrix clothing:

S = H * P * H^T + R

K = P * H^T * S^-1

x = x + K * (z - H * x)

P = (I - K * H) * P

S is the variance of the innovation, a single number here, and K is now a column of two gains: one for position and one for velocity. The velocity gain is non-zero only because of the covariance. When the measurement says the robot is further ahead than predicted, the filter pushes position up and, because errors in the two states are linked, pushes velocity up as well.

A full tracker in numpy

Time to see it work. A robot moves along a corridor at about 2 m/s, with its true speed wandering a little (sigma_a = 0.5 m/s^2). A noisy position sensor, such as a cheap ultrasonic or GPS fix, reports every 0.1 s with a standard deviation of 2 m. The filter starts knowing nothing: position 0, velocity 0, and large variances. We compare the error of the filter's position estimate with that of the raw measurements over 300 steps.

kalman_cv.py

ResetRun

import random import numpy as np random.seed(3) dt, steps = 0.1, 300 sigma_a = 0.5 # how much the true velocity wanders (m/s^2) sigma_z = 2.0 # position sensor noise (m) F = np.array([[1.0, dt], [0.0, 1.0]]) H = np.array([[1.0, 0.0]]) Q = sigma_a ** 2 * np.array([[dt**4 / 4, dt**3 / 2], [dt**3 / 2, dt**2]]) R = np.array([[sigma_z ** 2]]) truth = np.array([0.0, 2.0]) # true position and velocity x = np.array([0.0, 0.0]) # filter: knows nothing about velocity P = np.diag([4.0, 25.0]) err_raw, err_kf = [], [] for k in range(steps): truth = F @ truth + np.array([0.5 * dt**2, dt]) * random.gauss(0, sigma_a) z = truth[0] + random.gauss(0, sigma_z) x = F @ x # predict P = F @ P @ F.T + Q S = H @ P @ H.T + R # update K = P @ H.T @ np.linalg.inv(S) x = x + (K @ (z - H @ x)).ravel() P = (np.eye(2) - K @ H) @ P err_raw.append(z - truth[0]) err_kf.append(x[0] - truth[0]) rms = lambda e: float(np.sqrt(np.mean(np.square(e)))) print("final true position: %.2f m, velocity: %.2f m/s" % tuple(truth)) print("final estimate: %.2f m, velocity: %.2f m/s" % tuple(x)) print("RMS position error, raw measurements: %.2f m" % rms(err_raw)) print("RMS position error, Kalman filter: %.2f m" % rms(err_kf)) print("final gain K:", np.round(K.ravel(), 3)) print("position std from P: %.2f m" % np.sqrt(P[0, 0]))

The raw measurements have an RMS error of about 2.00 m, exactly the sensor noise. The Kalman filter brings that down to about 0.72 m, nearly three times better, and that includes the first few seconds while it is still learning the speed. At the end, the true position is about 48.5 m and the estimate lands within 0.1 m of it. The true velocity is about 1.14 m/s and the estimate says about 1.32 m/s, an error of less than 0.2 m/s from a sensor that never measured velocity. The final gains are about 0.07 for position and 0.02 for velocity, small values showing that the filter is now confident and each new measurement moves it only a little. The filter's own claim of uncertainty, sqrt(P[0, 0]) at about 0.52 m, is in the same range as its observed error, which is the sign of an honest filter. Lesson 5 turns that into a test.