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.
Our state transition function looks 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!
** note: add system dynamics equation for clarity