1 00:00:04,110 --> 00:00:09,550 In this video, we're going to cover the prediction step for the unscented, come and fill it up now. 2 00:00:09,570 --> 00:00:14,930 We will break down the prediction, step into the two cases based on the presence model noise. 3 00:00:15,180 --> 00:00:18,540 So the first case is going to be for additive noise. 4 00:00:18,920 --> 00:00:22,370 And in this case, the process model will have a form that looks like this. 5 00:00:22,380 --> 00:00:28,550 So you can see that we have added noise at the end of the term here is not included inside the precise 6 00:00:28,560 --> 00:00:29,310 model function. 7 00:00:30,000 --> 00:00:34,860 We will also have a look at the second case, which is the general or non additive process of noise. 8 00:00:35,250 --> 00:00:41,460 And in this case, the process model function itself has the noisy input as part of one of the parameters. 9 00:00:41,730 --> 00:00:46,530 So we'll have a look at how the prediction step changes with these two process model functions. 10 00:00:47,250 --> 00:00:51,990 So for the first one, the additive noise, which we're going to look at first, this is just a straightforward 11 00:00:51,990 --> 00:00:52,800 implementation. 12 00:00:52,800 --> 00:00:54,470 Using the incident transform. 13 00:00:54,780 --> 00:00:59,700 However, for the second case, for the general process, model noise is a bit more complicated. 14 00:00:59,700 --> 00:01:04,280 We're going to have to augment the state with the process model noise and we'll cover that later on 15 00:01:04,280 --> 00:01:04,910 in this video. 16 00:01:06,890 --> 00:01:12,290 So the first step in the prediction step for the additive process, more noise is to generate signal 17 00:01:12,290 --> 00:01:15,410 points for the onset and transform for the prediction step. 18 00:01:16,070 --> 00:01:21,950 So basically, we set the first single point for the zero point just to be the main of the current state. 19 00:01:22,520 --> 00:01:30,230 And then for all the other points, for I equals one to two n where and is the size of the state vector. 20 00:01:30,650 --> 00:01:35,920 We're going to evaluate this relationship here where this term here is going to be based on what I know 21 00:01:35,930 --> 00:01:36,530 we're looking at. 22 00:01:38,110 --> 00:01:43,870 So for the first I equals one to N we're going to be looking at this term here. 23 00:01:43,900 --> 00:01:49,440 So this is a square root of the covariance matrix multiplied by the weighting function here. 24 00:01:49,780 --> 00:01:56,650 And this is the eighth column of that matrix and then the Delta term for the rest of the terms from 25 00:01:56,650 --> 00:02:00,450 the end, plus one all the way up to 210 is going to be this term here. 26 00:02:00,460 --> 00:02:04,600 This is just going to be the negative of the square root of the Matrix. 27 00:02:04,600 --> 00:02:06,910 And we're going again for each of the columns. 28 00:02:08,650 --> 00:02:13,450 So this is the straightforward implementation that we've covered inside the enten and transform for 29 00:02:13,450 --> 00:02:15,190 the general and senator transform. 30 00:02:16,990 --> 00:02:22,060 So what this is saying is that if you look at the first single point for the zero point here, this 31 00:02:22,060 --> 00:02:26,230 signal point is just going to be a copy of the Maine or the state vector. 32 00:02:26,950 --> 00:02:31,750 Now, the next sticking point for point number one, and this is just going to be the Maine again, 33 00:02:31,840 --> 00:02:38,770 plus the weighting of the square root metrics of the first column and then for all the other terms for 34 00:02:38,770 --> 00:02:41,260 up to the tenth term is going to be the same thing. 35 00:02:41,270 --> 00:02:45,880 So we're going to have all the different columns of the Matrix square root matrix all the way up to 36 00:02:45,880 --> 00:02:46,150 end. 37 00:02:46,420 --> 00:02:48,250 And that's going to be the end single point. 38 00:02:49,540 --> 00:02:54,010 Now for the next and single points, we're going to do the same thing is that we're going to be subtracting 39 00:02:54,010 --> 00:02:57,220 the columns of the square root matrix rather than adding them. 40 00:02:57,220 --> 00:03:02,470 So I can say the only difference between these terms here and these terms here is just that we're subtracting 41 00:03:02,470 --> 00:03:03,090 the terms. 42 00:03:04,420 --> 00:03:10,360 The next step in the prediction process is then to use the process model of the system to predict all 43 00:03:10,360 --> 00:03:12,210 the single points forward in time. 44 00:03:12,700 --> 00:03:17,650 So the process is pretty much to take all the single points where I is equal zero all the way up to 45 00:03:17,650 --> 00:03:23,470 two and put them into the state transition function this function here and then calculate the transform 46 00:03:23,470 --> 00:03:24,190 to similar points. 47 00:03:25,720 --> 00:03:28,310 So we're looking at all the single points we've calculated before. 48 00:03:28,330 --> 00:03:33,670 What we do is for the first thing, the point we take it, we put it into the state transition matrix. 49 00:03:33,670 --> 00:03:38,650 We evaluate that and then we get the transform single point for the short term. 50 00:03:39,280 --> 00:03:42,640 And then we do pretty much do the same thing for all the other sigma points. 51 00:03:42,650 --> 00:03:47,890 So we put them into the state transition function one at a time, and then we end up with all the different 52 00:03:47,890 --> 00:03:49,090 transform sigma points. 53 00:03:50,050 --> 00:03:53,440 Now the next step is then to recover the main and the covariance. 54 00:03:54,020 --> 00:03:59,230 So to calculate the main, we basically calculate the weighted mean again using this equation here. 55 00:03:59,260 --> 00:04:01,780 So these are the transformed sigma points here. 56 00:04:02,140 --> 00:04:07,060 This is the weighting and the weightings again from the general unscented, transform, adjust these 57 00:04:07,060 --> 00:04:07,800 weightings here. 58 00:04:08,140 --> 00:04:11,260 Sorry, the first weighting is going be for our main. 59 00:04:11,590 --> 00:04:13,900 It's just going to be this kapper over and plus cappa. 60 00:04:14,320 --> 00:04:19,020 And then the waiting for all the other sigma points is going to be this time down here. 61 00:04:20,200 --> 00:04:23,640 And we do a similar process for when we're calculating the covariance. 62 00:04:23,650 --> 00:04:30,660 So we use the same weighting matrixes and then we just evaluate the covariance using this equation here. 63 00:04:30,940 --> 00:04:36,340 But in this term now to get the final covariance for the prediction, we have to increase the uncertainty 64 00:04:36,340 --> 00:04:37,210 based on the process. 65 00:04:37,230 --> 00:04:40,950 Brunoise and then to do that, this is where the key metrics come in. 66 00:04:41,470 --> 00:04:45,490 So this time here is the uncertainty from the transformation. 67 00:04:45,820 --> 00:04:49,800 And then this time he calculates the process, model uncertainty. 68 00:04:49,810 --> 00:04:52,810 So this adds the extra uncertainty for the process model. 69 00:04:53,320 --> 00:04:58,180 So this is very similar to how the linear commas literally expanded my field of work. 70 00:04:58,570 --> 00:05:03,460 We calculated first transform the uncertainty from the process model and then we add in the additional 71 00:05:03,460 --> 00:05:05,200 uncertainty from the transformation. 72 00:05:06,820 --> 00:05:11,140 So those steps were for the process model when we had the additive noise. 73 00:05:11,530 --> 00:05:17,860 Now we're going to look at the general case when we have processed more noise inside the process model 74 00:05:17,860 --> 00:05:18,340 function. 75 00:05:19,760 --> 00:05:24,590 So basically, we want to augment the state and covariates with the noise model and what that actually 76 00:05:24,590 --> 00:05:27,880 means is that we wanted to find a Augmentin state vector. 77 00:05:27,890 --> 00:05:29,300 So this is Aleksei. 78 00:05:29,780 --> 00:05:33,650 And the first and terms are going to be to be a copy of the state vector. 79 00:05:33,980 --> 00:05:37,870 And then the last terms are going to be the noise vector itself. 80 00:05:38,510 --> 00:05:42,540 So we're augmenting the state vector with the noise components. 81 00:05:43,700 --> 00:05:46,280 We also want to do the same thing for the covariance matrix. 82 00:05:46,280 --> 00:05:52,040 So we get this augmented covariance matrix with the diagonal terms being the original covariance for 83 00:05:52,040 --> 00:05:53,150 the state estimates. 84 00:05:53,450 --> 00:05:55,920 And then we had the covariance for the noise. 85 00:05:55,940 --> 00:05:59,600 So this is the key metrics and covariance of our noise vector here. 86 00:06:00,910 --> 00:06:06,100 So then we want to generate the signal points using the augmented state of covariance, so basically 87 00:06:06,100 --> 00:06:11,560 for every sigma point we're going to get the first end states being the state vector and then the last 88 00:06:11,560 --> 00:06:13,990 states going to be the noise into the system. 89 00:06:16,400 --> 00:06:21,440 So what that actually means is that we use a process model of the system to predict all the signal points 90 00:06:21,440 --> 00:06:26,230 forward in time, breaking each signal up into the state and noise inputs. 91 00:06:26,720 --> 00:06:31,820 So when we evaluate the prediction step, we break the argument and state vector of the signal points 92 00:06:31,940 --> 00:06:34,910 up into the state components and the noise components. 93 00:06:34,910 --> 00:06:41,210 And we put the state components into the state input and we put the noise inputs over here into the 94 00:06:41,210 --> 00:06:41,930 noise inputs. 95 00:06:42,230 --> 00:06:47,750 And we evaluate this state transition function to get the transform signal points here. 96 00:06:50,060 --> 00:06:55,010 Once we've got the transform signal points, it follows a very similar process, we want to first calculate 97 00:06:55,010 --> 00:07:01,340 the main of the new state, which is again just calculating the weighted average of the transform single 98 00:07:01,340 --> 00:07:01,700 points. 99 00:07:02,540 --> 00:07:04,490 How about when we calculate the covariance? 100 00:07:04,490 --> 00:07:11,330 We now no longer have to add in the Q matrix because we've calculated the covariance with the noise, 101 00:07:11,330 --> 00:07:13,140 uncertainty inside the precise model. 102 00:07:13,160 --> 00:07:17,900 So we basically just want to recover the covariance straight from the sigma points themselves. 103 00:07:20,010 --> 00:07:26,190 So hopefully this should be a fairly simple explanation of how to go about implementing the prediction 104 00:07:26,190 --> 00:07:28,200 step for the answer to come and filter. 105 00:07:29,220 --> 00:07:34,130 We can also compare this to the extent of Hilda that we've covered in the past. 106 00:07:34,500 --> 00:07:40,500 So for the process model prediction, i.e. the prediction step for the state, the extent to come and 107 00:07:40,500 --> 00:07:46,620 fill it up pretty much just took in the previous main vector for the state estimate, put it into the 108 00:07:46,620 --> 00:07:46,860 price. 109 00:07:46,860 --> 00:07:54,000 This model took the input in and then we predicted it forward to get the main predicted main on the 110 00:07:54,000 --> 00:07:54,430 output. 111 00:07:55,920 --> 00:08:02,270 Now when we do the same thing for the extended coming for the state prediction, it looks a bit different. 112 00:08:02,580 --> 00:08:09,120 We take in the original state estimates and the variance we generate our similar points according to 113 00:08:09,120 --> 00:08:10,590 whatever transformation we're going to use. 114 00:08:11,220 --> 00:08:16,580 Then we take our signal points and put them into the nonlinear process model to predict the Sigma points 115 00:08:16,590 --> 00:08:17,010 forward. 116 00:08:17,580 --> 00:08:24,450 Once we've had these transform signal points, then we can then estimate the main using this equation 117 00:08:24,450 --> 00:08:27,360 here and all of that will come our new prediction. 118 00:08:27,510 --> 00:08:34,980 So the the Appu I mean, we can also look at the same step for the covariance matrix, for the prediction 119 00:08:34,980 --> 00:08:35,370 step. 120 00:08:35,760 --> 00:08:40,760 So the extended comma filter basically took in the covariance matrix for the step. 121 00:08:41,160 --> 00:08:46,890 It used the linear approximation or the Jacobins or the models that this equation here. 122 00:08:46,890 --> 00:08:50,490 And then it gave us the new covariance prediction. 123 00:08:51,660 --> 00:08:56,730 Now, the answer to my filter, again, is very similar as when we calculate the main. 124 00:08:56,740 --> 00:09:01,950 So we have the same first couple of steps of calculating the signal points, transforming them through 125 00:09:01,950 --> 00:09:02,700 the process model. 126 00:09:03,210 --> 00:09:08,280 But then in the difference, we use this equation here to estimate the covariance matrix. 127 00:09:08,760 --> 00:09:10,230 So we calculate that the weighted. 128 00:09:11,240 --> 00:09:16,310 So we calculate the weighted covariance of the different signal points to get our new covariance matrix 129 00:09:16,310 --> 00:09:17,810 for the predicted step. 130 00:09:19,160 --> 00:09:25,220 So a quick summary of the prediction step equations for when we have additive noise, we first want 131 00:09:25,220 --> 00:09:28,220 to calculate the signal points using these equations here. 132 00:09:28,220 --> 00:09:31,100 So the Cirrus Sigma Point is just coming in the main. 133 00:09:31,100 --> 00:09:38,330 And then for the sigma point, one to end is just going to be the main plus each of the columns of the 134 00:09:38,330 --> 00:09:41,930 square root of the covariance matrix and then the remaining single points. 135 00:09:41,930 --> 00:09:46,820 I just give me the main minus the square root of the covariance matrix for the different columns. 136 00:09:49,540 --> 00:09:55,000 Next step is we want to do the state transformation, so we take our similar points, put them into 137 00:09:55,000 --> 00:10:02,010 our process model and transform them into the transform single points from the transform signal points. 138 00:10:02,090 --> 00:10:08,140 We can then estimate the state vector by calculating the weighted mean using these weighting functions 139 00:10:08,140 --> 00:10:08,440 here. 140 00:10:08,620 --> 00:10:13,690 And we can do the same thing for the covariance, making sure we add on the presence of noise at the 141 00:10:13,690 --> 00:10:15,700 end once we've calculated the covariance. 142 00:10:18,060 --> 00:10:23,220 The prediction that equations for when we have non-addictive noise, so in the general, Casey is going 143 00:10:23,220 --> 00:10:24,240 to be slightly different. 144 00:10:24,720 --> 00:10:27,650 The first thing we want to do is to augment the signal points. 145 00:10:27,650 --> 00:10:33,910 So basically we take the main vector and then we want to augment it with the noise vector down below. 146 00:10:33,930 --> 00:10:38,400 And we want to do the same thing with the covariance matrix before my augmented covariance matrix, 147 00:10:38,730 --> 00:10:44,100 with the diagonal matrix being the original covariance for the state estimates and then the covariance 148 00:10:44,100 --> 00:10:45,210 matrix for the noise. 149 00:10:46,380 --> 00:10:52,290 We then calculate our signal points using the augmented state vector and augmented covariance matrix, 150 00:10:52,890 --> 00:10:58,230 using the exact same process to calculate the sigma points, once we've got the same points, we can 151 00:10:58,230 --> 00:10:59,720 use them in the straight transition. 152 00:10:59,730 --> 00:11:02,100 So we take the augmented signal points. 153 00:11:02,100 --> 00:11:07,920 We break them down into the state and into the noise, putting them into the appropriate places inside 154 00:11:07,920 --> 00:11:14,280 the process model to transform the state vector into the new state or to predict the state once we've 155 00:11:14,280 --> 00:11:16,260 calculated these transform single points. 156 00:11:16,680 --> 00:11:22,770 Then again, we recover the main using the weighted estimate and we cover the covariance again using 157 00:11:22,770 --> 00:11:23,560 the weighted estimate. 158 00:11:23,760 --> 00:11:28,800 However, in this general case, we don't need to add on the extra noise term because it's already been 159 00:11:28,800 --> 00:11:33,300 calculated inside the transform signal points inside this calculation here.