1
00:00:03,820 --> 00:00:08,480
Welcome to the second exercise for the extended common shoulder in this exercise.

2
00:00:08,500 --> 00:00:14,110
We want to implement the extended coming update equations and the non-linear light on measurement model.

3
00:00:15,650 --> 00:00:21,560
The first step in this process is to open the last common file from the previous exercise, which had

4
00:00:21,560 --> 00:00:26,990
the prediction step completed, and we want to review the code that has already been written for the

5
00:00:26,990 --> 00:00:29,300
update step as part of this exercise.

6
00:00:30,410 --> 00:00:35,120
So looking at the file, you can see that we have this handle light measurements function.

7
00:00:35,150 --> 00:00:39,980
And this function is called every time we get a series of the measurements, it automatically takes

8
00:00:39,980 --> 00:00:41,990
in the data set of the line of measurement.

9
00:00:42,040 --> 00:00:47,060
So a vector of measurements that are made at that time step and it takes in the landmark map.

10
00:00:47,270 --> 00:00:52,820
And all of this function does is that recursively calls a lot of measurement for each of the measurements

11
00:00:52,820 --> 00:00:53,930
inside the data set.

12
00:00:54,650 --> 00:00:58,570
So this function here is this larger function that's expanded out of here.

13
00:00:58,850 --> 00:01:04,370
And this is where we're going to be modifying the code for the Lider measurement model inside the external

14
00:01:04,640 --> 00:01:05,060
filter.

15
00:01:06,920 --> 00:01:11,780
It is important to know that a lot of measurements contains the measurement from the light, a sensor

16
00:01:11,780 --> 00:01:16,250
for that landmark, so it contains a range and relative bearing for that measurement.

17
00:01:16,530 --> 00:01:19,880
It also contains the associated landmarked.

18
00:01:20,570 --> 00:01:23,830
So this set of functions here, it gets the measurement ID.

19
00:01:23,840 --> 00:01:29,120
So this is landmark ID that the measurement is made against and then it looks at this ID inside the

20
00:01:29,120 --> 00:01:30,820
map and that returns.

21
00:01:30,830 --> 00:01:36,830
Bakan data for the associated bacon of this relies on the fact that the measurement ID does not equal

22
00:01:36,830 --> 00:01:39,740
negative one and the Beacon ID does not equal the negative one.

23
00:01:40,310 --> 00:01:45,710
So if these two conditions are met, then we have matched up the light and measurement to a beacon that

24
00:01:45,710 --> 00:01:47,000
we know inside the map.

25
00:01:47,240 --> 00:01:53,390
Once we've done that, then we can access the Bacons X and Y locations using the map back into X and

26
00:01:53,390 --> 00:01:54,710
map Y.

27
00:01:57,960 --> 00:02:01,120
So the light of measurement model that we want to implement is shown here.

28
00:02:01,140 --> 00:02:02,650
So this is a set of equations.

29
00:02:02,670 --> 00:02:09,540
Now, the X and Y are the landmark locations and they can be read from the map beacons or X and that

30
00:02:09,540 --> 00:02:11,480
bacon Y that we've already just said.

31
00:02:11,970 --> 00:02:18,600
And then the state vectors for the position of the X position Y as well as the heading of the vehicle

32
00:02:18,930 --> 00:02:21,110
can all be extracted from the state vector.

33
00:02:22,500 --> 00:02:26,690
And it's also important to remember that the state vector follows this convention here.

34
00:02:26,880 --> 00:02:32,080
So the state that Texas or the position in the X Y and then the heading and then the velocity.

35
00:02:32,340 --> 00:02:36,030
So this is a form of the state variables inside the state vector.

36
00:02:37,870 --> 00:02:42,910
The next step is to implement the innovation calculations, so we want to calculate the measurement

37
00:02:42,910 --> 00:02:48,130
innovation and remember any of the angle measurements inside, the innovation has to be normalized with

38
00:02:48,130 --> 00:02:51,020
the angle function like we did in the prediction step.

39
00:02:51,730 --> 00:02:54,610
We also want to calculate the measurement model Jacobean.

40
00:02:54,610 --> 00:02:58,740
So The Matrix, so we can do that from this set of Jacobean matrices here.

41
00:02:59,620 --> 00:03:04,100
And then we want to calculate the innovation covariance matrix, the matrix.

42
00:03:04,120 --> 00:03:07,090
And again, that is done through this equation here.

43
00:03:07,930 --> 00:03:12,760
And for this equation, we will assume that the uncertainty in the measurements is given by a range

44
00:03:12,760 --> 00:03:18,040
variance and a relative bearing variance which can be accessed by the constants inside the top of the

45
00:03:18,040 --> 00:03:18,430
file.

46
00:03:18,640 --> 00:03:23,890
So they lied our range standard deviation and the light theta standard deviation.

47
00:03:26,630 --> 00:03:32,720
Then the next step is to implement the actual EAF update step equations themselves, so we want to calculate

48
00:03:32,720 --> 00:03:37,120
the come again and using the common gane and innovation, update the state vector.

49
00:03:37,250 --> 00:03:40,270
And we also want to update the covariance matrix as well.

50
00:03:42,140 --> 00:03:47,000
Once we've done that, we want to run the simulation and we want to run the simulation for all the different

51
00:03:47,000 --> 00:03:47,810
profiles.

52
00:03:47,810 --> 00:03:49,480
So propose one to eight.

53
00:03:49,970 --> 00:03:54,170
Now, the profiles, five, six, seven, eight are pretty much just profiles.

54
00:03:54,170 --> 00:03:55,390
One, two, three, four.

55
00:03:55,730 --> 00:04:01,730
But we also include the Leida measurements inside the simulation so we can see how, including the light

56
00:04:01,730 --> 00:04:06,040
and measurements inside the simulation improves the performance of the filter.

57
00:04:08,120 --> 00:04:14,330
And then the last step is to compare the simulation results between the linear computer and the extended

58
00:04:14,340 --> 00:04:16,890
computer with and without the Leida.

59
00:04:17,150 --> 00:04:22,930
So that's comparing profiles, wonderful and profiles five to eight for the extended common theorem.

60
00:04:22,940 --> 00:04:26,360
And then the profile is one to four for the linear common filter.