1 00:00:04,870 --> 00:00:09,700 So now let's have a more detailed look in how we initialize the filter and what the effects of that 2 00:00:09,700 --> 00:00:12,650 initialization has on the actual response of the filter. 3 00:00:13,300 --> 00:00:15,760 So let's run the code as is at the moment. 4 00:00:16,720 --> 00:00:21,760 So when we do this, we can see that outraced state starts up over here and shoots off in this direction. 5 00:00:22,390 --> 00:00:25,830 Now the common filter starts again at our origin zero zero. 6 00:00:25,840 --> 00:00:28,930 So up over here and it tracks down this way here. 7 00:00:28,930 --> 00:00:33,610 But you can see there's still an error between the common filter estimate and the true state estimate. 8 00:00:34,120 --> 00:00:40,470 So if we have a look at the error positions, you can see that our estimated position and our trade 9 00:00:40,510 --> 00:00:41,600 position is offset. 10 00:00:41,620 --> 00:00:43,320 So there's a bias in here. 11 00:00:43,330 --> 00:00:48,420 So you can see that we actually have a bias position error inside our common filter response. 12 00:00:48,880 --> 00:00:53,910 And this is because the common filter still starts at our origin zero zero. 13 00:00:54,220 --> 00:00:56,790 But the true state doesn't start at our origin. 14 00:00:56,800 --> 00:01:03,340 So I can see here the true state starts at a non-zero value of about one hundred here and another hundred 15 00:01:03,340 --> 00:01:07,330 here, whereas our Common Filter estimate starts at zero zero. 16 00:01:07,840 --> 00:01:11,950 So this bias in the initial state causes this buys over long term. 17 00:01:12,430 --> 00:01:19,480 This is because the initial position of the filter estimate is outside the uncertainty bubble of the 18 00:01:19,480 --> 00:01:20,490 initial covariance. 19 00:01:20,500 --> 00:01:22,750 So the filter is inconsistent at this stage. 20 00:01:22,840 --> 00:01:29,290 So you can say here he is, a three sigma uncertainty bounds for the position in the X and this is one 21 00:01:29,290 --> 00:01:29,880 for the Y. 22 00:01:29,890 --> 00:01:33,630 And you can see in both cases the error is outside these bounds. 23 00:01:33,640 --> 00:01:35,200 So the filter is inconsistent. 24 00:01:36,160 --> 00:01:41,530 So the filter does not have enough uncertainty inside the system to let the system actually catch up 25 00:01:41,530 --> 00:01:44,210 with the true state and converge to the true state. 26 00:01:44,920 --> 00:01:50,320 So how we set the initial conditions is quite important in terms of the overall field of performance. 27 00:01:50,680 --> 00:01:54,750 If we had to sort of like this, this tracking filter would not work properly. 28 00:01:54,760 --> 00:01:56,320 It would give us the wrong result. 29 00:01:59,130 --> 00:02:05,460 So one easy fix of this is to set the initial standard deviation for the position to be a large value 30 00:02:05,790 --> 00:02:11,670 such that when we start at a non-zero origin location, it will be inside the uncertainty bubble. 31 00:02:11,700 --> 00:02:13,270 So the filter will be consistent. 32 00:02:13,290 --> 00:02:14,460 So let's try this again. 33 00:02:17,890 --> 00:02:23,440 Aha, so now you can see that the filter is tracking the story straight pretty well, where you can 34 00:02:23,440 --> 00:02:30,280 have a look at precision error and was the you can see now that the position starts at a non-zero value. 35 00:02:30,760 --> 00:02:35,070 It's still withinside uncertainty bubble here for the X and for the Y. 36 00:02:35,440 --> 00:02:39,190 So I can see that as we get measurements, it converges down quite nicely. 37 00:02:39,880 --> 00:02:44,230 And he can see our initial filter started out with zero zero position. 38 00:02:44,530 --> 00:02:49,810 But since the uncertainty of our initial estimate is so large, it allows the filter to quickly adapt 39 00:02:50,080 --> 00:02:55,890 when we get the first measurement and start tracking nicely along the actual motion of the object. 40 00:02:57,250 --> 00:03:02,410 So whenever we initialize a filter, we always want to make sure that the filter is as consistent as 41 00:03:02,410 --> 00:03:02,870 possible. 42 00:03:02,890 --> 00:03:09,400 We want to make sure that the true state, which we don't know, is withinside our uncertainty bubble 43 00:03:09,400 --> 00:03:11,260 around our estimated position. 44 00:03:11,560 --> 00:03:15,070 This makes a filter consistent and this will make the filter work correctly. 45 00:03:16,840 --> 00:03:22,420 So this step relied on us initializing a filter at TimeStep zero before we get any information about 46 00:03:22,420 --> 00:03:26,950 the system, but this might not be the best way to go about it, depending on the application. 47 00:03:27,650 --> 00:03:31,900 Devanny of this is that it allows the filter to start running from T was not. 48 00:03:32,260 --> 00:03:38,020 But the information that is providing at the initial time might not be relevant for what we want to 49 00:03:38,020 --> 00:03:43,090 use the information from or if it's controlling the heading of the vehicle and we stay at the heading 50 00:03:43,090 --> 00:03:43,630 of zero. 51 00:03:43,630 --> 00:03:47,830 But a vehicle is really traveling at 180 degrees from that. 52 00:03:48,130 --> 00:03:52,480 Then any information we get from this filter is going to be wrong into the filter converges to the true 53 00:03:52,480 --> 00:03:52,890 value. 54 00:03:53,380 --> 00:03:56,030 But during this time we can't use that information. 55 00:03:56,650 --> 00:04:02,530 So even though we start the filter, so even though we start the filter at tale's not, we can't use 56 00:04:02,530 --> 00:04:04,990 the information from the filter until the filter has converged. 57 00:04:06,590 --> 00:04:11,930 A different way to approach this problem is to use delayed initialization, so instead of initializing 58 00:04:11,930 --> 00:04:17,960 the filter at time zero, we initialize the filter on the first measurement and then we can actually, 59 00:04:17,960 --> 00:04:23,240 instead of setting our initial origin position to be zero zero, we can set up to the first position 60 00:04:23,240 --> 00:04:23,600 measurement. 61 00:04:23,600 --> 00:04:26,180 So we get the MPI from the measurement. 62 00:04:27,110 --> 00:04:28,170 So let's have a look at that. 63 00:04:28,190 --> 00:04:31,050 So if we comment out this part of the code here. 64 00:04:31,970 --> 00:04:38,900 So we now are using delayed initialization inside the step down here where we initialize our state from 65 00:04:38,900 --> 00:04:43,510 the first measurement and we initialize our covariance for the position from the matrix. 66 00:04:43,520 --> 00:04:45,850 So the sentence and measurement, noise, uncertainty. 67 00:04:46,160 --> 00:04:51,990 And then we set an initial velocity variance, just like we did before, to a constant value of ten 68 00:04:52,040 --> 00:04:52,940 standard deviation. 69 00:04:53,960 --> 00:04:55,040 So now if we run this. 70 00:04:58,420 --> 00:05:03,340 You can see the filter is working quite nicely, and in fact, you can see that we don't start off with 71 00:05:03,340 --> 00:05:03,920 a large error. 72 00:05:03,940 --> 00:05:10,000 We always start off with innovation being consistent with our measurement covariance. 73 00:05:11,200 --> 00:05:16,180 And if you look at our position errors, you can see that now we have a very sort of smaller position, 74 00:05:16,180 --> 00:05:20,530 error position errors inside the bounds and the initial bounds are a lot smaller. 75 00:05:20,530 --> 00:05:22,400 The uncertainty bounds are a lot smaller. 76 00:05:23,350 --> 00:05:27,760 So if we use this condition, even though we don't start at Tegus not, we still have a bit of a delay 77 00:05:27,760 --> 00:05:29,410 between here and the first measurement. 78 00:05:29,990 --> 00:05:34,630 As soon as we start running the filter, the information that the field is providing is pretty good. 79 00:05:34,660 --> 00:05:40,060 It allows us to use this information directly from this time forward, inside an additional process 80 00:05:40,060 --> 00:05:42,010 or whatever we're trying to control or estimate. 81 00:05:42,520 --> 00:05:47,950 Whereas if we start to filter from agency recognition, we'll have to wait for the filter to converge 82 00:05:47,950 --> 00:05:50,410 before we get good information about the positions. 83 00:05:51,320 --> 00:05:55,570 So hopefully this should have given you a bit of an overview of two different ways that we can start 84 00:05:55,570 --> 00:05:56,540 or initialize the filter. 85 00:05:56,740 --> 00:06:02,800 One is to assume a initial condition that works for all conditions and let the uncertainty bubble around 86 00:06:02,800 --> 00:06:05,410 that be large enough to actually encompass the truth. 87 00:06:05,770 --> 00:06:10,900 Or the second way is to use the first measurement information and use that to initialize the state using 88 00:06:10,900 --> 00:06:12,190 this known information.