Return to Video

Kalman Matrices - Artificial Intelligence for Robotics

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

more » « less
Video Language:
English
Team:
Udacity
Project:
CS373 - Artificial Intelligence
Duration:
05:45
Udacity Robot edited English subtitles for 02-47 Kalman Matrices
Udacity Robot edited English subtitles for 02-47 Kalman Matrices
Udacity Robot edited English subtitles for 02-47 Kalman Matrices
Udacity Robot edited English subtitles for 02-47 Kalman Matrices
Gundega edited English subtitles for 02-47 Kalman Matrices
Amara Bot added a translation

English subtitles

Revisions Compare revisions