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:
WHAT IF I HAD A DIAGRAM
- prior -- predict (u)
- prediction -- update (z)
- posterior -- back to the beginning!
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 (split into the predict and update steps):
###Prediction
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.
###Update
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:
