1 00:00:03,940 --> 00:00:08,410 In this video, we're going to look at how to calculate the measurement innovation, and this is going 2 00:00:08,410 --> 00:00:13,810 to be the first step in the process of understanding the onset and coming through the update step. 3 00:00:14,970 --> 00:00:19,980 So just like all common filters, the common filter corrects for the state estimate errors by fitting 4 00:00:19,980 --> 00:00:23,560 back a way to time based on the observed measurement errors. 5 00:00:23,940 --> 00:00:29,280 So the first step in the update process is to calculate the measurement innovation, which is just another 6 00:00:29,280 --> 00:00:33,270 name for the measurement error and the uncertainty inside the amount of error. 7 00:00:33,300 --> 00:00:35,190 So this is the innovation covariance. 8 00:00:36,390 --> 00:00:37,690 We can predict the measurement. 9 00:00:37,710 --> 00:00:44,460 So I said hat and innovation covariance is using the unscented transform of the measurement model function. 10 00:00:45,960 --> 00:00:48,140 Then we can calculate the innovation vector. 11 00:00:48,150 --> 00:00:51,320 So this means to be the difference between the measurement. 12 00:00:51,330 --> 00:00:56,670 So I said hat that we used inside the unseen transform and then the true measurement that we get from 13 00:00:56,670 --> 00:01:01,550 the center said such that the innovation is just going to be equal to the difference. 14 00:01:01,560 --> 00:01:07,020 So the innovation is just the true measurement, minus our predicted measurement using the best information 15 00:01:07,020 --> 00:01:08,580 of the state at this time. 16 00:01:10,310 --> 00:01:15,290 We're going to break down the measurement innovation, step down into the two cases based on the measurement 17 00:01:15,290 --> 00:01:20,170 model noise, and then the first case is going to be again, for additive measurement model noise. 18 00:01:20,180 --> 00:01:26,000 So this is when we have noise on the sensor, which is just additive, so we can separate the nonlinear 19 00:01:26,000 --> 00:01:30,290 model of the sensor into the nonlinear model and then the noise. 20 00:01:31,220 --> 00:01:36,450 And then the second case is again going to be for the general or out of the measurement noise, noise. 21 00:01:36,770 --> 00:01:41,440 So this is when the measurement noise is part of the nonlinear function for the measurement model. 22 00:01:41,780 --> 00:01:45,370 So we can't just separate it out into an additive term. 23 00:01:46,010 --> 00:01:51,320 So just like we did with a prediction set, the first method is just going to be a straightforward implementation 24 00:01:51,320 --> 00:01:52,720 of the unseen to transform. 25 00:01:53,060 --> 00:01:57,800 And then just like before, to handle the second type of noise, we're going to have to augment the 26 00:01:57,800 --> 00:02:00,080 state with the measurement in the model noise. 27 00:02:00,920 --> 00:02:06,110 So first for the case when we just have additive noise, so when the noise is added on at the end, 28 00:02:06,650 --> 00:02:11,990 we can generate the signal points and weights for the Empire State and covariance matrix. 29 00:02:12,200 --> 00:02:16,850 We then can use the measurement model function to predict measurements for all the generated signal 30 00:02:16,850 --> 00:02:17,240 points. 31 00:02:17,480 --> 00:02:21,800 So basically for each signal point that we've generated, we can put it into the nonlinear measurement 32 00:02:21,800 --> 00:02:25,730 model and calculate a measurement predicted for that sigma point. 33 00:02:26,840 --> 00:02:31,310 Once we've done that, we can estimate what the actual predicted measurement is by taking the weighted 34 00:02:31,310 --> 00:02:31,730 mean. 35 00:02:31,740 --> 00:02:35,270 So this is just the same equation for the answer to transform. 36 00:02:35,280 --> 00:02:40,100 We're just calculating the weighted mean and that's going to give us the predicted measurement. 37 00:02:40,990 --> 00:02:45,070 Once we have attained the predictive measurement, then we can calculate the measurement innovation. 38 00:02:45,100 --> 00:02:49,540 So this is just the difference between the true sense and measurement and what we've just calculated 39 00:02:49,540 --> 00:02:50,770 for the predicted measurement. 40 00:02:51,660 --> 00:02:54,870 And then lastly, we can also calculate the measurement covariance. 41 00:02:54,880 --> 00:03:00,130 So we just take we calculate the weighted covariance, which is going to be our matrix. 42 00:03:00,130 --> 00:03:03,490 And then we also have to add on our noise uncertainty. 43 00:03:03,520 --> 00:03:05,110 So this is the sensor model noise. 44 00:03:05,470 --> 00:03:11,040 So we have to add on our metrics at the end to compensate for the amount of noise in the center. 45 00:03:12,480 --> 00:03:18,090 So overall is a pretty straightforward method for calculating the measurement innovation for when we 46 00:03:18,090 --> 00:03:19,260 have additive noise. 47 00:03:19,290 --> 00:03:23,690 So now let's have a look at the case when we have a general noise or non-native noise. 48 00:03:23,700 --> 00:03:27,800 So the noise is part of the nonlinear input to the measurement model function. 49 00:03:28,620 --> 00:03:33,620 And in this case, we have to augment the state a covariance with the measurement noise model. 50 00:03:34,020 --> 00:03:36,680 So first we want to generate an augmented state vector. 51 00:03:36,690 --> 00:03:40,400 So that's just the current state and then we have the noise component. 52 00:03:40,500 --> 00:03:45,510 At the end, we want to do the same thing to generate a augmented covariance matrix. 53 00:03:45,510 --> 00:03:51,480 So we have the original state covariance and then on the diagonal we have the noise covariance inside 54 00:03:51,480 --> 00:03:52,650 the sense of noise. 55 00:03:54,150 --> 00:04:00,750 Once we form the Augmentin state and covariance matrices, we can then generate the Sigma points based 56 00:04:00,750 --> 00:04:02,970 on these augmented state and covariance. 57 00:04:03,270 --> 00:04:09,510 So our Augmented Sigma points are going to have a component for the sigma point for the state and a 58 00:04:09,510 --> 00:04:10,980 single point for the noise. 59 00:04:13,080 --> 00:04:17,790 We then can use the measurement model function to predict the measurements for all the augmented generated 60 00:04:17,790 --> 00:04:18,450 sigma points. 61 00:04:18,900 --> 00:04:23,440 So basically inside our nonlinear sensor model, we take the augmented state vector. 62 00:04:23,460 --> 00:04:27,150 Break it down into the state and into the noise components. 63 00:04:27,160 --> 00:04:33,300 We can put them into the nonlinear measurement model and then we get our measurement for this. 64 00:04:33,300 --> 00:04:39,690 Each sigma point in turn, once we've got the single point, we pretty much do the same steps as we 65 00:04:39,690 --> 00:04:40,290 did before. 66 00:04:40,320 --> 00:04:46,400 We actually use the weighted mean of the signal transform signal points to estimate our predictive measurement. 67 00:04:47,280 --> 00:04:50,560 Once we have a predictive measurement, we then calculate the innovation. 68 00:04:50,580 --> 00:04:56,190 So the difference between the measurement and our prediction and then lastly, we calculate the innovation 69 00:04:56,190 --> 00:05:03,750 covariance matrix just based on the weighted covariance of the prediction sigma points based away from 70 00:05:03,750 --> 00:05:05,000 the main prediction. 71 00:05:05,640 --> 00:05:11,070 And in this case, we don't have to add in the metrics because we've generated these signal points based 72 00:05:11,070 --> 00:05:16,980 on the augmented state vector, which already has the uncertainty inside the measurement taking into 73 00:05:16,980 --> 00:05:17,460 account. 74 00:05:18,700 --> 00:05:24,280 So now for some extra clarity, let's compare the process for calculating the measurement innovation 75 00:05:24,280 --> 00:05:27,700 between the extended Kamasutra and the unscented comment filter. 76 00:05:28,540 --> 00:05:34,420 So like he would have a in the past, the extended homefield calculates the innovation by taking the 77 00:05:34,420 --> 00:05:39,790 current state, putting it into the measurement model, assuming zero noise and then just subtracting 78 00:05:39,790 --> 00:05:41,680 that from the true sense of measurement. 79 00:05:41,710 --> 00:05:45,670 So this is how we calculate the innovation in the extended common field. 80 00:05:46,240 --> 00:05:48,510 So it uses the nonlinear measurement model. 81 00:05:48,820 --> 00:05:55,060 However, we're estimating about the current state, the antenna come a filter is a bit more involved. 82 00:05:55,550 --> 00:05:58,600 So we take in the estimate the state and covariance. 83 00:05:58,810 --> 00:06:01,890 We generate our signal points based on this information here. 84 00:06:02,410 --> 00:06:08,080 We use these sigma points inside our known in the measurement model to get a number of predictive measurement 85 00:06:08,080 --> 00:06:08,490 points. 86 00:06:09,160 --> 00:06:15,460 Once we have all these predictive points we then use, we then estimate the main using the way to Maine 87 00:06:15,460 --> 00:06:16,990 to calculate, I predict a measurement. 88 00:06:18,050 --> 00:06:22,640 Once we had this predictive measurement, we then can calculate the innovation, so just the difference 89 00:06:22,640 --> 00:06:27,740 between the true measurement and a predicted measurement and we get the innovation at the end. 90 00:06:29,910 --> 00:06:36,960 We can also compare the steps for the calculation of the innovation covariance, so the matrix inside 91 00:06:36,960 --> 00:06:39,890 the extended filter, it's fairly straightforward. 92 00:06:39,900 --> 00:06:46,500 We take in the original covariance, we use the Jacobean approximation to the first order approximation 93 00:06:46,500 --> 00:06:51,950 of the nonlinear model, and then we can add in our added anois term at the end. 94 00:06:51,990 --> 00:06:55,170 So our matrix and out of this will come out. 95 00:06:55,170 --> 00:06:56,310 Innovation covariance. 96 00:06:56,910 --> 00:07:01,820 Now for the unscented Kamasutra, just like before, is a bit more complicated. 97 00:07:01,830 --> 00:07:07,140 We take in the current state of the system, the state and covariance generate the single points, put 98 00:07:07,140 --> 00:07:12,480 them into the nonlinear process model, and out of that we get a number of predicted measurements for 99 00:07:12,480 --> 00:07:13,190 the Sigma points. 100 00:07:13,590 --> 00:07:18,630 We calculate the way to Maine and then we can calculate the weighted covariance. 101 00:07:20,020 --> 00:07:26,080 So to summarize all this up, when we have additive noise, the first step is to calculate the signal 102 00:07:26,080 --> 00:07:28,210 points for the unseen and transform. 103 00:07:28,810 --> 00:07:34,300 Once we've done that, we can then put them into the measurement model to transform them into the measurement 104 00:07:34,300 --> 00:07:34,780 space. 105 00:07:35,650 --> 00:07:40,390 Then we can recover the measurement prediction by basically taking the weighted mean. 106 00:07:40,750 --> 00:07:47,440 And we can also then use that way to Maine to calculate the innovation and then we can recover the innovation 107 00:07:47,440 --> 00:07:54,580 covariance from taking the weighted covariance of the transform signal points and add on the additional 108 00:07:54,580 --> 00:08:02,760 time for the noise at the end for the case when we have non additive noise or general noise or a nonlinear 109 00:08:02,770 --> 00:08:07,680 transform, first thing we want to do is to augment the signal points with the noise model. 110 00:08:07,690 --> 00:08:13,750 So we on the to get the augmented state vector, we add the noise components to the end and for the 111 00:08:13,750 --> 00:08:18,070 augmented covariance matrix we add the measurement covariance at the end. 112 00:08:19,750 --> 00:08:25,240 Once we've done that, we then use the unscented and transform on these augmented points to find these 113 00:08:25,240 --> 00:08:30,970 augmented signal points, we then use the augmented signal points inside the nonlinear measurement model 114 00:08:30,970 --> 00:08:34,830 to calculate these transform measurement points for each single point. 115 00:08:35,470 --> 00:08:41,770 We then can use the way to Maine again, just like we did before, to calculate the measurement predicted. 116 00:08:43,560 --> 00:08:48,360 We can use the measurement predicted to calculate the innovation, and then we can also recover the 117 00:08:48,360 --> 00:08:53,160 innovation covariance by using the weighted covariance of the transform measurement points. 118 00:08:53,520 --> 00:08:59,630 However, in this case, we don't have to add on the additive noise termed The Matrix, because we've 119 00:08:59,640 --> 00:09:05,520 already taken into account when we calculated the signal points because we use the augmented covariance 120 00:09:05,520 --> 00:09:06,360 matrix up here. 121 00:09:07,710 --> 00:09:12,570 So this whole process should look very similar to the prediction step because it's pretty much just 122 00:09:12,570 --> 00:09:17,930 the unscented transform just applied to the measurement model rather than the process model.