explainer
Kalman filter: predict position, then correct with measurements
Work through a scalar Kalman update, then estimate position and velocity from noisy readings. Explore missing measurements, covariance, and a biased sensor that defeats the filter’s assumptions.
What you will learn
- Calculate a scalar measurement update from a prior and a sensor reading.
- Predict position and velocity with a linear motion model.
- Distinguish a covariance from a standard deviation and interpret their units.
- Explain how missing measurements and sensor bias affect the estimate.
- State the assumptions behind the displayed uncertainty intervals.
Before you start
A Kalman filter combines a motion prediction with a sensor reading. It tracks uncertainty alongside the estimate, then uses that uncertainty to weight new evidence. A robot can use this process to estimate velocity even when its sensor reports only position.
Start with one number, then build a position-and-velocity filter. The experiment includes missing readings and a biased sensor, so you can inspect where the model's confidence fails.
Start with one scalar Kalman update
Suppose your prior position estimate is m⁻ = 0 m, with variance P⁻ = 4 m². A sensor reports z = 2 m, with measurement-error variance R = 1 m². Assume independent, zero-mean Gaussian errors and a sensor that measures position directly.
The Kalman gain determines how much of the discrepancy to apply:
The prior standard deviation is 2 m. The posterior standard deviation is √0.8 ≈ 0.894427 m. The superscript minus means before this reading; plus means after it.
This is a Gaussian Bayesian update: the prior and measurement likelihood produce a posterior. A smaller R gives this reading more weight. That statement assumes the supplied R describes the measurement error reasonably well.
Keep the state and its uncertainty together
For motion along one line, store two state components:
Here x denotes the unknown physical state, and m its estimated mean. The filter also stores a symmetric covariance matrix P:
| Entry | Meaning | Units |
|---|---|---|
| Pₚₚ | Position-error variance | m² |
| Pᵥᵥ | Velocity-error variance | m²/s² |
| Pₚᵥ = Pᵥₚ | Position-velocity error covariance | m²/s |
A standard deviation is the square root of a variance. σₚ = √Pₚₚ therefore uses meters; σᵥ = √Pᵥᵥ uses meters per second. The covariance lesson shows why the off-diagonal term matters when interpreting both states together.
The lab starts at time 0 with m₀⁻ = [0, 0]ᵀ and P₀⁻ = diag(4, 1) in the units above. The first reading corrects that prior immediately. Prediction starts when time advances to 1 s.
Model position and velocity over time
Between samples, the mean model keeps velocity constant:
F advances the state. H extracts the position that the sensor observes. The measured position follows zₖ = Hxₖ + εₖ, where εₖ is the sensor error.
The controllability and observability lesson explains why measurements over time can reveal more than one instantaneous reading. Its continuous-time example also shows a position offset that velocity measurements cannot reveal under the stated model.
The experiment uses Δt = 1 s exactly. Position and velocity form a two-component state; the robot still moves along one spatial axis. Knowing the timestamps is part of the model.
Two position readings at different times carry information about velocity. Prediction also creates position-velocity covariance, allowing later position corrections to change both estimates. The filter does not receive a velocity measurement.
To let the estimate choose the next applied force, continue with LQG control. Its cart example adds a known control input to prediction and compares feedback under matched noise.
For a continuous-time estimator with fixed gains, work through the Luenberger observer. That lesson chooses the estimation-error poles directly. The Kalman filter here computes its gains from the evolving covariance and declared noise assumptions.
Choose process noise from a physical assumption
A constant-velocity mean does not require certainty that acceleration is zero. Assume an unknown acceleration aₖ stays constant within each interval, then changes independently for the next interval. Let its mean be zero and its standard deviation be σₐ, in m/s².
That same acceleration changes both position and velocity. Its state covariance is:
At σₐ = 0.2 m/s² and Δt = 1 s, the numerical entries are Q = [[0.01, 0.02], [0.02, 0.04]]. Each entry carries the corresponding covariance units from the table. Measurement standard deviation σ𝓏 = 0.5 m gives R = σ𝓏² = 0.25 m².
Roger Labbe's FilterPy process-noise documentation derives this discrete acceleration model through GGᵀ. A continuous white-acceleration model has a different Q and parameter units. Specify which acceleration model you use before copying a matrix.
Predict before using the next reading
Advance the previous posterior through the model:
The mean predicts the next position. The covariance carries existing uncertainty forward and adds the interval's process uncertainty. In this example, uncertain velocity spreads the predicted position over time.
For the default record, the update at 0 s gives m₀⁺ = [0.376471, 0]ᵀ and P₀⁺ = diag(0.235294, 1). Predicting to 1 s gives:
The new cross-covariance is 1.02 m²/s. That coupling lets the next position reading teach the filter about velocity.
Correct with the measurement innovation
The innovation is the observed position minus the predicted position. Its variance S includes uncertainty from both the prediction and the sensor:
K now has two entries. Its position entry is dimensionless; its velocity entry has units 1/s. Both multiply a position innovation measured in meters.
For the 1.2 m reading at 1 s, the innovation is 0.823529 m. The gain is approximately [0.832809, 0.682139]ᵀ, producing m₁⁺ = [1.062313 m, 0.561762 m/s]ᵀ.
The implementation uses the Joseph covariance update:
I is the identity matrix. Both terms preserve positive semidefiniteness in exact arithmetic, and this form helps limit numerical damage from rounding. FilterPy's update implementation uses this form; numerical implementations still need finite-value and covariance checks.
Replay the position and velocity filter
The synthetic reference moves at 1 m/s, starting at 0 m. At times 0 through 10 s, the baseline position readings are:
0.4, 1.2, 1.7, 3.4, 3.9, 4.8, 6.3, 6.6, 8.2, 9.1, 9.8 m
These are fixed teaching values. They do not form a random trial or validate a noise distribution. The filter receives only the readings, initial prior, timestamps, and assumed noise parameters; the separate reference supplies the error check.
At the default 6 s step, the estimated position is 6.072409 m and velocity is 1.054274 m/s. The position standard deviation is 0.384624 m. Replay from 0 s to see velocity emerge from position updates.
Changing either noise slider reruns the same record. Increasing σ𝓏 tells the filter to trust each reading less. Increasing σₐ lets the prediction spread faster and can give new readings more influence; neither slider repairs an incorrect sensor model.
Compare missing readings and sensor bias
Choose Missing readings at 4–6 s. During those steps, retain the predicted mean and covariance without a correction. Never substitute a zero measurement for an absent reading.
At 6 s, the position estimate is 5.960773 m, with standard deviation 1.340561 m. That larger uncertainty gives the returning reading a position gain of 0.924401 at 7 s. WPILib's observer documentation describes prediction and correction as separate operations that can run at different rates.
Now choose Sensor bias from 4 s. Each later reading adds 2 m, but the filter still assumes zero-mean sensor error. At 6 s it reports 8.287431 m, although the reference is 6 m.
Its reported position standard deviation stays 0.384624 m, exactly as in the baseline. With fixed F, H, Q, R, and measurement availability, covariance updates do not depend on the observed values. A biased measurement can therefore move the estimate far outside an unchanged uncertainty band.
Read uncertainty under stated assumptions
The linear Gaussian interpretation requires a Gaussian initial prior, linear motion and measurement models, and known covariances. This lesson assumes independent, zero-mean Gaussian acceleration and sensor errors across samples, also independent of the initial state. Under those assumptions, the Kalman mean and covariance describe the conditional state distribution.
Each plotted band spans mean ±2 standard deviations for one state component. A univariate Gaussian assigns about 95.45% probability to that interval. This is a marginal model statement, not a joint two-state region or evidence of 95% coverage on these eleven fixed samples.
The filter equations can still compute linear estimates without Gaussian errors. A Gaussian probability interpretation needs the extra distribution assumptions. Welch and Bishop's discrete Kalman filter tutorial explains the model and noise assumptions behind the recursion.
For a real robot, check timestamp alignment, sensor calibration, and whether residuals show bias or time correlation. Compare predictions against independent measurements across varied motion. Unmodeled acceleration, delayed observations, correlated sensors, and bad associations need attention beyond making the displayed band wider.
For a nonlinear range-and-bearing sensor, the extended Kalman filter evaluates the motion and measurement models, then uses their Jacobians to approximate uncertainty locally. Its worked update shows why the reported covariance can shrink around a poor estimate. SLAM also estimates a map and must manage the links between robot and landmark uncertainty.
Reproduce the filter in Python
This standard-library example uses the same prior, records, and Joseph update as the lab. The variable names pp, pv, and vv store the three unique covariance entries. No reference position enters filter_record.
from math import sqrt
readings = [0.4, 1.2, 1.7, 3.4, 3.9, 4.8, 6.3, 6.6, 8.2, 9.1, 9.8]
def filter_record(record, sigma_a=0.2, sigma_z=0.5):
p, v = 0.0, 0.0
pp, pv, vv = 4.0, 0.0, 1.0
q, r = sigma_a ** 2, sigma_z ** 2
result = []
for k, z in enumerate(record):
if k > 0: # dt = 1 s; k=0 uses the supplied initial prior
p += v
pp, pv, vv = pp + 2*pv + vv + q/4, pv + vv + q/2, vv + q
if z is not None:
innovation = z - p
kp, kv = pp / (pp + r), pv / (pp + r)
p, v = p + kp*innovation, v + kv*innovation
remaining = 1 - kp
pp, pv, vv = (
remaining**2 * pp + kp**2 * r,
remaining * (pv - kv*pp) + kp*kv*r,
vv - 2*kv*pv + kv**2 * (pp + r),
)
result.append((p, v, sqrt(pp)))
return result
gain = 4 / (4 + 1)
print(f"scalar K={gain:.6f}, mean={gain*2:.6f}, variance={(1-gain)*4:.6f}")
records = {
"baseline": readings,
"dropout": [None if 4 <= k <= 6 else z for k, z in enumerate(readings)],
"biased": [z + (2 if k >= 4 else 0) for k, z in enumerate(readings)],
}
for name, record in records.items():
states = filter_record(record)
for k in (6, 10):
p, v, sigma_p = states[k]
print(f"{name} {k}s: p={p:.6f}, v={v:.6f}, sigma_p={sigma_p:.6f}")
Expected output:
scalar K=0.800000, mean=1.600000, variance=0.800000
baseline 6s: p=6.072409, v=1.054274, sigma_p=0.384624
baseline 10s: p=9.935972, v=0.967141, sigma_p=0.383521
dropout 6s: p=5.960773, v=0.966819, sigma_p=1.340561
dropout 10s: p=9.933802, v=0.984656, sigma_p=0.386031
biased 6s: p=8.287431, v=1.516665, sigma_p=0.384624
biased 10s: p=12.018721, v=0.961668, sigma_p=0.383521
Check two updates yourself
Exercise 1: Give a less precise scalar reading its proper weight
Keep prior mean 0 m, prior variance 4 m², and reading 2 m. Increase the sensor standard deviation to 3 m. Find R, the gain, the posterior mean, and posterior variance.
Solution: R = 3² = 9 m². Then K = 4/13 ≈ 0.307692, m⁺ = 8/13 ≈ 0.615385 m, and P⁺ = 36/13 ≈ 2.769231 m². The higher assumed measurement variance gives the same reading less influence.
Exercise 2: Predict through one missing measurement
Start with mean [3 m, 2 m/s] and P = [[1, 0.2], [0.2, 0.5]] in the appropriate units. Use Δt = 1 s and σₐ = 0.2 m/s². The next position reading is missing: find the new mean and covariance.
Solution: The mean becomes [5 m, 2 m/s]. Add Q = [[0.01, 0.02], [0.02, 0.04]] after propagating P, giving P⁻ = [[1.91, 0.72], [0.72, 0.54]]. Keep these predicted values because no correction occurs; the new position standard deviation is √1.91 ≈ 1.382027 m.
A single Gaussian summarizes one concentrated estimate. When several separated positions explain the observations, particle-filter localization can retain several hypotheses as weighted samples. Its corridor example separates prediction, measurement weighting, and resampling.
For a configured ROS estimator, continue with robot_localization in ROS 2. Its separate odometry/IMU fixture shows how selected fields and a biased gyro affect the estimated path.
Sources and further study
- Greg Welch and Gary Bishop, An Introduction to the Kalman Filter, discrete-filter chapter: the authors' tutorial, preserved by the University of Edinburgh, covers the linear model and recursive estimation equations.
- WPILib, State Observers and Kalman Filters: a robotics implementation guide to covariance, prediction, and correction timing.
- Roger Labbe, FilterPy Kalman filter source: an inspectable implementation of the Joseph covariance update.
- Roger Labbe, FilterPy process-noise models: the discrete acceleration covariance and its distinction from a continuous white-noise model.
Next, draw the joint uncertainty ellipse. Its tilt explains information that separate position and velocity bands leave out.