Skip to content
ELECTRONICS ENGINEERINGelectronics-engineering.com
Digital Signal Processing

The Kalman Filter Explained Simply: Equations and Example

Prediction and correction, the five Kalman filter equations, a scalar example and the EKF and UKF variants for nonlinear systems.

https://www.electronics-engineering.com/en/the-kalman-filter-explained-simply-equations-and-example/

Was this page useful? Rate it:
average 5.0 from 1 rating
The Kalman Filter Explained Simply: Equations and Example

The Kalman filter is an algorithm that estimates the state of a system (a position, a velocity, a temperature…) by combining two imperfect pieces of information: what the model of the system predicts and what the sensors measure. Neither is one hundred percent reliable; the filter blends them, giving more weight to the one that is, at that moment, more credible.

It is found wherever there are noisy measurements to clean up: in GPS receivers fused with accelerometers and gyroscopes, in radar tracking, in drones, in guidance systems, in economic forecasting. It was proposed by Rudolf Kálmán in 1960 and one of its first uses was the navigation of the Apollo missions.

The idea in two lines

At each step the filter does two things. First it predicts: it uses the model to say where the system should be now, and by how much the uncertainty has grown. Then it corrects: it compares the prediction with the new measurement and moves the estimate towards the measurement by an amount proportional to the relative confidence. If the sensor is very noisy the correction is small; if the model is not very reliable the estimate follows the measurements more closely.

What you need to build it

  • a sequence of measurements of the system;
  • a linear model that describes how the state evolves over time and how the measurements depend on the state;
  • a statistical description of the noise: how uncertain the model is (matrix Q) and how noisy the measurements are (matrix R).

In formulas, the system is described like this:

    \[ x_k = A\,x_{k-1} + B\,u_k + w_k \qquad z_k = H\,x_k + v_k \]

where x_k is the state, u_k a possible known input, z_k the measurement, and w_k, v_k are zero-mean Gaussian noises with covariances Q and R.

The equations

Prediction: the estimate and its covariance P are carried forward.

    \[ \hat{x}_k^- = A\,\hat{x}_{k-1} + B\,u_k \qquad P_k^- = A\,P_{k-1}\,A^T + Q \]

Correction: the Kalman gain K is computed, the estimate is corrected with the difference between the measurement and the predicted measurement (the “innovation”) and the covariance is updated.

    \[ K_k = P_k^- H^T \left( H P_k^- H^T + R \right)^{-1} \]

    \[ \hat{x}_k = \hat{x}_k^- + K_k \left( z_k - H\,\hat{x}_k^- \right) \qquad P_k = \left( I - K_k H \right) P_k^- \]

That is all: five equations repeated at every new measurement. The filter does not have to keep the history of past measurements, it only needs the estimate and the covariance of the previous step, which is why it runs without problems even on a microcontroller.

A scalar example

Suppose we want to measure a temperature that stays constant with a sensor that has a noise of 0.5 °C standard deviation (R = 0{,}25). The model is trivial: A = 1, H = 1, no input, and a very small Q because the temperature does not change. Starting from any initial estimate with a large P, at the first measurement the gain K is almost 1 and the estimate jumps practically onto the measured value. As more measurements arrive P decreases, K falls towards zero and each new reading moves the estimate less and less: the filter behaves like an average that becomes progressively more “stubborn”. With a slightly larger Q, on the other hand, K settles at an intermediate value and the filter keeps following the variations, like a low-pass filter that adjusts itself.

In practice the choice of Q and R is the most delicate part. R is often obtained from the sensor datasheet or by recording some measurements with the system at rest; Q is tuned by trial: too small and the filter reacts late to real changes, too large and it lets noise through.

When the system is not linear

The equations above hold for linear systems with Gaussian noise, and in that case the Kalman filter is the optimal estimator. Many real systems, however, are not linear: a radar measures range and angle, while the state is in Cartesian coordinates. In such cases we use the Extended Kalman Filter (EKF), which linearises the model around the current estimate with the Jacobian matrices, or the Unscented Kalman Filter (UKF), which propagates a small set of sample points through the nonlinear functions and is generally more accurate when the nonlinearities are strong.

An example of a Kalman filter run in MATLAB

Learn more

DESCRIPTION AND COMPLETE ALGORITHM

Presentation of the filter with the details of all the steps of the algorithm.

LESSON WITH AN EXAMPLE (UNIVERSITY OF PADUA)

Lecture notes from the course on the design of control systems, with a worked example.

Subscribe
Notify of
guest
1 Comment
Oldest
Newest Most Voted
1
0
Would love your thoughts, please comment.x
()
x