Kalman filter is a “smart” signal processing algorithm that takes advantage of the way the Universe works and even predicts the future to overcome measurement noise and deliver timely, accurate readings.

This algorithm is ubiquitous: it’s used in autonomous navigation systems like Robot Localization in ROS, for Object Tracking in OpenCV, and inside actuators like Dynamixel where measurements from the absolute encoder are fused with measurements from the quadrature encoder to enable precise positioning.

This filter was first used in the Apollo space program and various defense projects in the early 1960s. It was introduced by Rudolf E. Kálmán in 1960 in his seminal paper, A New Approach to Linear Filtering and Prediction Problems, published in the Journal of Basic Engineering.

In this article I introduce the Kalman Filter in the shortest way possible with practical examples. When you’re ready to go deeper check out Kalman Filter from the Ground Up by Alex Becker at kalmanfilter.net:

A basic moving average filter can be implemented as follows:

Compare the filter result to the original data with buffer size 8, 32, and 64:

Filtered data has been shifted forward on the time axis proportionately to the smoothing amount, introducing delay:

• In the case of DC motor control for robotics, a moving average filter would cause the PID algorithm to oscillate, as it would see this delay as a signal that it’s not working hard enough, and over-compensate.
• In the case of a vehicle, a moving average would cause the estimated position to lag too far behind, causing the navigation system to suggest changes to the route too late for the driver to respond.

A basic low-pass filter can be implemented as follows:

Compare the filter result with 0.1 and 0.001 coefficients to the original data:

This filter missed short temporal spikes in the data. This may be fine if the system is cyclical in nature, but if the subsequent state depends on previous system state, missing events could cause it to diverge far from reality.

In the following video we test both filters on real data:

If you know what a moving vehicle or a motor shaft are supposed to do (since you are controlling them in the first place), why should you let measurements have so much influence on where they are estimated to be next?

In the case of a DC motor, commanded velocity can be used to determine the current shaft position:

In the case of a moving vehicle, the pressure on the accelerator pedal can determine the velocity and position:

Prediction can be a valuable tool provided the model is detailed enough. The Kalman filter’s key strength lies in its ability to combine measurements with predictions, depending on which offers the most accurate estimate.

A basic Kalman filter algorithm looks something like this:

1. Start with an initial estimate
2. Take a measurement
3. Correct the estimate by the measurement
4. Predict the next estimate

We will add more detail to these steps in the filter algorithm section later.

Another special thing about the Kalman filter is that its output isn’t simply a value but a likelihood of it being a certain value. This concept is borrowed from statistics where it’s called a random variable.

• A random variable is defined by a mean and a spread from that mean.
• The mean is a sum of a set of samples divided by the size of the set. This represents the center of the random variable.
• The spread from the mean is called variance and it’s denoted by the greek letter σ (sigma).
• The shape of this spread is a “bell curve” called a Gaussian distribution.

Measuring physical quantities results in readings that follow this Gaussian distribution due to the Holographic Principle of the Universe:

When the Universe is “sampled” by taking a measurement, the sample is interpolated by using a strategy discovered by Carl Gauss: surrounding bits are blended in depending on distance of the sample from the “center” of each bit.

Variance can be calculated from a set of samples as follows:

1. Calculate the mean
2. For each number in the set, calculate distance from the mean (subtract mean from the number)
3. Square each distance from the mean, which will give you variance for this item in the set
4. Calculate the mean of all variances in the set (variances summed over number of items). This will give you the variance of the set.

In Matlab or Octave, mean and variance can be calculated as follows:

The following video demonstrates how to calculate variance:

Since the filter estimate is a random variable, the algorithm has to keep track of its uncertainty, making the same changes to the estimate uncertainty as the ones made to the estimate mean value.

The measurement is also a random variable and its uncertainty could change each iteration or remain constant.

If we include the additional steps for tracking estimate and measurement uncertainties, the algorithm would look like the following:

1. Start with initial estimate and its uncertainty
2. Take a measurement and determine its uncertainty
3. Correct the estimate by the measurement
4. Correct the estimate uncertainty by the measurement uncertainty
5. Predict the next estimate
6. Predict the next estimate uncertainty

Kalman filter starts with an initial estimate, the uncertainty of this estimate, and an initial state of the system being measured.

Each iteration, the system state is corrected by the last measurement according to the ratio of estimate uncertainty to measurement uncertainty.

The next estimate is then predicted by evaluating the system model with the corrected state:

The next few sections will explain the algorithm parameters in more detail.

The initial estimate y0 can come from initial measurement, output of the system model with initial state, or an educated guess.

A more accurate guess will let the algorithm converge on the optimal estimate faster and avoid a “jump” when the filter performs a large correction.

Each iteration begins by taking a measurement (labeled z in equations) and determining its uncertainty or variance, typically denoted by R.

Measurement variance can be constant or vary based on conditions:

• Hall effect encoders like AKSIM report the distance of the rotating magnet from the center of the sensor because this affects the accuracy of the readings. This could be used to derive a measurement variance.
• Complex measurement devices like radar let you estimate the dynamic effects of wind and rain on their signal-to-noise ratio to derive a measurement variance.
• A basic potentiometer can be sampled in a range of expected conditions to calculate a constant variance (see variance).

