1 00:00:03,810 --> 00:00:08,880 In this video, we're going to try to develop a deeper understanding and some insights into how the 2 00:00:08,880 --> 00:00:13,980 extended karma filter works and how different things affect it, and in particular, we will have a 3 00:00:13,980 --> 00:00:19,340 deeper look into trying to understand the effects of the linear approximation of the nonlinear uncertainty, 4 00:00:19,350 --> 00:00:20,030 transformation. 5 00:00:20,310 --> 00:00:26,190 And we will also have a look at how the large initial conditions or large estimation errors affect a 6 00:00:26,190 --> 00:00:28,350 solution with linear ization effects. 7 00:00:30,150 --> 00:00:35,440 Of the key assumptions that Linnear come, I feel to operate with is that the uncertainty in the system 8 00:00:35,440 --> 00:00:40,430 can be represented as a Gaussian distribution made up of a mean and covariance. 9 00:00:41,230 --> 00:00:47,620 This allows the linear transformation of the probability distribution to be used as the linear transformation 10 00:00:47,620 --> 00:00:51,050 of Gaussian distribution is just another Gaussian distribution. 11 00:00:52,810 --> 00:00:59,380 So what that means is that if we have a random variable vector X that is made from a normal distribution 12 00:00:59,380 --> 00:01:07,960 of a main export and covariance Sigma X, then we apply the linear transformation X plus B is equal 13 00:01:07,960 --> 00:01:14,740 to Y, then the random variable vector Y is just going to have A, it's just going to be a normal distribution 14 00:01:14,740 --> 00:01:15,860 with these parameters. 15 00:01:16,090 --> 00:01:21,760 The main is just going to be a linear transformation of the main of X and then the covariance is just 16 00:01:21,760 --> 00:01:27,300 going to be the transform variance into the wireframe using this transformation here. 17 00:01:28,720 --> 00:01:31,310 And we have seen this and prove this mathematically before. 18 00:01:31,690 --> 00:01:35,430 However, let's actually have a look at a numerical simulation example. 19 00:01:37,140 --> 00:01:44,370 So let's say we have a random variable X that is a two dimensional Gaussian distribution with Amane 20 00:01:44,370 --> 00:01:48,900 of exper and covariance Sigma X and we sample it 100 times. 21 00:01:48,930 --> 00:01:53,240 So these are the 100 samples of this normal distribution here. 22 00:01:55,350 --> 00:01:57,270 We can apply the linear transformation. 23 00:01:57,270 --> 00:02:03,290 Y is equal to X, plus be you, where A is the transformation vector and B is the offset vector. 24 00:02:03,600 --> 00:02:07,110 And we end up with this distribution of Y samples here. 25 00:02:07,740 --> 00:02:14,220 Now we can also plot the covariance of our examples, which is just plotting the ellipse for our three 26 00:02:14,220 --> 00:02:16,400 sigma value of these parameters here. 27 00:02:17,130 --> 00:02:23,220 And then we can use this covariance and we can transform it using our linear transformation, using 28 00:02:23,220 --> 00:02:25,590 this equation here that we've shown on the previous slide. 29 00:02:25,890 --> 00:02:28,770 And we can plot the answer on the Ellipse in green here. 30 00:02:29,190 --> 00:02:35,310 And we can see that the green, the linear transformation of the blue eyes only to the green on. 31 00:02:35,310 --> 00:02:37,840 So you can see that it fits the data very well. 32 00:02:38,850 --> 00:02:44,600 So this is the assumption that the linear homophily uses when we have a linear model. 33 00:02:45,150 --> 00:02:51,990 Now, we can confirm this is true by actually sampling and measuring the covariance of the Y data points 34 00:02:51,990 --> 00:02:55,230 to get a Y bar and sigma y. 35 00:02:55,470 --> 00:03:01,860 And what this means is that we just take all these data points, the green data point, and we can calculate 36 00:03:01,860 --> 00:03:05,980 these parameters numerically using our equations for the main. 37 00:03:06,000 --> 00:03:09,500 So we just sum up all the values divided by the number of samples to get the main. 38 00:03:09,840 --> 00:03:14,640 And can we also work out the covariance by using this equation here as well? 39 00:03:15,030 --> 00:03:20,400 So if we do this and get these two parameters here and then we got the covariance matrix, we get this 40 00:03:20,400 --> 00:03:27,810 red Dasht covariance ellipsoid here and these two, the green one and the red one are going to converge 41 00:03:27,810 --> 00:03:29,520 if we had more sample points. 42 00:03:29,820 --> 00:03:35,610 But you can see even with only 100 points, the covariance ellipses fit quite well with each other. 43 00:03:36,180 --> 00:03:42,510 So this shows that the linear transformation, this operation here is a very good well, it is a it 44 00:03:42,510 --> 00:03:46,890 is the actual answer for the uncertainty, transformation into the frame. 45 00:03:48,540 --> 00:03:54,030 So we know that the extended coming through allows us to operate on non-linear systems and it uses a 46 00:03:54,030 --> 00:03:56,990 linear approximation of the nonlinear system. 47 00:03:57,000 --> 00:04:02,580 So let's actually have a look at how acceptable that is in a numerical example such as this. 48 00:04:04,200 --> 00:04:07,400 So in this case, we're going to change it up a bit. 49 00:04:07,620 --> 00:04:11,070 We're going to have a nonlinear function for basically a range and bearing. 50 00:04:11,070 --> 00:04:18,120 So we have a range and a bearing theta and we want to take the parameters in this space is a range and 51 00:04:18,120 --> 00:04:22,020 bearing and convert it into contingent coordinates, X and Y. 52 00:04:22,410 --> 00:04:25,640 So you can see we have a range and bearing value. 53 00:04:25,950 --> 00:04:29,430 We put it through this transformation here and give X and Y and we plot it. 54 00:04:29,850 --> 00:04:32,130 So a range is this parameter here. 55 00:04:32,130 --> 00:04:33,690 A bearing is going to be this one. 56 00:04:34,020 --> 00:04:37,470 We take a parameter here and we transform it into the X and Y frame. 57 00:04:37,900 --> 00:04:42,860 So these 100 data points here are sampled from a distribution shown here. 58 00:04:42,870 --> 00:04:48,990 So we have a normal distribution for range of a main one hundred and a variance of five squared. 59 00:04:49,680 --> 00:04:54,840 And we also have the bearing distribution as being another normal distribution with a main value of 60 00:04:54,840 --> 00:04:58,510 forty five degrees and a variance of twenty five degrees squared. 61 00:04:59,070 --> 00:05:05,700 So one hundred point sampled from this, these two distributions he plotted in the X and Y plane gives 62 00:05:05,700 --> 00:05:07,320 us all these green points here. 63 00:05:07,800 --> 00:05:12,240 So these are the transform results of the nonlinear function here. 64 00:05:14,270 --> 00:05:19,430 Now we can look at what happens if we apply the extended karma filter approximation of the uncertainty 65 00:05:19,430 --> 00:05:20,160 transformation's. 66 00:05:20,510 --> 00:05:23,450 So this is where we calculate the jakovčić matrixes. 67 00:05:23,450 --> 00:05:29,780 So Alef Matrix education and multiply the covariance in the original frame to get the covariance in 68 00:05:29,780 --> 00:05:30,500 the final frame. 69 00:05:30,620 --> 00:05:37,010 The Jacobean matrix from the measurement model that we had on the previous slide is this matrix shown 70 00:05:37,010 --> 00:05:37,310 here. 71 00:05:37,640 --> 00:05:42,440 And our covariance matrix for the range in bearing is just going to be the variance parameters for the 72 00:05:42,440 --> 00:05:44,870 range and the variance parameters for a bearing. 73 00:05:45,410 --> 00:05:50,870 So if we take these two parameters here, do this operation, find out the uncertainty inside the X 74 00:05:50,870 --> 00:05:56,900 and Y frame and plot that we get the green Lipps there and we can see that the green ellipse is not 75 00:05:56,900 --> 00:06:01,790 the best approximation of this distribution for all these green points. 76 00:06:02,480 --> 00:06:04,830 So this is what the extent Kamasutra uses. 77 00:06:05,270 --> 00:06:10,970 However, if we actually calculate the if we actually calculate the mean and covariance from all these 78 00:06:10,970 --> 00:06:16,460 data points numerically like we did in past, we get this measured uncertainty ellipse here. 79 00:06:16,460 --> 00:06:21,920 And you can see that this is a well, this is the true uncertainty inside this data set here. 80 00:06:22,190 --> 00:06:27,950 And this distribution is where the mean and covariance a sample numerically and measured from the data 81 00:06:27,950 --> 00:06:35,180 directly by doing the domain of Rasor and the expectation of radar to get the main and the covariance. 82 00:06:35,970 --> 00:06:41,300 So we can see in this case here the linear approximation of the nonlinear function for the uncertainty, 83 00:06:41,810 --> 00:06:45,050 which the extent of Kamasutra uses is not always the best fit. 84 00:06:45,680 --> 00:06:51,410 So there's going to be cases where the uncertainty inside the extended Kamasutra is not the best approximation 85 00:06:51,410 --> 00:06:53,490 of the true uncertainty inside the system. 86 00:06:54,080 --> 00:07:00,230 We can actually have a look at the green ellipse here is pretty valid for when we're near the main. 87 00:07:00,260 --> 00:07:05,570 So over here you can see that if it's this so we can see the distribution inside this area here, if 88 00:07:05,570 --> 00:07:06,160 it's very well. 89 00:07:06,440 --> 00:07:12,110 But as we get away from all initialization point, it breaks down in terms of how good approximation 90 00:07:12,110 --> 00:07:12,530 it is. 91 00:07:14,840 --> 00:07:18,020 And we can see that by carrying out the same experiment again. 92 00:07:18,020 --> 00:07:23,060 But this time we're going to change our uncertainty inside Alberini measurement to be five degrees. 93 00:07:23,090 --> 00:07:24,910 So from twenty five to five degrees. 94 00:07:25,310 --> 00:07:29,990 So now we can say that everything sort of stays in the linear range of the system and we can see that 95 00:07:29,990 --> 00:07:35,990 our Jacobean approximation and our measured approximation of the uncertainty inside the system are pretty 96 00:07:36,740 --> 00:07:40,690 are a lot more correct, a lot more similar. 97 00:07:40,700 --> 00:07:43,240 So I can say that this becomes a better approximation. 98 00:07:43,490 --> 00:07:49,970 So the smaller the uncertainty is and closer to the linear ascension point, the better the approximation 99 00:07:49,970 --> 00:07:50,570 is going to be. 100 00:07:52,300 --> 00:07:57,540 So we can say that the Ukrainian transformation is a linear approximation of the non-linear transform, 101 00:07:58,180 --> 00:08:02,690 and it works quite well if the system is approximately linear at the winterization point. 102 00:08:03,190 --> 00:08:09,640 We can also see that large uncertainties can lead to more linear causation error effects depending on 103 00:08:09,640 --> 00:08:10,470 the measurement model. 104 00:08:11,050 --> 00:08:15,450 We can see that the linear approximation is most accurate in the linear ization point. 105 00:08:15,460 --> 00:08:20,590 And we're going to go into more detail in the second half of this video on the effects of linear ization. 106 00:08:21,160 --> 00:08:27,820 The linear approximation of uncertainty that uses the Jacobean tends to underestimate the error in the 107 00:08:27,820 --> 00:08:28,320 system. 108 00:08:28,570 --> 00:08:34,900 And this means it leads to overconfident estimates so that the error, uncertainty inside the extended 109 00:08:34,900 --> 00:08:39,790 computer usually underestimates how much uncertainty in the real system it has. 110 00:08:40,480 --> 00:08:45,910 And if when we're operating the extended field, there's a large mismatch in the true uncertainty of 111 00:08:45,910 --> 00:08:50,960 the system and what it thinks it is, then this can actually lead to a degraded performance and the 112 00:08:51,310 --> 00:08:53,200 field can actually start to diverge. 113 00:08:54,220 --> 00:08:57,610 Now we can have a look at what happens when we have large estimation errors. 114 00:08:57,910 --> 00:09:02,500 So let's have an example where our state vector is just going to be a single scalar and it's just going 115 00:09:02,500 --> 00:09:04,180 to be parameter X. 116 00:09:05,020 --> 00:09:08,930 We also have a measurement model which is linear and it looks like this. 117 00:09:09,400 --> 00:09:15,580 So we have a Y equals to X and it's just going to be a linear and it's just going to be a linear function 118 00:09:15,580 --> 00:09:16,420 as shown here. 119 00:09:19,150 --> 00:09:23,170 Let's assume that our true state is given by this value here. 120 00:09:23,200 --> 00:09:28,500 So this is a true value is and at this true value, this is what the measurement is going to be. 121 00:09:28,510 --> 00:09:32,060 So we can have a Y is equal to X where X is the true value. 122 00:09:32,680 --> 00:09:36,400 So this is what the sensor or measurement information is going to get. 123 00:09:37,090 --> 00:09:43,480 However, let's assume that we have an estimated value for our current best estimate of X to be this 124 00:09:43,480 --> 00:09:44,260 parameter here. 125 00:09:44,350 --> 00:09:46,580 So we can say we have a large estimation error. 126 00:09:47,320 --> 00:09:53,470 So when we calculate what our think, what we think the measurement should be, it will be this primarily. 127 00:09:53,570 --> 00:09:56,680 So this is going to be our predicted measurement for the current state. 128 00:09:58,530 --> 00:10:04,140 So when we evaluate our recovery metrics for our best estimate now, it's going to be the Caribbean 129 00:10:04,140 --> 00:10:08,250 at this point here and we're going to see that the Caribbean is going to be positive. 130 00:10:08,430 --> 00:10:13,140 So that's pretty much saying that the slope of this line here, the measurement function is going to 131 00:10:13,140 --> 00:10:13,850 be positive. 132 00:10:13,860 --> 00:10:16,570 So it's going to be in this direction, increasing this way. 133 00:10:18,150 --> 00:10:24,480 So when we use a positive Jacobean and we calculate the common gain metrics and then we do this operation 134 00:10:24,480 --> 00:10:24,660 here. 135 00:10:24,690 --> 00:10:27,300 So this is the innovation between our measurement. 136 00:10:27,450 --> 00:10:29,280 So that's going to be this value over here. 137 00:10:29,310 --> 00:10:33,810 So this is going to be measurement made about the true state and we're going to subtract, subtract 138 00:10:33,810 --> 00:10:35,230 off the predicted measurement. 139 00:10:35,250 --> 00:10:37,410 So this is what we think the measurements are going to be about. 140 00:10:37,410 --> 00:10:43,080 Our estimate is that if we do this innovation, multiply it by our common gain. 141 00:10:43,590 --> 00:10:45,680 So this whole term here is going to be negative. 142 00:10:45,690 --> 00:10:47,020 It's going to be smaller than zero. 143 00:10:48,330 --> 00:10:54,720 So when we do the common update step, we know that the updated state estimate, our X plus is going 144 00:10:54,720 --> 00:11:00,790 to be smaller than our current best estimate, X minus, because this term here is smaller than zero. 145 00:11:01,140 --> 00:11:04,310 So the updated state is going to be over here somewhere. 146 00:11:04,530 --> 00:11:07,680 So we can say this is where our updated estimates are going to be. 147 00:11:09,650 --> 00:11:14,270 We look at the error inside the system, so the difference between the true position and the estimated 148 00:11:14,270 --> 00:11:20,660 position, we can see after we do the update, this amount of error here has decreased compared to the 149 00:11:20,660 --> 00:11:22,800 true statements of previous best estimate. 150 00:11:23,210 --> 00:11:29,360 So our estimation error after we do the update is smaller than the estimation error before the update. 151 00:11:29,840 --> 00:11:35,620 So this leads to a smaller estimation error as we do the update, which is what we want to do. 152 00:11:36,020 --> 00:11:37,970 This is the whole purpose of the common filter. 153 00:11:39,590 --> 00:11:43,940 So we can see that when we do this on a linear system, we have a nice response. 154 00:11:43,940 --> 00:11:46,250 The UPD predicted the update. 155 00:11:46,250 --> 00:11:48,170 The state moves towards the true state. 156 00:11:48,860 --> 00:11:50,780 So now let's have a look at this example. 157 00:11:50,960 --> 00:11:56,390 But instead of using a linear measurement model, we're going to use a nonlinear measurement model that 158 00:11:56,390 --> 00:11:57,350 now looks like this. 159 00:11:58,370 --> 00:12:04,580 So even though our true state and estimates are the same at and then our measurement and our predicted 160 00:12:04,580 --> 00:12:08,570 measurement are exactly the same, we have a different describing metrics. 161 00:12:08,590 --> 00:12:14,600 So now our describing at this point is going to be negative because we have the slope decreasing over 162 00:12:14,600 --> 00:12:16,250 here at this linear regression point. 163 00:12:17,090 --> 00:12:20,750 So now let me do the common update multiplied by the innovation. 164 00:12:20,900 --> 00:12:23,530 This whole time here is going to be greater than zero. 165 00:12:23,540 --> 00:12:29,150 So it's going to be positive, which means that the value of the optimal state is going to be greater 166 00:12:29,180 --> 00:12:31,250 than the previous best estimate. 167 00:12:31,760 --> 00:12:36,340 So that means that our estimate is going to move this way in this direction here. 168 00:12:36,890 --> 00:12:40,280 And this means that we've actually increased the amount of error. 169 00:12:40,280 --> 00:12:46,370 So the error between the true state and estimate of state before the update, this amount here and after 170 00:12:46,370 --> 00:12:48,810 the update is this amount over here. 171 00:12:49,190 --> 00:12:54,950 So the estimation error after the update is actually larger than the estimation error before the update. 172 00:12:55,400 --> 00:12:57,300 So we have a larger estimation error. 173 00:12:57,710 --> 00:12:59,850 So this means that failure is actually diverging. 174 00:13:00,050 --> 00:13:02,860 The more we run it, the worse the estimate is going to get. 175 00:13:02,870 --> 00:13:03,890 And we don't want this. 176 00:13:03,900 --> 00:13:06,000 This is where the system becomes unstable. 177 00:13:06,350 --> 00:13:11,560 So this is the effect of large estimation errors on a nonlinear system. 178 00:13:11,960 --> 00:13:18,770 We can get to a state where the linear approximation does not hold is no longer valid for the point 179 00:13:18,770 --> 00:13:23,470 that we are evaluating the system about, and it means the filter will not converge to the truth. 180 00:13:24,080 --> 00:13:27,360 So this is one of the problems with the extended common filter. 181 00:13:27,590 --> 00:13:32,060 We always want the estimated state to be true close to the true state, which we don't know. 182 00:13:32,900 --> 00:13:34,550 So the filter operates correctly. 183 00:13:36,110 --> 00:13:42,650 So large estimation errors can occur when we have wrong or incomplete initial conditions, such as if 184 00:13:42,650 --> 00:13:47,660 the vehicle is initialized to be traveling at zero degrees heading, but is really traveling at 180 185 00:13:47,660 --> 00:13:48,340 degrees heading. 186 00:13:48,350 --> 00:13:51,140 We've seen that this can cause the field to diverge. 187 00:13:51,770 --> 00:13:57,890 Large estimation errors can also occur when we get bad faulty data or measurements that get fused into 188 00:13:57,890 --> 00:13:58,430 the system. 189 00:13:58,790 --> 00:14:04,760 It can drive the estimated state away from the true state, which then which then in turn causes the 190 00:14:04,760 --> 00:14:05,870 field to diverge. 191 00:14:06,170 --> 00:14:10,760 So this is why we confuse bad data or measurements into the filter is not robust. 192 00:14:11,840 --> 00:14:17,570 Large estimation errors can also happen when we miss data so we get a lack of information for a long 193 00:14:17,570 --> 00:14:18,320 period of time. 194 00:14:18,500 --> 00:14:20,090 The filter can actually start to drift. 195 00:14:20,090 --> 00:14:26,600 And if it drifts too far away from the true state, then these large linear ization effects can happen 196 00:14:26,600 --> 00:14:28,820 actually cause a filter to diverge. 197 00:14:30,660 --> 00:14:37,450 We can also get large estimation errors by bad moral assumptions, so if the model that we model inside 198 00:14:37,450 --> 00:14:45,300 the system does not represent the true model of the system, estimating this can cause large estimation 199 00:14:45,300 --> 00:14:46,070 errors as well. 200 00:14:51,920 --> 00:14:57,500 So large estimation errors can cause the extent to camouflage the verge when the linear approximation 201 00:14:57,530 --> 00:15:02,960 assumptions are invalid for the current condition and we can't tell that this is happening. 202 00:15:03,590 --> 00:15:07,310 To be able to tell that this is happening, we would need to know what the true state of the system 203 00:15:07,310 --> 00:15:11,870 really is to calculate the estimation error, to see whether it's increased after the update. 204 00:15:12,470 --> 00:15:19,400 So this is why we need careful tuning and simulation of the extent field to make sure that it works. 205 00:15:21,410 --> 00:15:26,950 So we need to test all the assumptions and operating points to make sure that the filter stays stable 206 00:15:26,990 --> 00:15:28,550 and it's giving us good results. 207 00:15:29,300 --> 00:15:34,610 Now, we know that inertial navigation systems use nonlinear equations and are typically implemented 208 00:15:34,610 --> 00:15:36,340 using the extended common filter. 209 00:15:36,800 --> 00:15:42,590 But in general it works because the nonlinear navigation equations can be approximated quite well with 210 00:15:42,590 --> 00:15:47,540 linear approximations if we have some idea about what the current state is.