## 02ps-11 Programming Exercise

• 0:00 - 0:03
Let's now come to our programming exercise
• 0:03 - 0:08
I want you to program exactly what we just talked about.
• 0:08 - 0:11
We're given a two-dimensional world
• 0:11 - 0:18
where we observe in 2D measurements of a moving object
• 0:18 - 0:21
with an unknown but fixed velocity.
• 0:21 - 0:27
Using a state vector of this type, I'd like you to implement the Kalman filter.
• 0:27 - 0:30
Now, this Kalman filter now has 4 state variables
• 0:30 - 0:33
whereas the one we used before had 2 state variables.
• 0:33 - 0:36
I will give you the entire code for the Kalman filter,
• 0:36 - 0:52
but I want you to set up the state vector x, the motion u, P, F, H, R, and I,
• 0:52 - 0:57
which are all those variables that define the Kalman filter.
• 0:57 - 1:01
• 1:01 - 1:05
and make it work in 4D.
• 1:05 - 1:10
Here is exactly the same matrix class that I wrote you before.
• 1:10 - 1:14
Here is the Kalman filter procedure.
• 1:14 - 1:18
We'll go through our measurements and apply the Kalman filter equations.
• 1:18 - 1:21
I should point out there's a slight difference to the code I gave you previously
• 1:21 - 1:24
where I insert a zeta transpose.
• 1:24 - 1:29
It makes it a little bit easier to work with multidimensional measurements than how I had it before.
• 1:29 - 1:33
But you don't have to pay attention to this. It's just fixed. There was a kind of a bug before.
• 1:33 - 1:40
As I scroll down, the output of the Kalman filter routine will be an x and a P.
• 1:40 - 1:43
In our example, the measurements will be a sequence of measurements
• 1:43 - 1:46
in two-dimensional spaces now--in x and y.
• 1:46 - 1:49
Look at the x's 5, 6, 7, 8, 9, and 10.
• 1:49 - 1:53
The y's go 10, 8, 6, 4, and 2.
• 1:53 - 1:56
You can imagine what the regularity is and what the velocity is.
• 1:56 - 2:00
We assume a dt of 0.1.
• 2:00 - 2:04
That means when it goes from 5 to 6 the velocity is actually 10, not 1.
• 2:04 - 2:09
We won't tell the system, but we will tell the system our initial x,y location, which is 4.
• 2:09 - 2:16
That goes nicely into 5, 6, 7, and it's 12. That blends nicely into 10, 8, and 6.
• 2:16 - 2:20
Our initial state vector I have already given you, which is the initial x and y.
• 2:20 - 2:24
and 0, 0 for the two unknown velocities.
• 2:24 - 2:29
The motion vector, just for completeness, will just be 0, 0, 0, 0.
• 2:29 - 2:31
We have no external motion.
• 2:31 - 2:35
That's a bit confusing, because there is actually motion in the system itself,
• 2:35 - 2:38
but this will be more like an external change of the motion
• 2:38 - 2:41
as if someone hit the object with an external force.
• 2:41 - 2:43
So it's 0, 0, 0, 0--please don't change it.
• 2:43 - 2:46
P is the initial uncertainty,
• 2:46 - 2:51
and I want you to initialize it so that the uncertainty for the x,y coordinates is zero,
• 2:51 - 2:56
but the covariance term for the velocities is 1000,
• 2:56 - 2:59
indicating that we really don't know the initial velocity.
• 2:59 - 3:01
We just know the initial position.
• 3:01 - 3:04
I want you to plug in the f matrix.
• 3:04 - 3:07
I want you to design an H matrix that's a projection matrix
• 3:07 - 3:10
from 4-dimensional state space to 2 dimensions,
• 3:10 - 3:14
reflecting the fact that we can only observe the first two state variables--x and y--
• 3:14 - 3:16
but not the velocities.
• 3:16 - 3:20
I want you to define the measurement uncertainty matrix, which now is a 2 x 2
• 3:20 - 3:25
that has 0.1 as the main diagonal as measurement noise.
• 3:25 - 3:27
This is an identity matrix over here.
• 3:27 - 3:33
Once you design all those, you should get the following output.
• 3:33 - 3:36
So when I run this, I get as an output
• 3:36 - 3:42
for my 4-dimensional example the x coordinates 10 and 0.
• 3:42 - 3:46
This makes sense given that these sequences over here
• 3:46 - 3:55
has a final measurement 10 and 0--5, 6, 7, 8, 9, 10--10, 8, 6, 4, 2, 0.
• 3:55 - 4:00
The interesting thing that I want your program to produce is the velocities.
• 4:00 - 4:06
They are approximately 10, which makes sense given out delta-t of 0.1
• 4:06 - 4:12
gives us per time step a 10 divided by 10 equals 1 increment over here.
• 4:12 - 4:17
The second velocity is -20 multiplied by 0.1 gives us a -2.
• 4:17 - 4:21
You can see it over here--10, 8, 6, 4, and 2.
• 4:21 - 4:25
I also want you to output the covariance matrix,
• 4:25 - 4:29
which has certain elements that are still 0, like these guys over here.
• 4:29 - 4:34
We find that along the main diagonal our uncertainty has shrunk substantially.
• 4:34 - 4:40
It's 0.03 for the coordinate estimates and 0.1 for the velocity estimates.
• 4:40 - 4:44
Remember, this number over here was 1000 before.
• 4:44 - 4:48
Here is a second example where we have an initial coordinate of -4 and 8.
• 4:48 - 4:52
We can see the measurements 1, 6, 11, 16.
• 4:52 - 4:55
It seems the increments are in x direction 5.
• 4:55 - 5:00
In the y direction they are -4--8, 4, 0, -4, -8.
• 5:00 - 5:06
If I now run this, I get for my x vector approximately 16--the number over here,
• 5:06 - 5:08
approximately -8--the number over here.
• 5:08 - 5:13
These are velocities--50 and -40 in approximation,
• 5:13 - 5:21
which multiplied with 0.1 is our plus 5 and our -4.
• 5:21 - 5:25
Here is yet another example. Initial state 1 and 19.
• 5:25 - 5:29
You can see the first coordinate doesn't change at all. You should get a velocity of 0.
• 5:29 - 5:34
The second coordinate goes 19, 17, 15, 13, and 11.
• 5:34 - 5:39
Running it gives us 1--unchanged--and 11.
• 5:39 - 5:43
Velocities are 0 and -20 for the decrements of -2.
• 5:43 - 5:50
Going into the covariance, we see values along the main diagonals--
• 5:50 - 5:55
0.05, 0.05, 0.33, and 0.33 for the velocities.
• 5:55 - 5:58
There are certain off-diagonal elements.
• 5:58 - 6:03
Make sure those all match what your code produces.
• 6:03 - 6:08
I can now change some of these measurements to make a noisy measurement.
• 6:08 - 6:12
One way to do this is to set an oscillating measurement between 2 and 0.
• 6:12 - 6:15
Remember that this Kalman filter assumes a fixed velocity.
• 6:15 - 6:20
There is no way to explain these measurements with a fixed velocity,
• 6:20 - 6:22
so there has to be measurement noise.
• 6:22 - 6:24
We can run the Kalman filter again.
• 6:24 - 6:27
Your filter should output the following values:
• 6:27 - 6:34
0.7 for the current state, 11 as before, and here are our two velocity estimates.
• 6:34 - 6:41
It actually believes there is a slight velocity of -0.66
• 6:41 - 6:42
in the x direction where we had noisy sensor input.
• 6:42 - 6:45
The covariance matrix would look exactly as before,
• 6:45 - 6:48
because it's not affected by the measurements themselves.
• 6:48 -
Your job is to fill in these various matrices. Good luck.
Title:
02ps-11 Programming Exercise
Description:

dummy description

more » « less
Video Language:
English
Team:
Udacity
Project:
CS373 - Artificial Intelligence
Duration:
06:53

• Amara Bot