Keep a Position Estimate Without Believing Every Detection

In our conveyor simulation, Botie estimates where a part will be when a robot could reach it. The belt’s movement suggests one position. A learned camera detector reports a slightly different one. Choosing the camera every time makes the estimate jump; ignoring it lets motion errors accumulate.

What should Botie keep between camera frames? A current position is part of it, but we also need a description of how uncertain that position is. That uncertainty will help decide how strongly a new reading should change the estimate. The conveyor and robot are simulated throughout this lesson.

The simulated belt movement predicts one position and the camera reports another. Use their supplied error variances to decide how much the estimate should change.

{
  "mean": 4.8,
  "variance": 0.8
}

The part has an actual position even when we cannot observe it exactly. Here that position, in metres along the conveyor, is the state we want to estimate. We keep a mean as our central estimate and a variance as its stated squared spread. Taking the square root of the variance gives a standard deviation measured in metres again.

A small variance is an assertion made by our model. It is not a promise that the estimate is close to the actual part. We will test that assertion in the later lesson on model mismatch.

Keep position and variance together. A position alone cannot tell the next measurement how much to influence it.

Add expected displacement to the earlier mean. Motion also introduces uncertainty: the belt might slip or the displacement estimate might be imperfect. In this scalar model, add the supplied variance of an independent zero-mean motion error.

The result is called the prior or predicted estimate at the new time. Its uncertainty already includes what we knew before moving.

earlier mean = 2 m; earlier variance = 1 m²
expected displacement = 2 m
added motion variance Q = 3 m²

predicted mean = 2 + 2 = 4 m
predicted variance P⁻ = 1 + 3 = 4 m²

Variance adds because the errors are independent in this model. Adding standard deviations would implement another calculation.

Move the estimate to the new time before comparing it with the camera reading taken at that time. Their difference tells us how much the observation disagrees with the prediction. This difference is called the innovation.

The camera’s error variance is R. The predicted position’s variance is P⁻. Under the independence assumption, their sum is the innovation variance. Both terms must use squared metres.

measurement z = 5 m
innovation = z - predicted mean = 5 - 4 = 1 m
innovation variance S = P⁻ + R = 4 + 1 = 5 m²

A learned detector’s confidence score is not automatically R. Position-error variance needs a justified measurement model or calibration data.

The scalar Kalman gain is the share of the innovation used to adjust the prediction. A more uncertain prediction and a more reliable observation give the observation more influence. With a noisy camera, the gain becomes smaller.

The gain is dimensionless here. It multiplies a position difference, so the correction remains in metres.

K = P⁻ / (P⁻ + R) = 4 / 5 = 0.8
corrected mean = 4 + 0.8 × (5 - 4) = 4.8 m

if camera variance instead R = 16 m²:
K = 4 / 20 = 0.2
corrected mean = 4.2 m

Neither result declares the camera correct or incorrect. Each follows its supplied uncertainty assumptions.

The camera changes what we know about the position as well as the position we report. The variance calculation below keeps the prediction-error contribution and the camera-error contribution visible. This is the Joseph form of the update. With the exact Kalman gain, it agrees with the shorter scalar expression, while making both contributions explicit.

Even an observation that exactly agrees with the predicted mean can reduce variance: agreement changes no position, but supplies information about uncertainty under this model.

P⁺ = (1 - K)² P⁻ + K² R
   = (1 - 0.8)² × 4 + 0.8² × 1
   = 0.16 + 0.64 = 0.8 m²

A zero innovation does not mean the observation contained no information. Reusing the same observation as if it were new independent evidence would be a different error.

A moving part can have position and velocity in one state vector. Covariance describes related errors between its coordinates. A position observation can then change the velocity estimate because an earlier velocity error also affected predicted position.

The transition matrix F describes how the state changes over the interval. H describes which parts of that state the sensor observes. With these linear relationships, the prediction and correction use the matrix equations below. When calculating the gain, solve the innovation system rather than explicitly forming its inverse.

x⁻ = F x⁺ + B u
P⁻ = F P⁺ Fᵀ + Q
S = H P⁻ Hᵀ + R
K = P⁻ Hᵀ S⁻¹
x⁺ = x⁻ + K (z - H x⁻)
P⁺ = (I - K H) P⁻ (I - K H)ᵀ + K R Kᵀ

Q and R are covariance matrices here. Their entries have units matching the state or measurement coordinates; they are not arbitrary confidence percentages.

import numpy as np

state = np.array([1., 1.])
covariance = np.eye(2)
transition = np.array([[1., 1.], [0., 1.]])
predicted = transition @ state
prior = transition @ covariance @ transition.T + np.diag([.25, .25])
observation = np.array([3.])
h = np.array([[1., 0.]])
r = np.array([[.25]])
innovation_covariance = h @ prior @ h.T + r
gain = np.linalg.solve(innovation_covariance, (prior @ h.T).T).T
corrected = predicted + gain @ (observation - h @ predicted)
residual_map = np.eye(2) - gain @ h
posterior = residual_map @ prior @ residual_map.T + gain @ r @ gain.T
print(np.round(corrected, 3).tolist())
print(np.round(posterior, 3).tolist())

[2.9, 1.4]
[[0.225, 0.1], [0.1, 0.85]]

For the full posterior to remain exactly Gaussian, begin with a Gaussian state distribution and use linear models with the stated independent Gaussian additive errors. With weaker assumptions, the same equations can still have a best-linear-estimation interpretation without describing the complete posterior.

Bias, correlated errors, bad associations and nonlinear motion can invalidate the uncertainty interpretation. A camera detector trained on different conditions can change its error behaviour. The workshop isolates the arithmetic so those assumptions remain visible.

The next step is to compare these uncertainty claims with independent reference trajectories.