Skip to content
Tutorials Center
My learning

Topics

Lesson 26 of 38 ยท Localization and Mapping

The Kalman Filter for Robots

Understand the Kalman filter as a Bayes filter with Gaussians. Build 1D and 2D constant-velocity filters in Python and learn about EKF and tuning Q and R.

A robot wants a smooth, accurate estimate of its state (position, velocity, heading) from noisy sensors and an imperfect motion model. The Kalman filter is the classic answer. It is a Bayes filter where the belief is always a Gaussian: a single bell curve described by a mean and a variance (or covariance matrix for several variables). Because Gaussians stay Gaussian under linear operations, the whole update reduces to a few lines of matrix algebra, cheap enough for a microcontroller.

The filter went to the Moon on Apollo and today it runs inside nearly every phone, drone, and robot. In this lesson you will build it from scratch for one variable, then for a moving object with unmeasured velocity.

The idea in one picture

Every cycle has two steps, the same as the Bayes filter:

  1. Predict. Use the motion model to push the estimate forward. Uncertainty grows because motion is noisy.
  2. Update. Compare the prediction with a measurement. Blend them with a weight called the Kalman gain. Uncertainty shrinks.

The gain K is between 0 and 1 (for one variable). If the measurement is very precise compared with the prediction, K is near 1 and you trust the sensor. If the sensor is noisy, K is small and you trust the prediction.

Kalman filter cycle

Predict Move the estimate. Variance grows.
Measure Read the sensor with its own noise.
Blend Kalman gain weighs prediction against sensor.
Shrink Variance falls after every update.
1 / 2

One-dimensional Kalman filter

State x (a position) with variance P. The robot is told to move by u each step, with process noise variance Q. A sensor gives position z with variance R.

Predict: x = x + u, P = P + Q

Update: K = P / (P + R), x = x + K (z - x), P = (1 - K) P

The term z - x is the innovation: how surprised the filter is by the measurement.

Python 1D Kalman filter tracking a moving robot

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

Watch the gain: the first update has K near 1 (the filter has no prior, so it believes the sensor) and then decreases toward a steady value. The variance P settles at a level that balances model noise and sensor noise. The estimate hugs the true position more closely than the raw measurements do.

Several variables: position and velocity

Real robots have vectors of state. A common model is constant velocity in 1D: the state is x = [position, velocity], and we only measure position. The filter infers the hidden velocity from how the position changes.

The matrix form is the same:

  • Predict: x = F x, P = F P F^T + Q
  • Update: S = H P H^T + R, K = P H^T S^-1, x = x + K (z - H x), P = (I - K H) P

Here F is the state transition matrix, H the measurement matrix (it picks out position), Q the process noise covariance, and R the measurement noise.

Python Estimating velocity nobody measures

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

Although the sensor never reports speed, the estimated velocity converges to about 2 m/s, because the filter sees position change over time. The covariance matrix P has off-diagonal terms that link the two variables: that correlation is how information about one improves the other.

Tuning Q and R

The filter is only as good as Q and R.

Symptom Likely cause Fix
Estimate lags behind real motion Q too small (filter too sure of its model) Increase Q
Estimate is jittery, copies the noise R too small or Q too large Increase R or reduce Q
Filter ignores a sensor jump after a failure Overconfident P Add inflation or reset covariance
Estimate diverges Wrong model or unmodeled bias Fix F, add bias states

Measure R from the sensor's datasheet or by recording stationary data and computing its variance. Tune Q by trial, using logged data.

Beyond linear: EKF and UKF

The Kalman filter is exact only for linear models with Gaussian noise. Robots are rarely linear: a wheeled robot's position depends on sin and cos of its heading, and a range-and-bearing sensor is non-linear.

  • The Extended Kalman Filter (EKF) linearizes the models around the current estimate using Jacobians. It is the standard in localization, SLAM and IMU/GPS fusion.
  • The Unscented Kalman Filter (UKF) pushes a few carefully chosen points (sigma points) through the real non-linear function and re-estimates mean and covariance. It avoids computing Jacobians and handles stronger non-linearity.
  • When the belief has several separate peaks (the robot could be in either of two hallways), a single Gaussian cannot represent it. Use a particle filter.

Always log the innovation (z - H x) and its expected size sqrt(S). If innovations regularly exceed three standard deviations, your noise values or model are wrong. This simple check catches most filter bugs, and it is the basis of "gating" outliers.

Check your understanding

Quick check ยท 4 questions

of 4 answered You scored / 4 ยท perfect!

  1. 1. What kind of belief does a Kalman filter keep?

    The Kalman filter represents the state as a Gaussian, which is why updates are cheap closed-form matrix operations.
  2. 2. If the sensor is much noisier than the prediction (large R), the Kalman gain is...

    K = P / (P + R). A large R makes K small, so the measurement barely changes the estimate.
  3. 3. What happens to the uncertainty (variance) during the predict step?

    Predict adds process noise Q. Only the measurement update reduces uncertainty.
  4. 4. Why use an Extended Kalman Filter?

    The EKF linearizes the non-linear functions around the current estimate so the Kalman equations still apply.

Summary

  • The Kalman filter is a Bayes filter with Gaussian beliefs: predict (uncertainty grows), then update (uncertainty shrinks).
  • The Kalman gain balances prediction against measurement according to their variances.
  • Hidden variables such as velocity can be estimated from other measurements via correlations in the covariance.
  • Q and R are tuning knobs; watch the innovation to check them.
  • EKF and UKF extend it to non-linear systems; particle filters handle multi-modal beliefs.

Next: Particle Filter Localization