1 00:00:03,870 --> 00:00:08,580 We are now going to cover it, the first exercise you have to do for the unscented, come and fill it 2 00:00:08,580 --> 00:00:11,240 up, and that is to implement the prediction step. 3 00:00:11,760 --> 00:00:16,890 So as part of this exercise, we want to implement the unscented in common field of prediction and the 4 00:00:16,890 --> 00:00:19,470 2D non-linear vehicle process model. 5 00:00:20,580 --> 00:00:27,270 So the first step is to open up the C++ for Common Filter Seipp, and we want to review the code that 6 00:00:27,410 --> 00:00:31,500 already been written as part of the student Ukhov exercise. 7 00:00:33,100 --> 00:00:38,980 We want to compile and run the simulation as is, and we can test it with profile three, we should 8 00:00:38,980 --> 00:00:41,410 be able to see that the cost starts at the Argin. 9 00:00:41,410 --> 00:00:46,930 And then the car performs a series of turns while maintaining the current speed of five meters per second. 10 00:00:49,150 --> 00:00:54,340 We should also note that the GPS measurement model and the state initialization is already copied from 11 00:00:54,340 --> 00:00:58,360 the linear common filtering exercises as the filter equation. 12 00:00:58,390 --> 00:01:01,780 So these steps and the logic is all exactly the same. 13 00:01:02,440 --> 00:01:08,260 So the filter in this example will start to run, but it will only be using GPS and it will be running 14 00:01:08,260 --> 00:01:13,720 without a process model until we've implemented it inside the step throughout the exercise. 15 00:01:13,750 --> 00:01:19,210 There's a few helper functions that you can use inside your code, and this is to normalize the state 16 00:01:19,210 --> 00:01:21,550 and normalize the line of measurements. 17 00:01:21,820 --> 00:01:28,600 So any angular values inside the state or the measurement, we can put them in and wrap them to you 18 00:01:28,600 --> 00:01:29,890 plus or minus pi. 19 00:01:30,160 --> 00:01:35,980 So this is going to be handy when they want to normalize the state or normalize the light relative bearing 20 00:01:35,980 --> 00:01:36,540 measurements. 21 00:01:37,990 --> 00:01:42,040 We can also have a look at the top of the file and you'll find that there's a number of constants just 22 00:01:42,040 --> 00:01:43,490 as before that you can use. 23 00:01:43,540 --> 00:01:49,540 So I would encourage you to use these constants as they give you a single point of access to changes 24 00:01:49,540 --> 00:01:52,000 constants rather than having to change your code later on. 25 00:01:54,470 --> 00:01:59,780 And as we said before, the GPS management model, the linear model is already being done because it's 26 00:01:59,780 --> 00:02:01,500 the same as linear algebra. 27 00:02:01,850 --> 00:02:06,010 So if you look at the code, this has already been filled in, so you don't have to do it for you. 28 00:02:06,470 --> 00:02:09,710 And then a very similar thing for the state initialization. 29 00:02:09,980 --> 00:02:11,990 This part of the code is already being done. 30 00:02:12,000 --> 00:02:16,970 So on the first GPS measurement, you'll take the X and Y position forward into the state. 31 00:02:17,360 --> 00:02:23,180 It will assume that the other velocity components are all going to be zero, is going to set the velocity 32 00:02:23,180 --> 00:02:26,980 uncertainty to be this initial constant defined in the type of file. 33 00:02:27,320 --> 00:02:33,630 And it's going to be setting the position uncertainty just be the uncertainty of the GPS measurements. 34 00:02:33,980 --> 00:02:39,890 So basically, just as we've done before for the extended common filter, it's just going to set up 35 00:02:39,890 --> 00:02:44,010 the initial state and initial covariance as shown here. 36 00:02:44,780 --> 00:02:50,540 We also just want to remind you that inside this example, the whole code assumes that the state vector 37 00:02:50,540 --> 00:02:51,840 has a following form. 38 00:02:51,950 --> 00:02:58,040 So it has a positions and then the heading and then the velocity is the position in the X Y and then 39 00:02:58,040 --> 00:03:00,480 the heading of the vehicle and then the velocity of the vehicle. 40 00:03:00,830 --> 00:03:04,520 So all the code here assumes that we have the state vector inside this form. 41 00:03:05,540 --> 00:03:11,300 So the second step in the exercise is to implement the generate signal points and generate sigma weighting 42 00:03:11,300 --> 00:03:11,960 functions. 43 00:03:12,350 --> 00:03:17,600 And to do this we can use the Koskie decomposition inside the Reagan Library. 44 00:03:17,600 --> 00:03:24,020 And to do that, all we have to do is just take the covariance matrix and use the dot lety function 45 00:03:24,020 --> 00:03:28,520 on it to actually return the lower triangle decomposition. 46 00:03:29,900 --> 00:03:35,630 So inside the function that we have generated signal points, we just want to implement these series 47 00:03:35,630 --> 00:03:36,680 of equations here. 48 00:03:36,980 --> 00:03:43,400 So we want to generate a vector of the sigma points where each of the segments follows these equations 49 00:03:43,400 --> 00:03:43,640 here. 50 00:03:44,270 --> 00:03:50,190 Now, since we're using a common filter, we're going to assume that the noise is relatively Gaussian. 51 00:03:50,810 --> 00:03:56,350 So for our scaling time, kapa, we're just going to assume it's going to be equal to three minus NP, 52 00:03:56,390 --> 00:03:59,260 where N is the size of the state vector. 53 00:03:59,810 --> 00:04:01,850 So I just like in the unscented transform. 54 00:04:01,850 --> 00:04:08,260 We're just going to be using these equations here to generate sigma weights inside this function. 55 00:04:08,270 --> 00:04:12,220 We just want to generate these weights here based on these equations. 56 00:04:12,230 --> 00:04:17,090 And again, we're going to assume that this cappa scaling function is just going to be this function 57 00:04:17,090 --> 00:04:17,420 up here. 58 00:04:17,490 --> 00:04:22,190 So it's going to be a function of the number of states inside the state vector. 59 00:04:23,540 --> 00:04:28,730 The third step in the exercise is to implement the 2D vehicle process, motor function, and this is 60 00:04:28,730 --> 00:04:31,990 going to be slightly different than the extended computer. 61 00:04:33,270 --> 00:04:35,540 We're going to be using the same mathematical model. 62 00:04:35,630 --> 00:04:40,070 However, we want to take in the augmented state vector and return the new state. 63 00:04:40,580 --> 00:04:47,120 And we also do not want to normalize heading inside this vehicle process model because the discontinuities 64 00:04:47,120 --> 00:04:49,280 when we do this will interfere with the results. 65 00:04:49,890 --> 00:04:53,990 So we have this vehicle process model function that takes in an augmented state. 66 00:04:53,990 --> 00:05:00,380 It takes in the current rotation rate and timestep so we can assume that this augmented state vector 67 00:05:00,380 --> 00:05:02,880 is going to be looking like this. 68 00:05:03,260 --> 00:05:07,040 So it's going to be the initial vector that we have for the system. 69 00:05:07,340 --> 00:05:12,050 And then the last two terms in this vector are going to be the noise components or the random variable 70 00:05:12,050 --> 00:05:12,610 components. 71 00:05:12,920 --> 00:05:15,920 So it's going to be the noise for the rotation. 72 00:05:15,920 --> 00:05:16,160 Right. 73 00:05:16,190 --> 00:05:22,070 So this is going to be so basically the noise and the gyroscope and this is going to be the acceleration 74 00:05:22,070 --> 00:05:22,670 noise. 75 00:05:23,450 --> 00:05:26,870 So the actual state model that we want to implement is shown here. 76 00:05:26,900 --> 00:05:34,220 So this is the process model for the system and is all exactly the same as Akef, except now we're going 77 00:05:34,220 --> 00:05:37,910 to add on our noise component for the gyro onto the rotation. 78 00:05:37,910 --> 00:05:38,150 Right. 79 00:05:38,450 --> 00:05:41,930 And we're going to set the acceleration noise to be the noise turn here. 80 00:05:42,440 --> 00:05:44,470 So we're going to take in the augmented state. 81 00:05:45,120 --> 00:05:48,850 However, we're just going to return the new state, which is just the normal state vector. 82 00:05:49,880 --> 00:05:54,890 So the main step then for the unscented coming for the prediction step is to implement the unsent and 83 00:05:54,890 --> 00:05:56,170 transform prediction step. 84 00:05:56,780 --> 00:06:00,470 So we're going to implement this inside the car method of prediction step equation. 85 00:06:00,480 --> 00:06:03,180 So you're going to decode between these two lines here. 86 00:06:03,950 --> 00:06:09,320 The first step in the process is to augment the state in the covariance matrix with the process model 87 00:06:09,320 --> 00:06:09,830 noise. 88 00:06:10,430 --> 00:06:16,280 So we're going to have we're going to generate to augment the state is going to consist of the current 89 00:06:16,280 --> 00:06:18,530 state of the vehicle and then zero zero. 90 00:06:18,560 --> 00:06:24,590 So this is going to be zero for the gyroscope noise and zero for the process model acceleration noise. 91 00:06:25,010 --> 00:06:28,880 And we're going to do this because we're going to assume that the main value of the noise is going to 92 00:06:28,880 --> 00:06:29,390 be zero. 93 00:06:30,170 --> 00:06:34,600 We're going to augment the covariance matrix to find this augmented variance matrix. 94 00:06:34,970 --> 00:06:37,450 And again, it's going to be the original covariance matrix. 95 00:06:37,460 --> 00:06:42,390 And then on the diagonal times are going to put the jarra noise and then the acceleration noise. 96 00:06:43,370 --> 00:06:49,310 So once we have the augmented vector and the augmented covariance vector, we're going to put these 97 00:06:49,310 --> 00:06:52,020 into the generate sigma points functions. 98 00:06:52,040 --> 00:06:55,130 So this is the function that we've just written on the previous slide. 99 00:06:55,460 --> 00:06:58,640 And we're going to calculate the sigma points and the weights. 100 00:07:00,170 --> 00:07:05,070 Once we've done that, then we want to transform all the signal points through the vehicle process model. 101 00:07:05,090 --> 00:07:10,700 So this is why we needed the presence model to take in our augmented step vector so that we can take 102 00:07:10,820 --> 00:07:14,660 we can directly put in the augmented single points in this vector. 103 00:07:15,410 --> 00:07:21,240 Once we have the transformed signal points, we then calculate the main of the transform points. 104 00:07:21,290 --> 00:07:23,000 So this is basically the step here. 105 00:07:23,360 --> 00:07:29,180 We can use a weighting vector that we've calculated and I'll transform Sigma points and calculate the 106 00:07:29,180 --> 00:07:29,500 main. 107 00:07:29,780 --> 00:07:33,440 And this will give us the new main for the updated prediction step. 108 00:07:34,100 --> 00:07:36,380 We can do the same thing for the covariance. 109 00:07:36,650 --> 00:07:42,200 So we evaluate this component down here and this will give us the covariance for the predicted state. 110 00:07:42,830 --> 00:07:48,830 And as we've come before with the unscented transform prediction step, since we've augmented the state 111 00:07:48,830 --> 00:07:54,440 vector with the process model noise, we don't have to add the process model noise matrix onto this 112 00:07:54,440 --> 00:07:55,560 covariance matrix here. 113 00:07:56,690 --> 00:08:01,020 So by the time you've done that, you should have a working version that we can run the simulation with. 114 00:08:01,040 --> 00:08:06,620 So if you compare it and run the simulation, you can check out how the simulation runs with profiles 115 00:08:06,620 --> 00:08:07,370 one to four. 116 00:08:07,730 --> 00:08:11,780 So that's all the profiles that don't include the line measurements. 117 00:08:13,070 --> 00:08:19,520 We can then quickly compare the X solutions to the UK solutions for the error statistics. 118 00:08:19,880 --> 00:08:24,220 Now that we some slight differences maybe between the results that you get and what I get. 119 00:08:24,620 --> 00:08:31,370 However, you can see that results from the akef compared to the UK are going to be very similar for 120 00:08:31,370 --> 00:08:34,340 most of profiles so artfully done. 121 00:08:34,340 --> 00:08:38,300 This is a good idea to play around with code and make sure it's working as expected. 122 00:08:38,600 --> 00:08:41,010 And you can sort of think about a few of these answers. 123 00:08:41,030 --> 00:08:46,070 So is the field performing better than the AKF and is that to be expected? 124 00:08:46,610 --> 00:08:52,010 You can also think about is the field a method easier or harder to implement compared to implementing 125 00:08:52,010 --> 00:08:54,020 the EAF as what you've done before? 126 00:08:54,650 --> 00:08:59,740 And then do you think the filter is computationally more or less expensive than the IQF?