1 00:00:03,760 --> 00:00:08,420 In this video, we're going to have a look at the results that you should have obtained from the extended 2 00:00:08,420 --> 00:00:13,540 karma filter update step exercise and we're going to have a look at the differences that you should 3 00:00:13,540 --> 00:00:18,850 have obtained between the GPS version, which is the previous exercise, and then this version of the 4 00:00:18,850 --> 00:00:23,050 filter, which includes the Lider measurements as well, and see how that affects the response. 5 00:00:23,620 --> 00:00:29,980 So if we run the simulation side by side, one that uses GPS and the other one that uses GPS and LIDAR, 6 00:00:30,190 --> 00:00:34,060 you should be able to see a dramatic difference in the actual quality of the state estimates. 7 00:00:34,450 --> 00:00:40,420 You can see that the uncertainty inside the GPS, only one is a lot larger and the estimates jump around 8 00:00:40,420 --> 00:00:43,550 a lot more, especially when we start doing turns in the system. 9 00:00:43,930 --> 00:00:49,540 The estimated trajectory is always a bit away from the truth, while if we look at the lighter one, 10 00:00:49,540 --> 00:00:52,290 you can see that it follows the true state a lot closer. 11 00:00:52,330 --> 00:00:58,540 You can see that we have a smaller covariance and it tracks the corners and the acceleration a lot more 12 00:00:58,540 --> 00:00:59,140 accurately. 13 00:00:59,410 --> 00:01:02,770 And this is actually shown inside the main square at aerostats. 14 00:01:02,770 --> 00:01:07,900 At the bottom, you can see that the one that uses the lighter is a lot smaller, dramatically smaller 15 00:01:07,900 --> 00:01:10,600 than the one that just uses the GPS information only. 16 00:01:13,820 --> 00:01:18,710 So we can see that the inclusion of the lighter moments allow the filter to better estimate the state 17 00:01:18,710 --> 00:01:19,350 of the vehicle. 18 00:01:19,790 --> 00:01:21,860 And this comes up from a number of reasons. 19 00:01:22,190 --> 00:01:25,430 First off is that we have information about the X and Y position. 20 00:01:25,580 --> 00:01:31,130 And now from the Leida, we also have heading information from the GPS measurements only. 21 00:01:31,130 --> 00:01:32,780 We can only get the X and Y position. 22 00:01:32,780 --> 00:01:34,490 We can't get any heading information. 23 00:01:34,890 --> 00:01:39,890 But since the law contains the relative bearing, this constrains what the heading of the vehicle is 24 00:01:39,890 --> 00:01:40,330 going to be. 25 00:01:40,340 --> 00:01:43,310 So it gives us more information about more of the states. 26 00:01:44,390 --> 00:01:46,790 Dilator measurements also has a faster measurement, right? 27 00:01:46,820 --> 00:01:50,710 We have a lot of measurements compared to the one Hertz GPS. 28 00:01:51,140 --> 00:01:55,590 This just means that for a given amount of time we're going to get a lot more information. 29 00:01:55,610 --> 00:02:00,020 And this just means that we better constrain the error in the system because we're updating it a lot 30 00:02:00,020 --> 00:02:02,180 more so the error can't drift as much. 31 00:02:03,110 --> 00:02:08,600 We also have more information inside each of the measurements because we can have model measurements 32 00:02:08,600 --> 00:02:09,700 per time update. 33 00:02:10,010 --> 00:02:12,490 We have one measurement per detector landmark. 34 00:02:12,500 --> 00:02:18,710 So if we detect five landmarks at a single time, we're going to get five Loda measurements compared 35 00:02:18,710 --> 00:02:22,430 to the GPS, which you only have one update per timestep. 36 00:02:23,840 --> 00:02:29,150 So overall, we can see that we have included a lot more information in the filter, we have included 37 00:02:29,150 --> 00:02:32,710 information about other states and we're using it in a lot quicker. 38 00:02:32,870 --> 00:02:38,240 And these two combination of things allow the state assessment of what the vehicle is doing to be a 39 00:02:38,240 --> 00:02:38,760 lot better. 40 00:02:38,990 --> 00:02:44,750 This is why I can dramatically see that the uncertainty surrounding the vehicle is so much smaller compared 41 00:02:44,750 --> 00:02:46,580 to when we are only using GPS. 42 00:02:47,090 --> 00:02:51,740 So in general, more data we are using the better solution we're going to get and the more information 43 00:02:51,740 --> 00:02:56,300 about all the states we get or as more states as possible, the better the field is going to be. 44 00:02:58,140 --> 00:03:03,930 Another interesting situation, which we should have observed is what happens when we use the non-zero 45 00:03:03,930 --> 00:03:06,250 initial conditions with and without Leida. 46 00:03:06,840 --> 00:03:10,500 Now we can see when to use a non-zero initial condition with the GPS. 47 00:03:10,500 --> 00:03:17,370 Only the estimated heading of the vehicle is incorrect is 180 degrees out of phase to where it really 48 00:03:17,370 --> 00:03:17,770 should be. 49 00:03:18,450 --> 00:03:23,790 This is because we initialize the filter with only the position in the X and Y location and we have 50 00:03:23,790 --> 00:03:25,380 no information about the heading. 51 00:03:25,620 --> 00:03:30,160 We let the heading evolve as it would normally evolve just using the filter. 52 00:03:30,660 --> 00:03:36,300 However, we can see when we incorporate the Lider information, the Lotter information has some heading 53 00:03:36,300 --> 00:03:38,010 information inside the measurement. 54 00:03:38,610 --> 00:03:41,790 It has the relative bearing and using this relative bearing. 55 00:03:41,790 --> 00:03:44,430 It helps constrain the heading to be the true heading. 56 00:03:45,180 --> 00:03:50,580 So you can say even though we start the filter from the same initial condition using the GPS, we get 57 00:03:50,580 --> 00:03:54,230 a lot more information, a lot quicker about the true heading of the vehicle. 58 00:03:54,570 --> 00:04:00,480 And this causes the filter estimate for the heading to flip around and actually be facing in the correct, 59 00:04:00,480 --> 00:04:01,530 correct direction. 60 00:04:01,860 --> 00:04:04,890 This is why the field of state and vehicles don't have a lot less error. 61 00:04:05,190 --> 00:04:09,090 So we can see if we look at the heading, error remains squared. 62 00:04:09,090 --> 00:04:13,860 Error is a lot smaller for the Lider measurements compared to the GPS. 63 00:04:14,280 --> 00:04:17,910 This is because we have more information about the actual heading of the vehicle. 64 00:04:18,150 --> 00:04:21,540 It helps to constrain the heading solution to a single solution. 65 00:04:21,900 --> 00:04:26,340 Compared to when we're using the GPS information only we sort of get two solutions. 66 00:04:26,340 --> 00:04:28,050 We can even be going forward or backwards. 67 00:04:28,050 --> 00:04:28,740 We don't know. 68 00:04:30,400 --> 00:04:36,340 The of measurement model used in the simulation assumed that we could uniquely identify and correlate 69 00:04:36,340 --> 00:04:41,210 a single range and bearing measurement to a single landmark in the known map. 70 00:04:41,680 --> 00:04:46,000 Now, somehow in real life, we're going to have to try to correlate that this measurement is that this 71 00:04:46,000 --> 00:04:51,790 landmark either here or this landmark over here, we have to have some identifying information to be 72 00:04:51,790 --> 00:04:52,500 able to do this. 73 00:04:53,170 --> 00:04:58,200 So in the simulation, we just use a unique I.D. and we pass that along for the measurement. 74 00:04:58,540 --> 00:05:01,220 But in real life, this is not always possible. 75 00:05:02,260 --> 00:05:06,880 So the problem becomes a lot harder if you don't know how to match up the measurement to a landmark 76 00:05:06,880 --> 00:05:07,410 in the map. 77 00:05:07,750 --> 00:05:11,440 And there are some potential solutions to this that can get applied in the real world. 78 00:05:11,800 --> 00:05:14,800 The first is the nearest neighbor association. 79 00:05:15,120 --> 00:05:20,410 So this is basically where you look at the landmarks surrounding where you think the vehicle is and 80 00:05:20,410 --> 00:05:24,760 try to match up the landmark that best correlates with the measurements being made. 81 00:05:25,240 --> 00:05:28,750 Now, this only works if you're fairly close to the truth. 82 00:05:29,140 --> 00:05:33,000 If you don't know where the vehicle is at all and you don't know what the heading is of, the vehicle 83 00:05:33,400 --> 00:05:38,530 is impossible to use nearest neighbor because there's no information to constrain the answer to what 84 00:05:38,530 --> 00:05:39,970 the truth would actually be. 85 00:05:40,300 --> 00:05:45,490 So this correlation method can only work if you have a good estimate, a way to start with. 86 00:05:45,700 --> 00:05:50,890 So it's good for if you're just trying to constrain the error as you move along, if you already have 87 00:05:50,890 --> 00:05:53,380 initialized to field off from a good location. 88 00:05:54,130 --> 00:05:59,020 Now, if you don't know where the vehicle is, initially is comes very difficult, you're going to have 89 00:05:59,020 --> 00:06:02,920 to use a global optimization approach or a particle filter approach. 90 00:06:03,250 --> 00:06:05,890 And this is sometimes called the kidnap robot problem. 91 00:06:06,280 --> 00:06:11,200 This is where if you take a robot and drop it in and known environment, but you don't know where you 92 00:06:11,200 --> 00:06:15,530 are in it, somehow it has to localize itself using the information around. 93 00:06:15,940 --> 00:06:21,790 Now, this can be potentially a very computationally expensive problem because you have to work out 94 00:06:21,790 --> 00:06:27,280 the data associations from each of the measurements for you can actually feed into the filter if you 95 00:06:27,290 --> 00:06:30,530 fuse measurements into the filter with the wrong data association. 96 00:06:30,850 --> 00:06:36,880 So if you take a range and very measurement to a landmark but a Miss MISCA correlate the landmark to 97 00:06:36,880 --> 00:06:41,170 another landmark is going to dramatically cause the theater to diverge. 98 00:06:41,590 --> 00:06:44,530 This is because the data that you see in the filter is incorrect. 99 00:06:44,530 --> 00:06:45,040 It's wrong. 100 00:06:45,260 --> 00:06:49,590 So it is only going to make the field of error grow larger later on. 101 00:06:49,600 --> 00:06:53,530 When we do the capstone project, we're going to actually look at implementing the nearest neighbor 102 00:06:53,530 --> 00:06:54,260 association. 103 00:06:54,550 --> 00:06:59,290 So if we don't know the data association, but we do know some idea where we are, such as from the 104 00:06:59,290 --> 00:07:05,350 GPS and we only want to constrain the drift in a solution, then they can use the nearest neighbor approach, 105 00:07:05,350 --> 00:07:09,190 which sort of looks at the best landmark correlation for where we currently are. 106 00:07:09,580 --> 00:07:14,410 And this assumption works fairly well, fully extend my thought out because one of the conditions of 107 00:07:14,410 --> 00:07:19,840 using the extended current filter is that we have some idea where we are, that the estimated position 108 00:07:19,840 --> 00:07:22,270 is not too far away from the truth location. 109 00:07:22,600 --> 00:07:24,520 Otherwise the filter would diverge anyway.