Return to Video

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
    Start with the assignment that we had in 2D
  • 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 added a translation

English subtitles

Revisions