Multivariate Kalman Filter
On a high level, multivariate Kalman filters work similarly to one dimensional Kalman filters. Both require an established system model, and then run through these steps:
However, this is not as simple as it sounds. Now that we're working with multiple variables, we're straying away from the more direct "multiplying Gaussians" approach to math and discretizing the system using linear algebra. Everything is matrices, many of which we have to design ourselves according to what we want to put in and get out of the system. We'll go through the steps we took to set up our specific multivariate Kalman filter, but for more general information you should, as always, read this book.
The entire system is governed by these four equations (Note: these are split into the predict and update steps, with colors representing changing variables and black letters representing constants within each step):
In the first equation, x bar (orange) and x (red) are vectors of the state variables of the system, F is the state transition function contained in a matrix, and B and u are the control functions. Together, this means that you update your previous state (x) with a model of your movement (F) since the last time step, plus whatever velocity you have added through a command (Bu). This serves as the model for our system.
In the second, P bar (blue) and P (green) are covariance matrices of the system, F is the same matrix as before, and Q is the process covariance matrix. P contains the expected variance of the state variables and their covariances (how the variances depend on each other), which the model alone doesn't account for. This equation updates it over the past time step using F, and uses Q to account for uncertainty between the model and the world.
The new variables here are K, y, I, and H. In short, K is the Kalman gain, y is the residual (or difference) between the prediction and measurement, H is the measurement function (which defines the conversion from state variables to measurement variables), and I is an identity matrix. Overall, the first equation represents taking a measurement and weighing the new position guess between it and the prediction obtained in the predict step. The second equation represents an updated covariance matrix with the new measurement information. Both the new x vector and P matrix will be fed back into the predict step for the next moment in time.
Here's where we unpack what the heck these equations mean:
As a reminder, the results of these equations we want are a new x position from the last movement and command velocity carried in a vector (x bar), and a new covariance matrix that represents how much we trust our model compared to the real world with new information.
We decided to expand on our previous Kalman filter by adding velocity as a state variable, while still working with one dimensional movement. This means we now have two state variables, position and velocity, contained in the colorful x vector below:
This is the same form x bar will have after transformation.
The equation of motion for our one-dimensional system looks like this:
We want to discretize this equation using vectors in order to create our state transition function. This ends up looking like this:
Because we have a control function (Bu) that includes the command velocity in x bar, we designed F so that the velocity is is removed from x in the matrix multiplication so it isn't counted twice. Bu also adds the change in x over the last time step to the previous x, so x bar ultimately carries the updated position and new command velocity!
The equation governing covariance for our system looks like:
Let's break this down:
P is our covariance matrix, which contains the variances of x position and velocity. Initially, this is all it contains. Over time, it will include the covariance between these two variances in the diagonals. ???
F is the same as before, and updates the covariance matrix (P) according to our model of the system. In this case, it reduces the covariance matrix to only account for the variance in x.
The process noise matrix, Q, then adds back in the variance in velocity... ??
Overall, Charlie isn't sure why this is.
As a reminder, here are the equations that determine the update step:
We'll begin with the first equation:
In plain words, this is adding the residual (the difference between the prior and measurement) scaled by the Kalman gain (how much we trust each of those) to the prior (x bar) to get our new estimate of our location.
The first step to design this equation is to figure out how to get our residual, the difference between our measurement and our prior from the predict step. We need both the measurement and the prior to have the same form in order to do this. However, the prior, x bar, is a vector that contains both position and velocity-- so we need to extract only the position from it. We do this with a measurement function, H, making the residual calculation looks like this:
where y is the residual, z is the measurement, x bar is our prior, and H is the measurement function. So if H converts x bar into a 1x1 matrix, what should it look like?
As you can see, it is a very simple matrix, but it does its job well! We get a residual between the measurement and prior position at the end.
- K
- I
- H