1 00:00:04,220 --> 00:00:10,370 So let's have a look at a correctly tuned common filter, so using this to track a problem that we've 2 00:00:10,370 --> 00:00:16,070 been developing, we're going to run the simulation with the position measurement noise or the X and 3 00:00:16,070 --> 00:00:18,670 Y position standard deviation set to be ten. 4 00:00:19,100 --> 00:00:22,550 We're going to assume a straight path motion for the object. 5 00:00:23,420 --> 00:00:26,280 We're going to start at a random origin. 6 00:00:26,540 --> 00:00:30,110 We're going to be traveling at a random speed and heading OK. 7 00:00:30,110 --> 00:00:35,390 Now for the properties of the common filter, we're going to say that the acceleration, standard deviation 8 00:00:35,600 --> 00:00:38,500 that which affects the key matrix is going to be zero. 9 00:00:39,140 --> 00:00:41,800 And we're going to set the measurement standard deviation to be 10. 10 00:00:42,050 --> 00:00:43,550 So this is the matrix. 11 00:00:43,550 --> 00:00:48,650 So, of course, that we're going to set the common field of noise variance for the sensor measurement 12 00:00:48,650 --> 00:00:52,970 model to be ten, because that's the same as what we've actually done in simulation. 13 00:00:54,860 --> 00:00:58,670 So and also we're going to run it with a delayed initialization. 14 00:00:59,030 --> 00:01:00,590 So when you run this, come and fill it up. 15 00:01:01,800 --> 00:01:07,380 We can see the results, so if we have a look over here at the measurement innovations for the X and 16 00:01:07,380 --> 00:01:11,570 Y components, you can see that the measurement innovations are zero mean. 17 00:01:12,030 --> 00:01:13,310 So the Senate about zero. 18 00:01:13,320 --> 00:01:16,710 So this is the first indication that the filter is working correctly. 19 00:01:17,490 --> 00:01:21,240 We also have a look at the innovation covariance. 20 00:01:21,240 --> 00:01:24,360 So the plus or minus one sigma for the X and for the Y. 21 00:01:24,570 --> 00:01:32,190 So this green dash line here, we can see that it sort of converges to a level of about we'll call that 22 00:01:32,190 --> 00:01:37,770 10 plus minus ten for one sigma, for the X and for the Y innovation. 23 00:01:37,980 --> 00:01:42,150 It converges to about the same level plus or minus ten for one sigma. 24 00:01:42,810 --> 00:01:48,450 And if we look at the standard deviation for the X innovation and Y innovation, which is plotted out 25 00:01:48,450 --> 00:01:52,830 in the simulation, we can see we get a value of about a 10 and a value of 11. 26 00:01:53,560 --> 00:01:57,830 So according to this, the filter is very well trained and that's to be expected. 27 00:01:57,840 --> 00:02:04,410 We're using the true noise density properties and we set that the standard deviation for the acceleration 28 00:02:04,410 --> 00:02:04,990 to be zero. 29 00:02:05,280 --> 00:02:06,960 So there's no process model noise. 30 00:02:08,520 --> 00:02:14,280 So if we look at the actual results, we can see that we're using delayed initialization, so our initial 31 00:02:15,090 --> 00:02:17,280 position starts off at the first measurement. 32 00:02:17,280 --> 00:02:21,630 So you can see here the state assessment starts off at this measurement here and same down here, although 33 00:02:21,630 --> 00:02:22,450 it's a bit hard to see that. 34 00:02:22,510 --> 00:02:29,610 So, I mean, you can see the velocity starts of zero zero because we have no idea what the initial 35 00:02:29,610 --> 00:02:30,360 velocity is. 36 00:02:30,360 --> 00:02:33,410 But you can see that as if it were to get to position measurements. 37 00:02:33,420 --> 00:02:36,480 It quickly converges down to the true value. 38 00:02:37,050 --> 00:02:40,550 And the same thing up here, you can see it starts of zero and it quickly converges. 39 00:02:41,940 --> 00:02:48,960 If we look at the x y position error, you can see that our standard deviation for how sorry I can say 40 00:02:48,960 --> 00:02:55,850 that our three sigma variance for the position error, it contains our state estimates with the error 41 00:02:56,220 --> 00:02:58,320 and the same thing pretty much for the Y position. 42 00:02:58,330 --> 00:03:02,910 You can see that most of the error is contained within plus or minus three sigma. 43 00:03:02,910 --> 00:03:07,550 So that's working correctly and likewise for the X and Y velocity. 44 00:03:07,560 --> 00:03:14,050 You can say that a nice a nice convergence for the Vocera and something for the year. 45 00:03:15,060 --> 00:03:20,130 Now if we have a look at a final X and Y position, error, uncertainty. 46 00:03:20,220 --> 00:03:21,950 So this is for our three sigma. 47 00:03:22,740 --> 00:03:29,610 We can see that we have a three sigma value of plus or minus five meters and the same thing probably 48 00:03:29,610 --> 00:03:33,330 for the Y direction as well, about plus or minus five meters. 49 00:03:34,260 --> 00:03:35,810 So that's for the three sigma value. 50 00:03:36,390 --> 00:03:41,430 So that's very good considering that our overall position measurements shown here that have a variance 51 00:03:41,610 --> 00:03:48,810 or they have a standard deviation, I should say, of 10 meters so we can see that we've pretty. 52 00:03:49,590 --> 00:03:55,380 So you can see that our position error is a lot smaller than 10 meters and this is three times the standard 53 00:03:55,380 --> 00:03:56,040 deviation. 54 00:03:56,640 --> 00:04:03,150 So we're about a standard deviation for the X and Y estimates of about what we say that too. 55 00:04:04,110 --> 00:04:04,980 So that's very good. 56 00:04:05,010 --> 00:04:06,630 So basically drop the variance down. 57 00:04:07,350 --> 00:04:11,880 So we basically drop the standard deviation down from about 10 to about two. 58 00:04:13,440 --> 00:04:18,960 If we also have a look at our velocity, you can see that initially when we have a large uncertainty, 59 00:04:19,110 --> 00:04:20,600 our velocity jumps around a lot. 60 00:04:20,940 --> 00:04:27,270 But as we longer we around the field, the uncertainty converges down so that the jumps in smaller and 61 00:04:27,270 --> 00:04:30,900 smaller and we end up converging pretty much to the truth. 62 00:04:30,900 --> 00:04:37,010 And you can see that as along this time here are vastly jumps a lot smaller. 63 00:04:37,020 --> 00:04:40,410 This is because you have less uncertainty in the side, the system. 64 00:04:40,980 --> 00:04:46,950 And if we look at the end here for hour plus or minus three sigma value, we can see that we've got 65 00:04:46,950 --> 00:04:51,720 a pretty good estimate of our velocity was converted to almost zero error. 66 00:04:52,140 --> 00:04:58,470 And the uncertainty is plus or minus point one meters per second for the three sigma value. 67 00:04:59,250 --> 00:05:05,280 So you can see that this is a very good estimate of what the velocity is and we don't even have any 68 00:05:05,280 --> 00:05:10,920 sense to measure the velocity at all is done through precision measurements and then the common field 69 00:05:10,920 --> 00:05:13,270 of process model of the dynamic system. 70 00:05:14,040 --> 00:05:15,750 So this field is working very well. 71 00:05:17,530 --> 00:05:21,800 So now let's rerun the simulation for a field that's not correctly trained. 72 00:05:22,240 --> 00:05:25,430 So now let's overestimate how good a sensor actually is. 73 00:05:25,450 --> 00:05:31,160 So let's say the measurement deviation, we assume it to be a standard deviation of one. 74 00:05:31,900 --> 00:05:33,140 But in reality, it's ten. 75 00:05:33,160 --> 00:05:34,240 So now if you run this. 76 00:05:39,510 --> 00:05:45,990 You can see we get a very different response, so now you can see the variance in our innovations is 77 00:05:45,990 --> 00:05:51,900 a lot larger than our plus or minus one sigma for the innovation experience. 78 00:05:51,930 --> 00:05:53,340 So this is from The Matrix. 79 00:05:53,730 --> 00:05:56,730 So you can see that this Philidor here is not quickly change. 80 00:05:56,740 --> 00:06:03,450 So even though we have about zero mean for the innovation, the covariance is not indicative of the 81 00:06:03,450 --> 00:06:05,190 actual variance inside the system. 82 00:06:05,610 --> 00:06:07,350 So the filter is inconsistent. 83 00:06:08,490 --> 00:06:11,320 And you can see this again on these graphs here. 84 00:06:11,760 --> 00:06:17,820 So even though the velocity might converge down to a four, the true value will we get down towards 85 00:06:17,820 --> 00:06:18,490 zero error? 86 00:06:19,170 --> 00:06:22,200 You can see that our position is jumping around so much. 87 00:06:22,260 --> 00:06:26,570 This is because we assume that every position measured when we get is very accurate. 88 00:06:26,880 --> 00:06:32,550 So the filter is always going to be trying to update to that position measurement rather than trust 89 00:06:32,550 --> 00:06:38,760 the model that it should be trusting more so you can see these large jumps in position and we can see 90 00:06:38,760 --> 00:06:44,110 most of the time our position is outside of our uncertainty balance. 91 00:06:44,130 --> 00:06:46,050 So this means the field is inconsistent. 92 00:06:47,260 --> 00:06:52,420 So these are indications of the field not being very well tuned. 93 00:06:55,880 --> 00:06:58,980 So now let's set the measurement standard deviation back to 10. 94 00:06:59,000 --> 00:07:00,170 So back to the true value. 95 00:07:00,500 --> 00:07:05,970 Now, what happens if we have like lots of uncertainty inside the system for the process model? 96 00:07:05,990 --> 00:07:09,760 So let's have a key matrix that's non-zero now. 97 00:07:09,770 --> 00:07:13,370 So let's set the acceleration standard deviation, three point one. 98 00:07:14,030 --> 00:07:19,970 So now we're introducing unneeded uncertainty inside the system through the process model. 99 00:07:19,970 --> 00:07:21,380 And let's run this. 100 00:07:32,380 --> 00:07:38,080 OK, so now looking at this, you might say, oh, it's working fairly well and yes, is still working 101 00:07:38,080 --> 00:07:43,510 fairly well, you can see that our innovation convenience is about 10 plus or minus again. 102 00:07:43,720 --> 00:07:46,660 And the same thing for the Y and it's main. 103 00:07:46,990 --> 00:07:53,360 And our innovation sequence has a standing ovation of about 11 and about one call at ten. 104 00:07:53,800 --> 00:07:59,260 So looking at the innovation and mystical properties of the innovation, it's fairly is working fairly 105 00:07:59,260 --> 00:07:59,490 well. 106 00:08:00,130 --> 00:08:07,180 But if we actually have a look at what's going on in the estimates, you can see now how position uncertainty 107 00:08:07,330 --> 00:08:11,050 in the X and Y for estimate is not as much is not as converged. 108 00:08:11,620 --> 00:08:18,760 It has not as it has not converged as well as it did in previous example in the Crickley chain Furo, 109 00:08:18,780 --> 00:08:24,260 we had an uncertainty of about four three seamer of plus or minus five here. 110 00:08:24,300 --> 00:08:29,740 Now we've got the uncertainty about plus or minus 10 for position and for the velocity. 111 00:08:29,740 --> 00:08:36,880 You see that we can't converge on a true velocity as well because we no longer have, because we are 112 00:08:36,880 --> 00:08:39,330 introducing more uncertainty inside the system. 113 00:08:39,340 --> 00:08:46,180 So now the velocity error, uncertainty is about plus or minus one for the three sigma value because 114 00:08:46,180 --> 00:08:47,740 of this uncertainty inside the system. 115 00:08:47,740 --> 00:08:52,300 As the filter runs, you can see that we're getting these larger jumps in the velocity the longer we 116 00:08:52,300 --> 00:08:52,670 run it. 117 00:08:53,050 --> 00:08:56,260 So this is the uncertainty inside the system that we're adding through. 118 00:08:56,260 --> 00:09:01,690 The process model is not letting the field all converge to a true value and it's not letting it stay 119 00:09:01,690 --> 00:09:02,280 converged. 120 00:09:02,290 --> 00:09:06,190 The true value, because every time we run the prediction step, we're going to get more uncertainty 121 00:09:06,190 --> 00:09:06,940 inside the system. 122 00:09:06,940 --> 00:09:10,800 So you can see that the uncertainty is causing the uncertainty grow up. 123 00:09:11,110 --> 00:09:16,120 So you can see the process model uncertainty is causing the uncertainty inside the estimates to increase. 124 00:09:17,050 --> 00:09:23,890 So our position error and was the error uncertainty does not converge to as a small level. 125 00:09:24,250 --> 00:09:29,890 So there's still more uncertainty inside the system, which lets the coming for updated move the state 126 00:09:29,920 --> 00:09:30,790 around a lot more. 127 00:09:31,520 --> 00:09:35,680 So we're trying to use these vosta estimates for something else, like for control. 128 00:09:35,920 --> 00:09:40,740 We're going to get a very noisy signal because every time we get a position, update is going to try 129 00:09:40,750 --> 00:09:43,450 to change velocity to make the field consistent with that. 130 00:09:44,680 --> 00:09:49,150 So this is just one example of a filter that's not tuned as well as it could be tuned. 131 00:09:51,320 --> 00:09:56,630 Now, all these examples of how the system model that replicates what's actually happening in real life 132 00:09:56,900 --> 00:10:01,640 is is a tracking, an object that's moving at a constant velocity. 133 00:10:02,090 --> 00:10:05,240 What happens if we had a bit of error inside the process model? 134 00:10:05,270 --> 00:10:11,270 So what happens if we actually assume that the object was travelling at a constant velocity, but it 135 00:10:11,270 --> 00:10:15,550 wasn't actually traveling at a constant velocity, it had a non-zero acceleration value. 136 00:10:16,160 --> 00:10:18,720 So we can have a look at this in the simulation. 137 00:10:18,740 --> 00:10:22,330 So it set our acceleration, standard deviation back down to zero. 138 00:10:22,880 --> 00:10:26,480 And let's try this change the motion type from straight to linear. 139 00:10:27,110 --> 00:10:34,100 So what this motion model is now going to do under law in the simulation is going to assume a constant 140 00:10:34,400 --> 00:10:36,130 but non-zero acceleration. 141 00:10:36,140 --> 00:10:40,550 And this acceleration, random constant is going to be chosen at the start of the simulation. 142 00:10:41,360 --> 00:10:42,590 So now let's run this. 143 00:10:43,630 --> 00:10:48,640 So the filter is not going to be going to be able to perform as well, so if you look at how well it 144 00:10:48,640 --> 00:10:54,080 tracks, you can see that the true object is getting ahead of it while we're lagging behind. 145 00:10:54,700 --> 00:10:58,870 So if we look at our innovations, you can see that we're not at a zero. 146 00:10:58,880 --> 00:11:02,520 I mean, you can say innovation starting to get larger and larger and larger. 147 00:11:03,010 --> 00:11:06,360 And this is because we have unmoral dynamics in the true system. 148 00:11:06,850 --> 00:11:13,060 So if we look at how the velocity of the object that we're trying to track, the true velocity, the 149 00:11:13,060 --> 00:11:17,770 blue line here and the blue line here, you can see that is no longer a constant velocity, is not a 150 00:11:17,770 --> 00:11:18,720 straight line anymore. 151 00:11:18,730 --> 00:11:20,860 We have a acceleration value. 152 00:11:22,780 --> 00:11:28,780 So since the filter assumes a constant velocity and and a zero acceleration value, there's not enough 153 00:11:28,780 --> 00:11:32,400 flexibility in the system to let the filter estimates catch up. 154 00:11:32,740 --> 00:11:36,690 So you can see that our measurements are following the true state as they should. 155 00:11:37,570 --> 00:11:42,280 But I feel that does not have enough uncertainty inside the system to let the filter catch up to what 156 00:11:42,280 --> 00:11:42,790 this is. 157 00:11:43,780 --> 00:11:49,990 So to get around this, this is where we have to start using our key metrics by adding extra uncertainty 158 00:11:49,990 --> 00:11:51,760 inside the system to help catch up. 159 00:11:52,360 --> 00:11:58,500 So now let's set our acceleration, starvation back to point one and run the filter again. 160 00:12:01,360 --> 00:12:06,280 We should be able to see the foot of trucks, the accelerating object, a lot better now. 161 00:12:06,490 --> 00:12:12,790 So now we have a zero mean for the innovations and the innovation variance is about the same size as 162 00:12:12,880 --> 00:12:13,330 it should be. 163 00:12:13,960 --> 00:12:21,370 And if you look at our graphs here, you can see that the filter is now tracking our non-zero acceleration. 164 00:12:21,380 --> 00:12:23,460 So we now it's tracking the velocity better. 165 00:12:24,760 --> 00:12:26,200 But you can see there's been a trade off. 166 00:12:26,200 --> 00:12:33,730 Now since we're using a the key metrics to add more uncertainty into the system, the velocity estimates 167 00:12:33,850 --> 00:12:36,600 no longer converge down to a very smooth value. 168 00:12:36,610 --> 00:12:42,130 You can see now, since we had to add this uncertainty into the system to compensate for the unmoral 169 00:12:42,130 --> 00:12:45,360 dynamics, we're not going to get a smooth loss, the estimate anymore. 170 00:12:45,370 --> 00:12:50,560 So we're going to get these random jumps in our velocity as it tries to update it using the position. 171 00:12:51,610 --> 00:12:53,790 So this is one problem with tuning the filter. 172 00:12:54,280 --> 00:12:58,300 We want to make the key matrix as small as possible so all our states are smooth. 173 00:12:58,660 --> 00:13:04,510 But in this situation here, when we have a unmoral dynamics or extra uncertainty inside the process 174 00:13:04,510 --> 00:13:06,910 model, we need to increase the key matrix. 175 00:13:07,210 --> 00:13:11,260 But we always want to do this in using the least amount as possible. 176 00:13:12,800 --> 00:13:18,740 If we have another look at a different example, so let's say instead of having a constant non-zero 177 00:13:18,740 --> 00:13:22,820 acceleration, we actually have a object traveling in a circle. 178 00:13:26,000 --> 00:13:32,300 And this time, let's go back to our zero key metrics, so this is definitely not what the system was 179 00:13:32,300 --> 00:13:33,080 designed for. 180 00:13:34,070 --> 00:13:40,970 So I can see as we travel around in a circle, our filter is going to start to get more and more delayed 181 00:13:40,970 --> 00:13:42,050 and more and more error. 182 00:13:43,980 --> 00:13:50,340 And we can say this in the innovation is no longer here, I mean, and our velocity measurements and 183 00:13:50,340 --> 00:13:52,220 position measurements are going to be very delayed. 184 00:13:52,410 --> 00:13:57,660 So you can see here our velocity estimates don't really follow the true velocity at all. 185 00:13:58,410 --> 00:14:03,270 So this is because in a circle, the object is constantly accelerating and changing velocity. 186 00:14:03,930 --> 00:14:07,900 This violates the underlying assumption that we're traveling at a constant velocity. 187 00:14:08,580 --> 00:14:14,490 So to handle this again, we can start adding some more process, more noise into it, using the acceleration. 188 00:14:15,830 --> 00:14:16,910 And if we run this. 189 00:14:19,100 --> 00:14:23,420 So let's see what happens this time, so this time you can see that we're tracking the object a bit 190 00:14:23,420 --> 00:14:27,320 better, but it's still having a bit of trouble keeping up with the object. 191 00:14:27,680 --> 00:14:33,800 So there's not enough uncertainty inside the system to let the system catch up with the moving object. 192 00:14:33,800 --> 00:14:36,830 The that's always constantly changing acceleration. 193 00:14:37,940 --> 00:14:41,090 But you can see our innovation is better this time. 194 00:14:41,090 --> 00:14:46,910 So we're closer to being see remain, but we still have a delayed response. 195 00:14:47,840 --> 00:14:53,780 So if we try this again, so it's adding a larger standard deviation for the acceleration. 196 00:14:53,780 --> 00:14:55,340 So let's actually say five. 197 00:14:57,070 --> 00:15:03,240 OK, so now I can see that the object is now you can see the computer is tracking the object a lot better, 198 00:15:03,550 --> 00:15:07,370 but you can still see that the object is jumping around a lot. 199 00:15:10,320 --> 00:15:12,750 This time, innovation is there, I mean, so that's good. 200 00:15:13,050 --> 00:15:16,680 So it seems to be fairly consistent in that point of view. 201 00:15:17,550 --> 00:15:21,520 And you can see our errors are mostly zero. 202 00:15:21,520 --> 00:15:27,090 I mean, but unfortunately, inside the system, to make it work, be able to track the object, we've 203 00:15:27,090 --> 00:15:29,070 had to increase the key metrics so much. 204 00:15:29,440 --> 00:15:34,410 And if we actually have a look at our position error, you can see the uncertainly that we think in 205 00:15:34,410 --> 00:15:36,960 the system is going to be plus or minus. 206 00:15:37,230 --> 00:15:40,380 Well, we said plus or minus about 20. 207 00:15:41,370 --> 00:15:42,780 So this is very large. 208 00:15:42,780 --> 00:15:49,470 And in fact, this is probably larger, almost as large as the real measurement standard deviation itself. 209 00:15:50,070 --> 00:15:55,140 So that's pretty much saying that this field is not adding anything into it, into the estimates. 210 00:15:55,380 --> 00:16:01,770 I've lost the sort of tracking the true state, but we have lots of noise inside it. 211 00:16:02,130 --> 00:16:05,940 So this is not a very good model to use in this situation. 212 00:16:06,990 --> 00:16:12,780 There'll be better models we can come up with, better for the common field to handle circular dynamics. 213 00:16:13,500 --> 00:16:20,010 So in this case, we've had to increase the key metrics a lot because the actual dynamics violate the 214 00:16:20,010 --> 00:16:23,580 assumptions we made inside the common filter for the process model dynamics. 215 00:16:24,630 --> 00:16:31,220 And because of this, we have these really large uncertainties around our state estimates. 216 00:16:31,230 --> 00:16:36,660 So we have a large position uncertainty and a lot of uncertainty, which means our signal estimates 217 00:16:36,660 --> 00:16:40,070 are going to be very noisy because it's going to be jumping around a lot and the same thing for our 218 00:16:40,080 --> 00:16:40,620 velocity. 219 00:16:41,280 --> 00:16:48,150 So in these situations, even though the common field is estimating the velocity and position, it's 220 00:16:48,150 --> 00:16:48,930 very noisy. 221 00:16:48,930 --> 00:16:55,590 And this is just because our system model is not a very good model for what we actually want to estimate. 222 00:16:57,090 --> 00:17:03,420 So it can be hard to come up with a simple coming for model that can track an object that does not add 223 00:17:03,420 --> 00:17:04,800 too much complexity into it. 224 00:17:05,160 --> 00:17:10,500 So for this example, we've had made a few assumptions, and one of those assumptions was that we had 225 00:17:10,500 --> 00:17:13,800 a zero acceleration and we had a constant velocity. 226 00:17:14,940 --> 00:17:18,870 So I can say in some situations in the first couple of simulations, it was working fine. 227 00:17:19,170 --> 00:17:25,330 But when we changed the actual motion type of the true system to something that's accelerating or something 228 00:17:25,330 --> 00:17:29,370 that's like circular is not a very good model to use in these cases. 229 00:17:30,000 --> 00:17:34,920 And the things that we needed to do to make it work, such as increasing the process of noise, means 230 00:17:35,130 --> 00:17:37,440 reducing the effectiveness of the filter. 231 00:17:38,860 --> 00:17:43,660 So these are some things to consider when we're designing a common filter and using it in practice, 232 00:17:44,620 --> 00:17:50,570 if the dynamic model for the true system is very different to what the computer thinks it is. 233 00:17:50,980 --> 00:17:53,970 Well, then it reduces the effectiveness of the common filter. 234 00:17:54,280 --> 00:17:58,990 And we have to start increasing the uncertainty inside the system to make the filter work. 235 00:17:59,380 --> 00:18:01,750 But that's going to degrade the overall performance.