To produce optimal estimates, the Kalman filter has to know if the estimates are more or less reliable than measurements.

The ratio of estimate uncertainty to measurement uncertainty is calculated at each iteration. This ratio is called the Kalman gain (typically denoted by K) and used as a weight to correct predictions with measurements.

Similar to other iterative optimization algorithms like Gradient Descent, the Kalman filter has to calculate an error at each iteration, defined as the difference between the prediction and the measurement.

This error is then used to correct the model, with the correction amount determined by the Kalman gain:

• If the measurements are trusted more, the Kalman gain will be closer to 1 which will correct the model prediction by a greater amount.
• If the model is trusted more, the Kalman gain will be closer to 0 which will correct the model prediction by a lesser amount.

Since estimates are a function of the model internal state, we can’t correct them directly. Instead we change the model state to get the desired output:

After correcting the model state, the estimate uncertainty is updated by blending measurement uncertainty with model uncertainty:

If the estimate was based more on the measurement, then the estimate uncertainty reflects more of the measurement uncertainty.

Conversely, if the estimate was based more on the model prediction, then the estimate uncertainty reflects more of the model uncertainty.

The Kalman filter borrows from System Identification theory which states that most systems (heaters, motors, hydraulic pumps, you name it!) can be approximated by a linear model of the following form:

The system output y depends on system state x and input u.

The relationship between system output and system state is described by C measurement matrix and the relationship between input and immediate output is described by D input contribution.

Differences between the real system and its model are represented by e as disturbance or noise. This is a theoretical term that’s there in the equation to represent “unknowns” and does not appear in the code.

When modeling systems, you’ll find that the input contribution to system output D is almost always 0 because real-world systems are not instantly affected by inputs. Instead, the inputs modify system state which eventually changes the output.

The following equation is used to update (or transition) the state of a linear system model at each iteration:

This equation involves the following terms:

• x is the system state being updated. The new state is based on the previous state so this term appears on both sides of the equation. The initial system state is usually denoted by x0.
• u is the input provided at this iteration.
• A is the state transition matrix. Multiplying the system state by this matrix will “simulate” the system, advancing it forward by one time step.
• B is the input matrix or control matrix that represents the weight of input on each system state variable. Multiplying the input by this matrix will change each system state variable by a different amount depending on how it’s affected by the input.
• e represents the noise or disturbance that affects the system state transition. This is a theoretical term used to model unknowns so it does not appear in the code.
• K represents the influence of noise on each state variable. Since e is not used in a practical implementation, we don’t use this term either.

For a system model with a single state variable, the initial estimate and disturbance uncertainties are represented by scalar variances.

For a system with multiple state variables, the initial estimate and disturbance uncertainties are represented by square covariance matrices with the same number of rows and columns as there are state variables.

In a covariance matrix, the diagonal entries encode variances, and the off-diagonal entries encode covariances.

Estimate uncertainty is typically denoted by P, and the uncertainty of initial estimate is denoted by P0.

For systems with only one state variable both terms are scalar variances, otherwise they are covariance matrices.

A covariance matrix that encodes the estimate uncertainty of a system with two state variables (position and velocity) would look like this:

This matrix encodes the following information:

• The variance of position (how far the estimated position could be from the true position, squared)
• The variance of velocity (how far the estimated velocity could be from the true velocity, squared)
• The covariance of position and velocity (the mathematical relationship between position and velocity random variables), with 0 indicating the two variables are not related.

A covariance matrix that encodes the estimate uncertainty of a system with three or more state variables follows the same pattern:

There are a few ways to initialize the estimate uncertainty:

• If the approximate variances of state variables are known, they can be projected on the diagonal of the covariance matrix:

  covariance = diag([ ...
    100, ...
    200, ...
    300 ...
  ]);
  
  covariance =
  
     100     0     0
       0   200     0
       0     0   300

• If the system was identified with Control System Toolbox, the standard deviations of state variables are available in dx0 property. Squaring elements of dx0 to convert standard deviation to variance and projecting them on the diagonal will give you the estimate covariance:

  % ss - identified system
  covariance = diag(ss.dx0.^2);
  
  5.5288e+13            0            0
            0   1.5450e+19            0
            0            0   2.6517e+21

• If you have a data set that records system state over time you could use Matlab’s cov function to initialize the estimate covariance:

  % CSV with position, velocity, acceleration
  dataset = readmatrix("states.csv");
  covariance = cov(dataset);

• If you can express the variance of the system model in terms of tolerance by using it in a sentence like “this estimate is within x or +/- x”, then square half the tolerance amount to convert it to variance.

  The model variance can then be back-projected through the measurement matrix to determine the variance of the initial estimate:

  modelTolerance = 50
  modelVariance = (modelTolerance / 2)^2;
  
  covariance = diag( ...
    inv(C) * ...
    eye(length(state)) * modelVariance ...
    * inv(C)' ...
  );

  Multiplying variance by another quantity requires squaring that quantity. To make this work with matrices, the variance is pre-multiplied by the matrix and post-multiplied by the transpose of the same matrix. This is why you’ll often see the pattern X * y * X'.

