I have a new, challenging programming assignment for you that will take you a while,
but I would like you to implement a multidimensional Kalman filter for the example
I've just given you.
The matrix class is a class for manipulating matrices that should make it really easy.
It has a function that initializes matrices--I'll show you an example in a second.
It can set them down to 0.
It can compute an identity matrix.
It can print out a matrix with show.
It overloads operators like addition, subtraction,
multiplication, and even computes the transpose of a matrix,
and in some more extended code, it can actually invert a matrix
using Cholesky factorization.
There's a function here called inverse.
This matrix class is available.
It's a small version of what is found in typical libraries.
I want to demonstrate it for you just for a second.
You can make a matrix with a command like this with the argument in the parenthesis.
It's a 2-dimensional matrix.
In this case it's a vertical vector.
With the show command, you can print out the result of the vertical vector.
You can put the transpose as follows.
If you run this, you'll find the horizontal vector.
Say you wish to multiply a matrix by a vector,
then we can make a 2 x 2 matrix with this initialization over here,
a matrix of [12., 8.] and [6., 2.].
We can print this matrix.
Here it is: 12, 8, 6, 2.
These are these values over here.
And it can multiply F and a.
So here b = F * a.
And if we show the result, we get the vector 200 and 80.
That's obtained by 10 x 12 + 10 x 8 is 200.
10 x 6 + 10 x 2 is 80.
So using my matrix libraries, I set an initial state.
This is a tracking in 1D where the state is the position
and the velocity.
I initialized both with 0 because I don't know the actual location and the velocity.
I give an uncertainty matrix
where I have a really high uncertainty in the position
and a really high uncertainty in the velocity,
and they're both uncorrelated.
That's the matrix of 1000, 0, 0, 1000.
I specify an external motion, but it's 0, 0, so it has no effect,
so just ignore this.
I build the next state function, which is the one we just discussed,
[1., 1] [0, 1.].
That assumes that the velocity is just being added to the position,
and the velocity and expectation stays the same.
I build a measurement function that extracts just the first
of the 2 values, 1 and 0,
so I can observe the location but not the velocity.
I have a measurement uncertainty.
It happens to be 1 in this example.
And I have an identity matrix, [1., 0.] [0., 1.].
And then I run a filter with these 3 measurements over here,
and what should come out is that by running the filter,
I can estimate the velocity
and therefore make better predictions.
In the filter that I want you to program,
I want the measurement update first and then the motion update.
Every time we run the filter,
I want you to update the measurement first, then the motion.
Here is my empty procedure filter that we have to fill in
where I go through our measurements,
and it builds the measurement update and then the motion update,
the prediction, and then I just print out the resulting estimates.
I do this a number of times, 3 times in this case.
Once we fill this in and I hit the Run button,
I get the following output.
After my first measurement update,
I observed the location 1, which gets copied over
essentially to .99 over here.
I learn nothing about the velocity, so it's still 0, just the way I initialized it.
And then there's an updated uncertainty matrix
which now shows what happens to be a strong correlation,
1000, 1000, 1000, 1000.
That differs from the initial one only that we filled in
the off-diagonal elements.
It happens to be exactly what the Kalman filter does.
And then I'll observe again the 2.
I want the output to now tell me that my next prediction is 3.
It's the observation plus the prediction.
But now I have a really good estimate
on what the velocity is.
It's essentially 1, and the reason is
my Kalman filter was able to use
the Kalman filter equations to find this value.
There's a new covariance matrix,
and for the third observation followed by the prediction,
the prediction is correctly effectively 4, 3.999.
The velocity estimate is correctly 1, which is .99999,
and I have yet another uncertainty matrix which illustrates
that I have a high certainty in the velocity estimate.
Notice a relatively high certainty in the position estimate
compared to my initial uncertainties.
So can you write the the algorithm: 'filter()'
that outputs those exact values over here?
This is an involved programming assignment.
What you have to do is you have to essentially
implement the equations I gave you.
You have to familiarize yourself with the matrix class
and then go and fill in the filter() code
in accordance to the things that I showed you
for the multivariate Kalman filter.