1 00:00:03,670 --> 00:00:09,730 Welcome to the first exercise for the extent and filter in this exercise, we want to implement the 2 00:00:09,730 --> 00:00:15,010 extended common field of prediction step equations and the nonlinear 2D vehicle process model. 3 00:00:16,150 --> 00:00:21,240 So the first step in this fall is the open the C++ follow the common field step. 4 00:00:22,240 --> 00:00:26,950 We want to review the code that has already been written as part of this exercise. 5 00:00:27,440 --> 00:00:32,890 Then we want to compile and run the simulation as is, and we want to use the motion profile. 6 00:00:32,920 --> 00:00:38,620 Number three, we can see that the car starts at the origin and then the car performs a series of turns, 7 00:00:38,620 --> 00:00:41,860 all while maintaining the current speed of five meters a second. 8 00:00:43,300 --> 00:00:48,580 We can also note that the GPS measurement model and the state initialization is copied from the linear 9 00:00:48,600 --> 00:00:54,580 computer as the filter equations and logic remain the same between the linear common filter and the 10 00:00:54,580 --> 00:00:57,220 extended column for the steps. 11 00:00:58,000 --> 00:01:03,190 So the filter will start to run, but is using GPS only and it's without a process model. 12 00:01:05,700 --> 00:01:10,020 Now, at the top of the file, you should be able to see a number of these constants and these are some 13 00:01:10,020 --> 00:01:15,030 constants you can play around with and you can modify and use them in the code to help you out in your 14 00:01:15,030 --> 00:01:15,610 programming. 15 00:01:16,500 --> 00:01:22,580 We can also see that inside the handle, GPS measurement or the GPS measurement code is already done. 16 00:01:22,590 --> 00:01:26,770 And this is exactly the same as a code that's been used inside the linear coming filter. 17 00:01:27,510 --> 00:01:31,200 We can do this because the GPS model is a linear model. 18 00:01:31,420 --> 00:01:36,720 Therefore we don't need any changes to implement it inside the extended common field on. 19 00:01:38,390 --> 00:01:43,550 We can also have a look at the end of this function, and we also see that the state initialization 20 00:01:43,550 --> 00:01:49,220 is already done, so the state initialization, as happens on the first measurement. 21 00:01:49,710 --> 00:01:54,950 And if we have a look at the state and the covariance that gets initialized, we can see that the state 22 00:01:54,950 --> 00:02:00,770 vector gets initialized with the X and Y locations directly from the of measurement. 23 00:02:01,250 --> 00:02:04,650 And we initialize the heading and velocity to be zero. 24 00:02:05,360 --> 00:02:10,880 We can look at the covariance matrix and we use the covariance from the GPS measurements for the precision 25 00:02:11,270 --> 00:02:18,470 and we set an initial heading variance and initial velocity variance just to be large so that we have 26 00:02:18,470 --> 00:02:23,870 some just to be large as we have no idea about what the true state of these parameters are going to 27 00:02:23,870 --> 00:02:28,200 be directly at the initialization because we don't have this information available. 28 00:02:28,940 --> 00:02:33,080 We should also note that in this example, the code uses the state vector. 29 00:02:33,680 --> 00:02:38,540 In this filing form, it has a position in the X, in a position in the Y, then the heading, then 30 00:02:38,540 --> 00:02:39,230 the velocity. 31 00:02:39,650 --> 00:02:45,320 So is it important to note that all the code uses this vector definition and this order of the state 32 00:02:45,320 --> 00:02:45,950 parameters? 33 00:02:48,350 --> 00:02:51,200 The second step is to start to implement the prediction step. 34 00:02:51,230 --> 00:02:54,020 So this is where we put the process model into the code. 35 00:02:54,740 --> 00:02:59,750 So what we want to do is we want to modify the prediction step function inside the code so you can see 36 00:02:59,870 --> 00:03:01,000 this block of code here. 37 00:03:01,040 --> 00:03:04,850 This is where we want to implement the prediction step equations. 38 00:03:05,510 --> 00:03:08,120 So we want to implement the process model, which is shown here. 39 00:03:08,150 --> 00:03:11,450 So this is just the two vehicle process model that we have covered before. 40 00:03:12,080 --> 00:03:16,520 And we can use some of these inputs from the function so we can see that we have the gyroscope measurement 41 00:03:16,520 --> 00:03:19,580 here and the data parameters inside this function. 42 00:03:19,910 --> 00:03:23,480 So we want to put them into this motion model here. 43 00:03:23,480 --> 00:03:29,930 So our data here is this parameter here, the Delta team, and this is going to be our Jarawa. 44 00:03:29,930 --> 00:03:30,450 You're right. 45 00:03:30,470 --> 00:03:33,760 So we can reference that inside the code as Jarro cyborg. 46 00:03:34,250 --> 00:03:36,390 So this is the you're right of the vehicle. 47 00:03:37,250 --> 00:03:40,490 Now, we also want to set the initial sorry. 48 00:03:40,500 --> 00:03:43,780 We also want to set the acceleration at each time set to be zero. 49 00:03:44,210 --> 00:03:49,490 And we're just going to assume that this is a random variable from a normal distribution zero mean and 50 00:03:49,490 --> 00:03:52,970 a variance of our sigma squared acceleration parameter. 51 00:03:53,450 --> 00:03:59,510 So just like in the linear computer, we are going to use our unknown acceleration. 52 00:03:59,810 --> 00:04:01,870 So we're going to increase this inside. 53 00:04:02,120 --> 00:04:05,390 So we're going to include this term inside the process, model noise. 54 00:04:06,500 --> 00:04:10,880 Now, once we do the prediction step, we also want to make sure we normalize the hitting angles or 55 00:04:10,880 --> 00:04:13,700 any angles inside the filter to be plus or minus pi. 56 00:04:13,700 --> 00:04:15,660 So plus or minus 180 degrees. 57 00:04:16,340 --> 00:04:22,250 So if we were to just to integrate this plainly using this equation here, outside value here might 58 00:04:22,250 --> 00:04:28,280 eventually get larger than, what, 180 degrees or smaller than negative hundreds of degrees if we keep 59 00:04:28,280 --> 00:04:29,960 turning in the same direction. 60 00:04:30,560 --> 00:04:36,590 But we always want to normalize the setting output, A.J. And to do this, we can use this equation 61 00:04:36,590 --> 00:04:36,770 here. 62 00:04:36,770 --> 00:04:38,720 We can use the angle function. 63 00:04:39,110 --> 00:04:44,420 So this basically wraps an angle here to within this range and returns the result here. 64 00:04:44,690 --> 00:04:50,240 So this line of code here is pretty much what you're going to need to do for the heading angle integration. 65 00:04:52,910 --> 00:04:59,630 The next step is to implement the Keverian step for the British step, so to do this, we need to calculate 66 00:04:59,630 --> 00:05:02,660 the Jicarilla matrix, the F matrix for the process model. 67 00:05:03,060 --> 00:05:04,970 We want to define the key metrics. 68 00:05:04,970 --> 00:05:10,680 So this is the process model noise matrix for the JavaScript noise and the acceleration noise. 69 00:05:10,700 --> 00:05:15,840 So this is where we assume that we don't know the acceleration, but we assume it's a random variable. 70 00:05:15,860 --> 00:05:19,030 So this is going to be a sigma squared acceleration. 71 00:05:19,700 --> 00:05:26,500 And we also want to implement the extended field covariance prediction step so that you can your metrics 72 00:05:26,510 --> 00:05:29,590 that we want to implement is going to be this set of equations here. 73 00:05:30,350 --> 00:05:33,450 So we've covered this in the measurement model. 74 00:05:34,940 --> 00:05:38,270 We also want to implement the key metrics for the process model noise. 75 00:05:38,270 --> 00:05:41,500 And we're not going to be adding any noise for the X and Y position. 76 00:05:41,930 --> 00:05:44,660 We're going to be adding the gyroscope sensor noise. 77 00:05:44,690 --> 00:05:45,870 So this is this term here. 78 00:05:46,160 --> 00:05:52,790 So this is going to implement the the noise from the uncertainty inside our gyroscope measurements. 79 00:05:53,240 --> 00:05:56,910 And then we want to implement our acceleration, uncertainty. 80 00:05:57,080 --> 00:06:02,270 This allows our process, model, acceleration and velocity to change as we run. 81 00:06:03,170 --> 00:06:07,540 And lastly, we want to implement the common fieldworker variance prediction set. 82 00:06:07,550 --> 00:06:08,990 So this set of equations here. 83 00:06:10,610 --> 00:06:15,740 Once we've done that, then we can run the simulation and we can run it with the profiles one to four, 84 00:06:16,400 --> 00:06:21,380 so we can run the simulation and check that everything's working and we can check out how accurate our 85 00:06:21,380 --> 00:06:27,080 simulation is with profiles one before so we can check out the root mean squared error that we get for 86 00:06:27,080 --> 00:06:28,130 the whole simulations. 87 00:06:32,110 --> 00:06:37,600 So we can can quickly compare the linnear Cominco, the solution and the extended calm and feel the 88 00:06:37,600 --> 00:06:43,150 solution that we've just done, so now we basically have an extended common and a version of the linnear 89 00:06:43,150 --> 00:06:43,780 common filter. 90 00:06:43,930 --> 00:06:47,730 All we have done is just changed the process model to be a non-linear model. 91 00:06:48,070 --> 00:06:52,750 So we should be able to compare the results with the with compared to the extent Mattilda, they're 92 00:06:52,750 --> 00:06:58,090 using the same inputs except for the extended coming up, which is now including the nonlinear model 93 00:06:58,090 --> 00:06:59,440 and gyroscope measurements. 94 00:06:59,770 --> 00:07:02,800 So we can see if this improves the situation at all. 95 00:07:03,520 --> 00:07:08,770 So we should end up with a few numbers that look fairly close to this and we should be able to see that 96 00:07:08,770 --> 00:07:11,880 the hopefully the extended common midfielder is performing very well. 97 00:07:14,190 --> 00:07:18,300 So now that you've implemented the predictions that play around with the code and make sure everything's 98 00:07:18,300 --> 00:07:24,270 working as expected, is this filter performing better than the Lynnae Common Filter is are expected 99 00:07:24,270 --> 00:07:24,630 to be? 100 00:07:24,660 --> 00:07:26,250 So is it more robust? 101 00:07:26,670 --> 00:07:32,220 Can you think of a better way to nationalize the state, i.e. how to initialize velocity and the heading 102 00:07:32,460 --> 00:07:36,900 without assuming them to be zero or unknown or known constant? 103 00:07:37,830 --> 00:07:41,690 So this is going to be one problem with initializing it from a single GPS measurement. 104 00:07:41,700 --> 00:07:47,670 All we can do is just measure the position, but we can't infer any information about the velocity of 105 00:07:47,670 --> 00:07:48,120 the heading. 106 00:07:48,720 --> 00:07:53,700 Can you think about any ways or techniques that you can overcome this problem or you could improve on 107 00:07:53,700 --> 00:07:54,540 the situation?