Author: Arthur Lovekin. Originally Published: 2024-10-27
| Control the robot's acceleration by dragging the number: \( \controlInput = \) |
|
|
The Kalman Filter is a powerful tool for combining information in the face of uncertainty, and is used widely throughout science and engineering for tracking time-varying systems. In this article, I hope to teach you how deep principles in probability and linear algebra (which you probably already know!) naturally lead to the equations of the Kalman Filter. I've made a series of interactive diagrams that will give you a better feel for the different parameters, and help you understand both the power and limitations of this method.
This is meant as an introduction that can kickstart your progress without having to digest an entire math textbook. That being said, the theory behind the Kalman Filter goes a lot deeper than what I show here, including theoretical proofs of optimality and extensions to nonlinear systems. At the end of the article, I've listed the resources I used to write this article, which I highly recommend if you want to dive deeper.
There are over 17 variables that are commonly used to derive the Kalman Filter, and to make matters more confusing many resources use different naming conventions! To alleviate this pain, I've made it so that you can choose the naming convention using the dropdown menu below. Also, you can click on any variable throughout the article to get more information about it.
The primary goal of the Kalman Filter is to estimate the state of a dynamical system. The need for state estimation shows up throughout engineering. To give a few examples, if you're driving a car, you need to know where you are and how fast you're going before you can intelligently turn the steering wheel. If you're controlling the central heating in an office building, it is helpful to know the temperature in each room. If you're designing a brain-computer interface for paralyzed people [4], your entire aim might be to estimate the intended position of a cursor on a screen based on their noisy neural signals.
In all of these cases, there are multiple sources of information that we can use to estimate the state of the system. On one hand, we have some background knowledge of how the system should generally behave (eg. the dynamical model of the car, building, or brain). On the other hand we have sensors (eg. cameras, thermometer, or brain electrodes), that can measure some aspect of the state. The challenge is that neither of these information sources is perfect: our dynamical model doesn't account for common disturbances, and although we often have many sensors, they are usually noisy, contradictory, and cannot individually perceive the full state. To get the most reliable state estimate, we need to continuously combine the information from the sensors and the dynamical model as time progresses.
The Kalman Filter is an algorithm that incorporates new pieces of information as they come in over time into one unified state estimate. It does this in two steps:
To understand the details of the Kalman Filter, we'll use the example of a simple robot. The goal will be to estimate the position and velocity of the robot so that it can navigate without crashing. On its back, the robot carries an ultrasonic sensor that can measure the distance to the nearest wall, and you as the operator will be able to control the acceleration.
The first step in building an engineering model is to define a state vector. For our robot, this will be a two-dimensional vector containing the \(p\) and velocity \(v\). In general, the state vector simply contains the minimum set of variables needed to make a complete description of the system (so in other examples it might contain things like temperatures, voltages, etc.). One caviat is that practically speaking, the Kalman Filter algorithm will only ever be able to estimate the state, not work with the true state values. For this reason, we'll add a little hat to indicate that this vector is just an estimate. $$ \text{Estimated State vector }\stateEstimate = \begin{bmatrix} p \\ v \end{bmatrix} = \begin{bmatrix} \text{position} \\ \text{velocity} \end{bmatrix} $$
Once we've defined our state vector, we need to model how it evolves over time. For our robot, we can use a simple kinematics formula: $$ \begin{alignat}{3} p_{\discreteTime} &= p_{\discreteTime-1} &+& \Delta t &v_{\discreteTime-1} \\ v_{\discreteTime} &= 0 &+& &v_{\discreteTime-1} \\ \end{alignat} $$
We'll package this up into matrix form so it is easy to use later. We'll call the matrix that converts from the old state to a new state using the dynamics model the state transition matrix \(\stateTransition\). $$ \text{State Transition Matrix } \stateTransition = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} $$ $$ \stateEstimate_{\discreteTime} = \stateTransition \stateEstimate_{\discreteTime-1} \\ $$
Now we have a simple dynamical model of our robot, but it fails to include one
common thing: a way to control the robot. In the interactive demo above, you
(as the operator) are able to control the linear acceleration \(a\) via the control input
vector \(\controlInput\). This [-1,1] throttle input doesn't directly control the
linear acceleration, but rather dictates the angular acceleration of the wheels.
If we assume that the wheel has radius \(r\), the maximum angular acceleration
is \( \alpha_{\text{max}} \), and that the wheel perfectly tracks the control
command, then we can extend our kinematics equation to include the control input:
$$ \begin{alignat}{3}
a &= r \alpha_{\text{max}} u \\
p_{\discreteTime} &= p_{\discreteTime-1} &+& \Delta t &v_{\discreteTime-1} &+& \frac{1}{2} (r \alpha_{\text{max}} u) \Delta t^2 \\
v_{\discreteTime} &= 0 &+& &v_{\discreteTime-1} &+& (r \alpha_{\text{max}} u ) \Delta t \\
\end{alignat}
$$
As with the state equation, we'll package this up in matrix form by defining
the control matrix \(\controlMatrix\), which determines how the input
vector \(\controlInput \) impacts the state.
$$ \text{Control Input Vector } \controlInput = \begin{bmatrix} u \end{bmatrix} $$
$$ \text{Control Matrix } \controlMatrix = \begin{bmatrix} \frac{1}{2} r \alpha_{\text{max}} \Delta t^2 \\ r \alpha_{\text{max}} \Delta t \end{bmatrix} $$
Combining this with our dynamics model, our complete dynamical model looks like this:
$$
\stateEstimate_{\discreteTime} = \stateTransition \stateEstimate_{\discreteTime-1} + \controlMatrix \controlInput_{\discreteTime} \\
$$
In summary, we
The linear model we derived above is great, but it's not very realistic. In the real world, the robot might slip and bounce over the terrain, the motors might not exactly reach the commands, and so on. In other words, there is random noise introduced by the messiness of the environment, which in turn makes our state estimate less certain.
One method that can represent both the uncertainty in our state estimate and the random noise introduced by the environment is to assign each of them a probability distribution. Essentially, we acknowledge that at a given instant we do not know the precise value of the state, but we can still assign a probability to the state lying within a given range (where that probability is the area under the probability distribution). For simplicity, we assume that both the state and noise distributions are gaussian.
So now, instead of representing our state estimate with a single vector, we'll say that our state estimate is a normal distribution parametrized by a mean \(\state\) and a covariance \(\stateCovariance\). $$ \stateEstimate \sim \gaussian \left( \state, \stateCovariance \right) $$
|
Hopefully you're already familiar with a 1D normal distribution, which is usually parametrized by its mean \(\mu\) and variance \(\sigma^2\). The multivariate version is similar, except now the mean is an n-dimensional vector (in our case, 2-dimensional), and the covariance is an n x n matrix whose (\(i,j\))th element represents the covariance between the \(i\)th and \(j\)th state variables. $$ \text{Mean State Vector } \state = \begin{bmatrix} \bar{p} \\ \bar{v} \end{bmatrix} $$ $$ \text{State Covariance Matrix } \stateCovariance = \begin{bmatrix} \text{Var}(p) & \text{Cov}(p,v) \\ \text{Cov}(v,p) & \text{Var}(v) \end{bmatrix} $$
Now comes an important point: In our dynamics equation above we know how to apply the matrix \(\stateTransition\) to the state estimate \( \stateEstimate \) when it was a single vector. But now our estimate is a gaussian, so we need to apply \(\stateTransition\) to the whole distribution. Fortunately, the mean is a single point, so we can directly apply the state transition matrix as we did before. The variance on the other hand is the average "squared-distance" from the mean, so how should it be treated?
We'll introduce a very useful rule for multiplying a gaussian distribution \(\gaussian \left( \mathbf{\bar{\mu}}, \mathbf{\Sigma} \right) \) by a matrix \( \mathbf{A} \):
$$ \begin{align} \text{If you linearly transform the mean: } & \mathbf{\bar{\mu}}_{\discreteTime} = \mathbf{A} \mathbf{\bar{\mu}}_{\discreteTime-1} \\ \text{ then the covariance is: } & \mathbf{\Sigma}_{\discreteTime} = \mathbf{A} \mathbf{\Sigma}_{\discreteTime-1} \mathbf{A}^T \end{align} $$
The other thing we need to know is that if we add or subtract two independent, normally distributed random variables, then their variances add. (see AP Central's explanation for a refresher.)
With this knowledge of probability, we have actually arrived to the point where we can write out the first component of the Kalman Filter: the Predict Step! We'll define the covariance of the state estimate to be \(\stateCovariance\), and the mean to be \(\state\). To handle environmental noise, we'll also add a noise term with mean 0 and covariance \(\processNoiseCovariance\). Writing this out, we have:
The Predict Step: $$ \state_{\discreteTime} = \stateTransition \state_{\discreteTime-1} + \controlMatrix \controlInput_{\discreteTime} $$ $$ \stateCovariance_{\discreteTime} = \stateTransition \stateCovariance_{\discreteTime-1} \stateTransition^T + \processNoiseCovariance $$ where $$ \text{State Estimate Distribution }\stateEstimate \sim \gaussian \left( \state, \stateCovariance \right) $$ and $$ \text{Process Noise} \sim \gaussian \left( 0, \processNoiseCovariance \right) $$
Fundamentally, the predict step describes how to
Drag the underlined values to see how they would affect one iteration of the dynamics model!
|
||||||||||||||||||||||||||||||||||
|
| Control the robot's acceleration by dragging the number: \( \controlInput = \) |
|
|
Ok so the state ... isn't terrible, but it's not great either, and the variance is exploding (which is reasonable considering the dynamics equation has no feedback). Fortunately, we have endowed this robot with an that should help. This sensor will output a measurement vector containing one value corresponding to the distance to the nearest object. $$ \text{Measurement Vector } \measurement = \begin{bmatrix} \text{distance} \end{bmatrix} $$ The only question is, how do we incorporate these measurements into the filter?
To start with, we need a linear model that relates the measurement vector \( \measurement \) to the state. Our first instinct might be to write a matrix that converts from the measurement vector to the state vector (we're trying to update the state after all). The problem is that since the sensor doesn't measure the velocity, there is no way to fully recover the state from the measurement using a linear transformation. We can however think about this from a more causal perspective:
In reality we know that the robot is in some true state \( \trueState \), and when the sensor takes a measurement of that state it will generally follow a linear process. If we define the measurement matrix \(\measurementMatrix\) to describe this linear measurement process, then in the ideal case we can write: $$ \measurement_{\text{ideal}} = \measurementMatrix \trueState $$
Let's get into some to see what \(\measurementMatrix\) looks like.
Most ultrasonic sensors do not directly provide a distance. Instead, they measure the time (in microseconds, \( \mu s \)) it takes for a pulse of sound to travel to the wall and back. In order to convert from \( \mu s \) to meters, we need to multiply by a factor of \( \frac{2 \times 10^6 }{c} \), where \(c\) is the speed of sound in air (343 \( m/s \)), there are \( 10^6 \mu s \) per second, and the factor of 2 accounts for the sound traveling double the distance (to the wall, and then back). Thus to convert from the measurement vector to the state vector, the measurement matrix would be: $$ \text{Measurement Matrix } \measurementMatrix = \begin{bmatrix} \frac{2 \times 10^6 }{c} & 0 \end{bmatrix} $$
Of course, no sensor is ever perfect, so we will also say that when the sensor takes a measurement it will also introduce some sensor noise. We assume that the noise is normally distributed with zero mean and covariance \(\measurementNoiseCovariance\) (which hopefully is listed in the sensor datasheet). $$ \text{Measurement Noise} \sim \gaussian \left( 0, \measurementNoiseCovariance \right) $$ Now we can say that when the robot is in state \( \trueState \), the measurements you can expect to get will follow a normal distribution with covariance \(\measurementNoiseCovariance\) and mean \(\measurementMatrix \trueState\). Of course, we never actually see the entirety of this distribution; the robot will only ever take individual measurements. Since \( \measurement \) is the only sample we have, we will say that every time we read some measurement, we infer that the distribution of possible measurements is a gaussian centered at \( \measurement \) with covariance \(\measurementNoiseCovariance\): $$ \text{Possible Measurements}\sim \gaussian \left( \measurement, \measurementNoiseCovariance \right) $$
To summarize, we've defined the measurement matrix \( \measurementMatrix \), whose role is to convert from the space of states to the space of measurements. We've used it here to convert from the ideal state to an ideal measurement, but in the next section we will see that \( \measurementMatrix \) can also be applied to the entire distribution of our current state estimate. This operation yields the measurement we expect given the current state estimate.
At this point, we have one normal distribution that represents our belief of the true state of the robot, and another normal distribution that represents the measurements you'd expect to get from the true state. How can we combine these two distributions into one new state estimate? $$ \text{State Estimate distribution from the Prediction Step: }\gaussian \left( \state, \stateCovariance \right) $$ $$ \text{Measurement distribution inferred from the actual measurement: } \gaussian \left( \measurement, \measurementNoiseCovariance \right) $$
The first challenge we face is that we're trying to combine different "types" of things. The state estimate distribution has a mean and variance with a particular dimension and units, while the measurement distribution has entirely different dimensions and units. Before we can do any combining, we have to first get both distributions into the same space.
There are two ways to do approach this. Choose your own adventure!
The other way we could approach combining the measurement distribution with the state estimate distribution can be outlined as follows:
Let's break this down.
In order to put our two distributions on the same footing, we'll multiply the measurement distribution by \( \measurementMatrix^{-1} \) to bring it into the space of states. Remembering our , we'll get two normal distributions in the same space:
(If you see something wrong here, suspend your disbelief and read on ... )
But how much, exactly, of each estimate do we want to use? One reasonable approach is to weight each distribution by the inverse of its variance, so the distribution with less uncertainty (ie. smaller variance) will play more of a role. To write this mathematically, we first define the variance ratio (in the space of states): $$ \varianceRatioState = \frac{\stateCovariance}{\stateCovariance + \measurementMatrix^{-1} \measurementNoiseCovariance (\measurementMatrix^{-1})^T} = \frac{\text{Uncertainty from state estimate}}{\text{Total uncertainty from the state estimate and sensor noise}} $$
Then we can use this to write a weighted sum of each distribution:
$$ \state = (\identityMatrix - \varianceRatioState) \state + \varianceRatioState \measurementMatrix^{-1} \measurement $$
Great! It seems like this weighted sum does exactly what we need.
There's only one problem: \(\measurementMatrix\) is not necessarily square, so it .
Well, that kind of throws a wrench in everything. Fortunately, there is a workaround! If we suspend our disbelief and do a bit of matrix algebra as if \( \measurementMatrix^{-1} \) exists, we can avoid ever having to invert the measurement matrix. We'll use the following identities: For any invertible matrices \(\mathbf{A}\), \(\mathbf{B}\) we have: \( (\mathbf{A} \mathbf{B})^{-1} = \mathbf{B}^{-1} \mathbf{A}^{-1} \), and \( (\mathbf{A}^{-1})^{-T} = (\mathbf{A}^T)^{-1} \), and we can always multiply anywhere by the identity \( \identityMatrix = \measurementMatrix^{-1} \measurementMatrix = \measurementMatrix^T (\measurementMatrix^T)^{-1} \)
So let's knock out those inverses! $$ \begin{align*} \varianceRatioState &= \stateCovariance \left[ \stateCovariance + \measurementMatrix^{-1} \measurementNoiseCovariance \left( \measurementMatrix^{-1} \right)^T \right]^{-1} \\ &= \stateCovariance \left[ \stateCovariance + \measurementMatrix^{-1} \measurementNoiseCovariance \left( \measurementMatrix^{-1} \right)^T \right]^{-1} \left( \measurementMatrix^{-1} \measurementMatrix \right)\\ &= \stateCovariance \left[ \measurementMatrix \left( \stateCovariance + \measurementMatrix^{-1} \measurementNoiseCovariance \left( \measurementMatrix^{-1} \right)^T \right) \right]^{-1} \measurementMatrix \\ &= \stateCovariance \left[ \measurementMatrix \stateCovariance + \measurementNoiseCovariance \left( \measurementMatrix^{-1} \right)^T \right]^{-1} \measurementMatrix \\ &= \stateCovariance \left( \measurementMatrix^T \left( \measurementMatrix^T \right)^{-1} \right) \left[ \measurementMatrix \stateCovariance + \measurementNoiseCovariance \left( \measurementMatrix^{-1} \right)^T \right]^{-1} \measurementMatrix \\ &= \stateCovariance \measurementMatrix^T \left[ \left( \measurementMatrix \stateCovariance + \measurementNoiseCovariance \left( \measurementMatrix^{-1} \right)^T \right) \measurementMatrix^T \right]^{-1} \measurementMatrix \\ \varianceRatioState &= \stateCovariance \measurementMatrix^T \left[ \measurementMatrix \stateCovariance \measurementMatrix^T + \measurementNoiseCovariance \right]^{-1} \measurementMatrix \\ \end{align*} $$
Now our Variance ratio is free of \( \measurementMatrix^{-1} \), but there is still one more remaining in the weighted sum formula. In preparation to eliminate this remaining \( \measurementMatrix^{-1} \), we'll separate out the trailing \( \measurementMatrix \) from the rest of the formula. The variance ratio without the trailing \( \measurementMatrix \) is actually so important that we'll give it a name: $$ \text{The Kalman Gain } \kalmanGain = \stateCovariance \measurementMatrix^T \left[ \measurementMatrix \stateCovariance \measurementMatrix^T + \measurementNoiseCovariance \right]^{-1} $$ Then we can write the variance ratio as: $$ \varianceRatioState = \kalmanGain \measurementMatrix $$
Now we can proceed in writing our state update equation without any remaining \( \measurementMatrix^{-1} \): $$ \begin{align*} \state &= (\identityMatrix - \varianceRatioState) \state + \varianceRatioState \measurementMatrix^{-1} \measurement \\ \state &= \left( \identityMatrix - \kalmanGain \measurementMatrix \right) \state + \kalmanGain \measurement \\ \end{align*} $$
Great! Now we have a matrix formula to combine the means of two Gaussians. The only step left is to propagate the variances too, which we can easily do using our .
$$ \stateCovariance_{\discreteTime} = \left( \identityMatrix - \kalmanGain \measurementMatrix \right) \stateCovariance_{\discreteTime-1} \left( \identityMatrix - \kalmanGain \measurementMatrix \right)^T + \kalmanGain \measurementNoiseCovariance \kalmanGain^T $$
The equation above is known as the Joseph Form of the Covariance Update Equation.
Here's an interactive diagram to visualize the whole process. The work done by the Kalman Gain is surrounded by a dashed box. Try sliding the underlined variables around to see how they affect the outcome!
| Update Step (State Space) | |||||||||||||||||||||||
|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
|
|||||||||||||||||||||||
| ↓ | |||||||||||||||||||||||
| Convert to Measurement Space with \( \measurementMatrix = \begin{bmatrix} 1 & 0 \end{bmatrix} \) | |||||||||||||||||||||||
| ↓ | |||||||||||||||||||||||
|
|
||||||||||||||||||||||
| ↓ | ↓ | ||||||||||||||||||||||
| Convert to state space with \( \measurementMatrix^{-1} \) | |||||||||||||||||||||||
| ↓ | ↓ | ||||||||||||||||||||||
|
|
||||||||||||||||||||||
|
|||||||||||||||||||||||
| ↓ | |||||||||||||||||||||||
|
|||||||||||||||||||||||
In each of our derivations, we've arrived at the same definition for the Kalman Gain: $$ \text{The Kalman Gain } \kalmanGain = \stateCovariance \measurementMatrix^T \left( \measurementMatrix \stateCovariance \measurementMatrix^T + \measurementNoiseCovariance \right)^{-1} $$ However, we have also shown that there are two different ways to interpret this formula (so long as you imagine that \( \measurementMatrix^{-1} \) exists). Rearranging \( \varianceRatioState = \kalmanGain \measurementMatrix \) and \( \varianceRatio = \measurementMatrix \kalmanGain \) we have: $$ \kalmanGain = \measurementMatrix^{-1} \varianceRatio = \varianceRatioState \measurementMatrix^{-1} $$ Therefore, we can choose to interpret the Kalman Gain as a two-step process that always takes measurements as inputs, then either:
Now that we have all the mathematical pieces of the Predict and Update steps, we can write out the full algorithm! In practice, we write each step as a function that takes the previous state estimate along with new information, and returns a new state estimate.
| Predict(\( \state_{\discreteTime-1}, \space \stateCovariance_{\discreteTime-1}, \space \stateTransition, \space \controlMatrix, \space \controlInput_{\discreteTime}, \space \processNoiseCovariance \)): | Update(\( \state_{\discreteTime-1}, \space \stateCovariance_{\discreteTime-1}, \space \measurementMatrix, \space \measurement_{\discreteTime}, \space \measurementNoiseCovariance \)): |
|---|---|
| \( \begin{align} \quad \quad \state_{\discreteTime} &= \stateTransition \state_{\discreteTime-1} + \controlMatrix \controlInput_{\discreteTime} \\ \quad \quad \stateCovariance_{\discreteTime} &= \stateTransition \stateCovariance_{\discreteTime-1} \stateTransition^T + \processNoiseCovariance \\ \end{align} \) | \( \begin{align} \quad \quad \kalmanGain &= \stateCovariance_{\discreteTime-1} \measurementMatrix^T \left( \measurementMatrix \stateCovariance_{\discreteTime-1} \measurementMatrix^T + \measurementNoiseCovariance \right)^{-1} \\ \quad \quad \state_{\discreteTime} &= \state_{\discreteTime-1} + \kalmanGain \left( \measurement_{\discreteTime} - \measurementMatrix \state_{\discreteTime-1} \right) \\ \quad \quad \stateCovariance_{\discreteTime} &= \left( \identityMatrix - \kalmanGain \measurementMatrix \right) \stateCovariance_{\discreteTime-1} \left( \identityMatrix - \kalmanGain \measurementMatrix \right)^T + \kalmanGain \measurementNoiseCovariance \kalmanGain^T \end{align} \) |
| \(\quad \quad \text{return } \left( \state_{\discreteTime}, \space \stateCovariance_{\discreteTime} \right) \) | \(\quad \quad \text{return } \left( \state_{\discreteTime}, \space \stateCovariance_{\discreteTime} \right) \) |
A few notes:
| Control the robot's acceleration by dragging the number: \( \controlInput = \) |
|
|
The Kalman Filter is an algorithmic blueprint for incorporating different sources of information into a unified probabilistic state estimate. It outlines two ways to propagate the current state estimate to the next:
My aim with this article was to give you an intuitive understanding of how the Kalman Filter works, but from here there is a lot of beautiful math that proves the Filter's optimality under certain conditions, and also provides insight on how to handle different physical systems. Hopefully while exploring this article you realized that there are many obvious shortcomings. For example, What if your model isn't linear? What if you don't know the covariance matrix for your sensors? How do small changes in parameters like the timestep (or accumulation of numerical error) affect the filter's behavior?
If you would like to see how the Kalman Filter is implemented under the hood, my github repo has a javascript implementation.
As far as dealing with non-linearities, there are several different algorithms you should look into: the Particle Filter, the Unscented Kalman Filter (UKF), and the Extended Kalman Filter (EKF). All three of these are similar in that they combine probabilistic state and measurement information, but they differ in how they incorporate non-linear process models and how they represent the state estimate distribution.
In total I've thrown you 17 variables -- that can be a lot to keep track of! I've collected them all here so you have a single reference, though clicking on the variables throughout the article will give the same information. Also remember that you can choose the naming convention using the dropdown menu at the top of the article.
| Symbol | Name | Description | Dimension |
|---|
If you see mistakes or want to suggest change, please create an issue on Github. I would like my writing and visual style to be as good as it can be, but it also takes a lot of work to create and maintain this type of content, so I may or may not make the change.
If you use this article in an academic context, please cite it as:
Lovekin, A. "The Interactive Kalman Filter." 2024. https://arthurlovekin.com/interactive-kalman-filter/index.html
Bibtex Citation:
@misc{lovekin2024interactivekalman, author = {Lovekin, Arthur}, title = {The Interactive Kalman Filter}, year = {2024}, note = {https://arthurlovekin.com/interactive-kalman-filter/}, }