• Lastly, you could assume the same variance for all initial state variables, leaving covariances blank:

  variance = 200;
  covariance = eye(length(state)) * variance
  
  covariance =
  
     200     0     0
       0   200     0
       0     0   200

These initialization strategies work because the Kalman filter will eventually converge on a more accurate estimate covariance.

However, significantly over-estimating initial estimate uncertainty will cause the algorithm to ignore model predictions while significantly over-estimating the measurement variance will cause the algorithm to ignore measurements.

A process noise or disturbance covariance matrix usually denoted by Q looks exactly like estimate covariance, but it encodes the variance of Gaussian noise affecting the change made to each state variable within a single iteration.

There are a few ways to initialize the Q covariance matrix:

• Similarly to estimate covariance, the approximate variance of noise or disturbance affecting all state variables in the same way could be projected by spreading the same variance across the diagonal:

  noiseVariance = 200
  noiseCovariance = eye(length(state)) * noiseVariance

• If the approximate variance of noise introduced by updating the system state is known, it could be projected by using the state transition matrix:

  noiseVariance = 200;
  noiseCovariance = A * noiseVariance * A'

  For systems identified with System Identification Toolbox, the noise variance is available in NoiseVariance property of the model:

  % ss1 - linear system model
  noiseCovariance = eye(length(state)) * ss1.NoiseVariance

• If the approximate variance of noise introduced by the system input is known, it could be projected by using the control matrix:

  inputNoiseVariance = 200;
  noiseCovariance = B * inputNoiseVariance * B'

• If the approximate variances of noise or disturbances affecting each state variable are known, they could be used to fill in diagonal entries of the covariance matrix, leaving off-diagonal entries blank:

  noiseCovariance = diag([ ...
    positionNoiseVariance, ...
    velocityNoiseVariance, ...
    accelerationNoiseVariance ...
  ])

• If the system was identified with Control System Toolbox, the covar function will output its disturbance covariance:

  noiseVariance = 200;
  
  [ ...
    % Initial estimate variance
    P, ...
    % Disturbance covariance
    Q ...
  ] = covar(ss, noiseVariance)

• Finally, you could simulate your system model with the same data used to identify it, and calculate the difference between the original measurements and the system model at each iteration.

  Since the system model includes a measurement matrix which maps system state to system output, you could multiply the difference between the original measurement and the model prediction by the inverse of this matrix to calculate the noise affecting each state variable at each iteration.

  With these results recorded in a separate vector for each state variable, you could then use cov to generate a disturbance matrix.

Since Kalman filter estimates are random variables represented by mean and variance, anything that we do to the mean of the estimate must also be reflected in its uncertainty.

Earlier we corrected the estimate uncertainty by the same amount (K) used to correct the estimate:

In this step, we transition the estimate uncertainty by the same amount (A) used to transition the estimate:

Recall that A is the state transition matrix that represents the change made to the system state at each iteration.

The noise covariance matrix Q represents how much uncertainty is added at each iteration by simulating the system based on an imperfect model and using noisy inputs to change the system state.

The system model can be derived by analyzing the system (as we did in the overly simplified examples of a vehicle and PWM-controlled motor in why predict? section earlier), or by using system identification.

In the following video we’ll identify a system in Matlab Control System Toolbox using a discrete linear state-space model:

Identifying a system refers to finding A, B, C, D coefficients that make the output of the linear model most closely resemble the original measurements. These coefficients can then be used as parameters to the Kalman filter.

The best way to compare the identified systems to original measurements is by viewing the Model Output in System Identification app:

This will simulate any identified models and display their outputs alongside the original measurements:

The properties of a system identified by Control System Toolbox can be extracted by using an object property notation, for example:

We will use the following properties in this article:

• A, B, C, D are the coefficients described earlier.
• x0 is the initial system state.
• dx0 is the uncertainty of initial state.
• NoiseVariance is the variance of Gaussian noise affecting the system.

For more background on system identification, try this series of tutorials assembled by two professors at Carnegie Mellon university.

The quickest way to simulate a linear system is by using lsim in Matlab:

Calling lsim without storing results in a variable will display a plot while assigning to a variable will store the simulated outputs in that variable.

It’s useful to understand how linear system equations presented earlier are used by lsim under the hood to simulate the system.

See the complete Matlab and C++ examples demonstrating how to simulate a linear system in systemid companion repository.

Live Kalman filter demonstration that lets you load a file and tweak parameters:

The kalman companion repository includes a complete filter implementation in addition to two live notebooks that explain the filter algorithm parameters.

The constantAcceleration.mlx notebook implements a Kalman filter that works with a simpler Newtonian motion model:

The linearSystemModel.mlx notebook implements a Kalman filter that works with a general-form linear discrete system model:

Finally, kalman.m demonstrates a Kalman filter with the same linear system model in the form of a .m script:

The kalman companion repository includes the following example. Steps that can be used to compile, run, and debug the code are in the readme file: