This kind of relationship is really important to keep track of, because it gives us more information: One measurement tells us something about what the others could be. And that’s the goal of the Kalman filter, we want to squeeze as much information from our uncertain measurements as we possibly can!
This correlation is captured by something called a covariance matrix. In short, each element of the matrix Σij is the degree of correlation between the ith state variable and the jth state variable. (You might be able to guess that the covariance matrix is symmetric, which means that it doesn’t matter if you swap i and j). Covariance matrices are often labelled “Σ”, so we call their elements “Σij”.
Describing the problem with matrices
We’re modeling our knowledge about the state as a Gaussian blob, so we need two pieces of information at time k: We’ll call our best estimate x^k (the mean, elsewhere named μ ), and its covariance matrix Pk.
x^kPk=[positionvelocity]=[ΣppΣvpΣpvΣvv](1)
(Of course we are using only position and velocity here, but it’s useful to remember that the state can contain any number of variables, and represent anything you want). Next, we need some way to look at the current state (at time k-1) and predict the next stateat time k. Remember, we don’t know which state is the “real” one, but our prediction function doesn’t care. It just works on all of them, and gives us a new distribution:
We can represent this prediction step with a matrix, Fk:
It takes every point in our original estimate and moves it to a new predicted location, which is where the system would move if that original estimate was the right one. Let’s apply this. How would we use a matrix to predict the position and velocity at the next moment in the future? We’ll use a really basic kinematic formula:
pkvk=pk?1+Δt=vk?1vk?1
In other words:
x^k=[10Δt1]x^k?1=Fkx^k?1(2)(3)
We now have a prediction matrix which gives us our next state, but we still don’t know how to update the covariance matrix.
This is where we need another formula. If we multiply every point in a distribution by a matrix A, then what happens to its covariance matrix Σ? Well, it’s easy. I’ll just give you the identity:
Cov(x)Cov(Ax)=Σ=AΣAT(4)
So combining (4) with equation (3):
x^kPk=Fkx^k?1=FkPk?1FTk(5)
External influence
We haven’t captured everything, though. There might be some changes that aren’t related to the state itself— the outside world could be affecting the system.
For example, if the state models the motion of a train, the train operator might push on the throttle, causing the train to accelerate. Similarly, in our robot example, the navigation software might issue a command to turn the wheels or stop. If we know this additional information about what’s going on in the world, we could stuff it into a vector called uk→, do something with it, and add it to our prediction as a correction.
Let’s say we know the expected acceleration a due to the throttle setting or control commands. From basic kinematics we get:
pkvk=pk?1+Δt=vk?1+vk?1+12aΔt2aΔt
In matrix form:
x^k=Fkx^k?1+[Δt22Δt]a=Fkx^k?1+Bkuk→(6)
Bk is called the control matrix and uk→ the control vector. (For very simple systems
with no external influence, you could omit these).
Let’s add one more detail. What happens if our prediction is not a 100% accurate model of what’s actually going on?
External uncertainty
Everything is fine if the state evolves based on its own properties. Everything is still fine if the state evolves based on external forces, so long as we know what those external forces are.
But what about forces that we don’t know about? If we’re tracking a quadcopter, for example, it could be buffeted around by wind. If we’re tracking a wheeled robot, the wheels could slip, or bumps on the ground could slow it down. We can’t keep track of these things, and if any of this happens, our prediction could be off because we didn’t account for those extra forces.
We can model the uncertainty associated with the “world” (i.e. things we aren’t keeping track of) by adding some new uncertainty after every prediction step:
Every state in our original estimate could have moved to a range of states. Because we like Gaussian blobs so much, we’ll say that each point in x^k?1 is moved to somewhere inside a Gaussian blob with covariance Qk. Another way to say this is that we are treating the untracked influences as noise with covariance Qk.
