1 00:00:03,700 --> 00:00:09,280 Now, let's have a look at our first exercise for the 2D tracking filter that we're developing, and 2 00:00:09,280 --> 00:00:11,110 this is going to be the prediction step. 3 00:00:12,460 --> 00:00:17,050 So we have already derived in past the process model, which is shown in this equation. 4 00:00:17,050 --> 00:00:18,790 Here we have the state transmission matrix. 5 00:00:18,790 --> 00:00:24,460 If we have the noise since the Matrix L and we have the state and noise vectors here. 6 00:00:24,470 --> 00:00:27,040 So we end up with this equation change here written out. 7 00:00:27,040 --> 00:00:30,970 And for now we know the noise properties of the system. 8 00:00:30,970 --> 00:00:35,890 We know that this random vector here is going to be a normal distribution with covariance. 9 00:00:35,890 --> 00:00:37,450 Q Where covariance. 10 00:00:37,450 --> 00:00:40,280 Q is just this Gaussian distribution here. 11 00:00:40,300 --> 00:00:46,120 So basically we just have a diagonal matrix saying that the X and Y components are independent and they're 12 00:00:46,120 --> 00:00:51,520 going to have these noise variance values, one for the X acceleration, one for Y acceleration. 13 00:00:52,720 --> 00:00:56,800 So the process of noise prediction set for the common filter is shown here. 14 00:00:57,430 --> 00:01:02,440 So since we have non additive noise up here, so since we're multiplying our noise backed up by this 15 00:01:02,440 --> 00:01:09,390 el matrix, we have to transform our variance from the process model units into our state units. 16 00:01:09,790 --> 00:01:12,200 So we do this using the EL transformation here. 17 00:01:12,520 --> 00:01:14,770 So basically for our covariance matrix. 18 00:01:14,770 --> 00:01:19,420 Q We have to multiply it by this transform that we've covered it in the past. 19 00:01:20,800 --> 00:01:24,310 So looking at this equation is l l transpose. 20 00:01:24,310 --> 00:01:28,090 If we expand it out, we'll get a matrix that looks like this. 21 00:01:29,510 --> 00:01:35,990 So this would be the amount of noise we have to inflate the system by Jews to the noise of the acceleration. 22 00:01:37,280 --> 00:01:39,040 Now we actually have a closer look at this. 23 00:01:39,050 --> 00:01:44,330 We can actually find that this is not a valid Gaussian distribution, is not about probability distribution 24 00:01:45,080 --> 00:01:49,370 and is not a valid probability density function because it is ranked deficient. 25 00:01:49,590 --> 00:01:53,930 If you look at the determinant of this matrix is going to be determinant of zero. 26 00:01:54,560 --> 00:01:56,960 So this matrix is not a valid distribution. 27 00:01:56,970 --> 00:02:02,960 And if we use this distribution inside the common field of process model noise prediction up here, 28 00:02:03,530 --> 00:02:07,430 eventually the field is going to become ill conditioned, is going to stop working. 29 00:02:08,090 --> 00:02:10,100 This is actually feeding it bad information. 30 00:02:10,730 --> 00:02:13,310 So unfortunately, we're going to have to do something about that. 31 00:02:15,080 --> 00:02:19,730 So what we're going to do, we're going to slightly modify the problem, so instead of assuming that 32 00:02:19,730 --> 00:02:24,950 we have an acceleration value for the X and Y separately, we're just going to first assume that it's 33 00:02:24,950 --> 00:02:26,150 just a constant variance. 34 00:02:26,150 --> 00:02:31,880 So the noise is going to change from a two dimensional Gaussian distribution just down to a one dimensional 35 00:02:31,880 --> 00:02:32,930 Gaussian distribution. 36 00:02:34,460 --> 00:02:40,010 And secondly, now, instead of having a modification of the noise, so having cell times all random 37 00:02:40,010 --> 00:02:42,880 vector w, we're now going to modify this. 38 00:02:42,890 --> 00:02:49,850 So we only have additive noise and we're going to use this el matrix as a scaling to the normal distribution. 39 00:02:50,180 --> 00:02:56,570 So now we're going to have for the noise vector W is going to be a normal distribution of this standard 40 00:02:56,570 --> 00:03:00,920 deviation, but it's going to be multiplied by or scaled by this el matrix. 41 00:03:01,880 --> 00:03:04,480 So this basically gives us a new covariance. 42 00:03:04,670 --> 00:03:07,250 This is now the covariance that we're going to get. 43 00:03:07,730 --> 00:03:13,140 So now we're breaking up a single random variable into four different components using this matrix here. 44 00:03:13,610 --> 00:03:19,210 So this key metrics now becomes the new covariance for our random vector W. 45 00:03:20,000 --> 00:03:22,770 So these equations here are now going to be modified. 46 00:03:23,330 --> 00:03:24,340 We're going to modify them. 47 00:03:24,350 --> 00:03:26,240 So now we only have additive noise. 48 00:03:26,600 --> 00:03:33,680 Therefore, we only have to add onto the covariance using this matrix and the matrix is shown here. 49 00:03:34,610 --> 00:03:39,990 So this is a valid probability distribution because we know it is just by looking at it. 50 00:03:40,080 --> 00:03:42,790 So we have a simple diagonal matrix. 51 00:03:43,520 --> 00:03:47,720 So this is now going to be a better distribution for the noise into the system. 52 00:03:48,290 --> 00:03:52,400 So we're going to use this version rather than the original version, because this one will cause the 53 00:03:52,400 --> 00:03:56,940 system to work better and it won't be your condition the longer we run it. 54 00:03:58,130 --> 00:04:04,070 So now for your first exercise, your first exercise is to implement the common for the prediction equations, 55 00:04:04,070 --> 00:04:09,950 which we've covered in the previous videos and the 2D tracking filter process model, which also covered 56 00:04:09,950 --> 00:04:11,150 in the previous videos. 57 00:04:12,170 --> 00:04:17,960 So to do this, the first step is to open the Python file called Assignment One underscore prediction. 58 00:04:18,740 --> 00:04:22,970 You if you run this example as is, you'll see that the object starts at the origin. 59 00:04:22,970 --> 00:04:31,520 So P and P Y equals zero zero and it moves at a 45 degree angle at 10 meters a second or such that the 60 00:04:31,520 --> 00:04:35,470 V, X and V Y a seven point seven seven point ninety seven. 61 00:04:37,550 --> 00:04:43,550 The next step in the assignment is to set up the initial state and covariance to honor this initialize 62 00:04:43,550 --> 00:04:43,890 function. 63 00:04:43,910 --> 00:04:48,460 Here you have these placeholder places to set up the variables. 64 00:04:48,460 --> 00:04:53,060 So you're going to have to go through and write the code to initialize the state and covariance. 65 00:04:53,600 --> 00:04:58,970 And when we do this, you can assume that the initial position is zero zero and you can assume that 66 00:04:58,970 --> 00:05:05,090 initial velocity is this seven point zero seven seven point zero seven, just like we've saved from 67 00:05:05,090 --> 00:05:08,200 running example without making any code modifications. 68 00:05:08,870 --> 00:05:12,530 We can also assume the initial uncertainty is just going to be zero matrix. 69 00:05:13,130 --> 00:05:19,520 So basically for the state and covariance, we just want to set up a matrix X for the vector looking 70 00:05:19,520 --> 00:05:23,330 at this and a matrix P for the covariance that looks like this. 71 00:05:26,090 --> 00:05:32,690 Once we've done that, then we want to set up the model F and key matrixes, so the Matrix F we've seen 72 00:05:32,750 --> 00:05:37,400 that looks like this, we've derived that just before and our new key matrix, what looks like this. 73 00:05:38,850 --> 00:05:43,380 So we're going to use the time set that is given in the function and define the F process model. 74 00:05:44,000 --> 00:05:49,190 We are also going to define the key metrics as a function of the variable acceleration, standard deviation. 75 00:05:50,120 --> 00:05:53,450 So inside the code we say these placeholder variables. 76 00:05:53,450 --> 00:05:56,440 We want to fill in the equations for these two equations. 77 00:05:57,260 --> 00:06:01,460 We also are going to assume that the process model noise acceleration, this acceleration, standard 78 00:06:01,460 --> 00:06:05,630 deviation or this one here squared is going to be zero initially. 79 00:06:08,040 --> 00:06:12,570 Now that we've done the previous step, then four, step four, we want to implement the common field 80 00:06:12,570 --> 00:06:14,490 of prediction step equations. 81 00:06:14,500 --> 00:06:18,300 This includes the prediction and the covariance propagation. 82 00:06:19,220 --> 00:06:25,160 So in the code on the prediction step function, we want to fill in this X predict and predict. 83 00:06:25,410 --> 00:06:30,780 And basically we want to use these equations showing here, which are these equations shine on the slide 84 00:06:30,780 --> 00:06:31,110 here. 85 00:06:32,940 --> 00:06:37,500 Now, once we've done all that, we can actually run your updated simulation and we actually want to 86 00:06:37,500 --> 00:06:40,590 check that the prediction now follows a true fairly closely. 87 00:06:40,950 --> 00:06:47,910 So we should end up with a very small, mean error and a main force, the area main velocity error at 88 00:06:47,910 --> 00:06:49,100 the end of the simulation. 89 00:06:49,890 --> 00:06:54,270 This is just because we actually know the real starting position and real velocity. 90 00:06:54,270 --> 00:06:56,760 So the prediction should follow it fairly closely. 91 00:06:57,030 --> 00:06:59,760 If it doesn't, it means you've implemented something incorrectly. 92 00:07:00,970 --> 00:07:03,510 So we've checked the state prediction is working. 93 00:07:03,660 --> 00:07:07,560 So now we want to check that the precision covariance prediction is also working. 94 00:07:08,340 --> 00:07:13,450 So now let's set the initial position, X and Y covariance to be five squared. 95 00:07:14,520 --> 00:07:18,510 So now when we run the simulation again, we see a three sigma position. 96 00:07:18,510 --> 00:07:22,740 Uncertainty stays at approximately plus or minus 15 metres as expected. 97 00:07:24,180 --> 00:07:28,410 So from this, we can see that the position, covariance prediction is working correctly. 98 00:07:29,990 --> 00:07:32,940 The next step is to check the velocity covariance prediction. 99 00:07:33,470 --> 00:07:37,480 So what we can do is we can set the initial states ought to be zero. 100 00:07:37,790 --> 00:07:39,800 So we have zero prediction. 101 00:07:40,430 --> 00:07:45,830 We can set the initial position covariance to be zero, but we're going to set the initial velocity 102 00:07:45,830 --> 00:07:49,900 X and Y covariance to be seven, divided by three squared. 103 00:07:50,990 --> 00:07:56,930 So if we do this and we run the simulation, we see that the three error position sigma on the graph 104 00:07:57,260 --> 00:08:00,230 grows at the same rate as the position changes of the truth. 105 00:08:00,230 --> 00:08:06,440 So you can say here under this green line, here is the red line, and that is the rate at which the 106 00:08:06,440 --> 00:08:07,310 object is moving. 107 00:08:07,860 --> 00:08:13,310 So we can see that the velocity covariance prediction is consistent with the actual velocity or the 108 00:08:13,310 --> 00:08:16,010 rate of change of the error inside the common field on. 109 00:08:18,490 --> 00:08:22,680 The last step is going to be the check that the acceleration covariance prediction is working. 110 00:08:23,200 --> 00:08:29,410 So to do this, we want to set the initial state back to the original value of the zero zero seven point 111 00:08:29,410 --> 00:08:31,070 zero seven seven point zero seven. 112 00:08:31,570 --> 00:08:36,400 We want to set the initial covariance all to be zero, but now we want to set the process. 113 00:08:36,400 --> 00:08:40,660 What annoys noise, the acceleration, standard deviation to be point one. 114 00:08:41,740 --> 00:08:47,530 Now, when we run the simulation, we should see that the three sigma velocity and grows quite radically 115 00:08:47,530 --> 00:08:48,120 with time. 116 00:08:48,130 --> 00:08:50,200 So you should see that sort of looks like this. 117 00:08:50,950 --> 00:08:57,280 You should see that our velocity uncertainty starts at zero and it has a quarter and it has a quadratic 118 00:08:57,280 --> 00:09:01,720 shape that Croz of time and ends up a value almost at three. 119 00:09:03,440 --> 00:09:08,330 So now it's your turn is up to you to go through these steps that we've just explained in this video 120 00:09:08,600 --> 00:09:15,380 inside the Python file for yourself and to get the same results in the next video after you complete 121 00:09:15,380 --> 00:09:20,450 the assignment, one prediction step, we're going to go through the results and explain what I mean.