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 equations:
Prediction
where 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).
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:
