1
00:00:03,620 --> 00:00:09,220
In this video, we're going to go over the C++ simulation and we'll look at how to use the simulation,

2
00:00:09,230 --> 00:00:13,400
so we'll look at what happens when after you build it and then you start running it, what different

3
00:00:13,400 --> 00:00:17,450
things you can do with it and how to use it to analyze your filters.

4
00:00:19,210 --> 00:00:23,410
So once you've compiled and start running the simulation, this is what you'll be able to see.

5
00:00:23,480 --> 00:00:28,210
And so this is the simulation environment that we're going to be using for this course here.

6
00:00:29,050 --> 00:00:32,560
So first, let's actually have a look at what's shown on the interface.

7
00:00:33,040 --> 00:00:38,560
Off the top here is the number is the profile number that we're currently running and information about

8
00:00:38,560 --> 00:00:39,200
this profile.

9
00:00:39,220 --> 00:00:45,460
So this is saying we're running motion profile one, which is a constant velocity with GPS, gyroscope

10
00:00:45,640 --> 00:00:47,350
and zero initial conditions.

11
00:00:47,890 --> 00:00:53,260
Next, we have the time and then the scaling factor in time so we can make the simulation run faster

12
00:00:53,260 --> 00:00:56,010
or slower using these keys down here.

13
00:00:56,020 --> 00:00:58,400
So we have our speed multiplier case.

14
00:00:58,720 --> 00:01:00,050
So these two brackets here.

15
00:01:00,370 --> 00:01:05,290
So as we run the simulation, we can make the simulation speed up or we can make the simulation slow

16
00:01:05,290 --> 00:01:07,450
down based on the settings up here.

17
00:01:07,450 --> 00:01:11,420
So changing the speed modifier, we also have a reset key.

18
00:01:11,440 --> 00:01:17,890
So when we first will restart the simulation again, we compress space to pause the simulation.

19
00:01:18,340 --> 00:01:24,610
And once this simulation gets to the end of simulation run, we can see the simulation automatically

20
00:01:24,610 --> 00:01:25,160
stops.

21
00:01:25,210 --> 00:01:27,350
So you can see it goes into the finished state here.

22
00:01:28,420 --> 00:01:34,960
We also have the ability, when we're running the simulation to zoom in so we can use a plus and minus

23
00:01:34,960 --> 00:01:35,270
case.

24
00:01:35,270 --> 00:01:39,190
The same in zoom out to see the whole simulation field.

25
00:01:41,910 --> 00:01:46,830
Now we can change what the simulation profile is running by the numbers case, I can see here we have

26
00:01:46,830 --> 00:01:48,160
the motion profile case.

27
00:01:48,570 --> 00:01:49,920
So this is the first profile.

28
00:01:49,950 --> 00:01:55,200
Number one, we have multiple different profiles that we can run each by pressing the different number

29
00:01:55,500 --> 00:01:57,590
and start up the different profile.

30
00:01:58,020 --> 00:02:02,220
So we have profiles one through to nine, including zero.

31
00:02:06,250 --> 00:02:10,780
On the screen at the top here and the top right, we have the vehicle state, so this is the current

32
00:02:10,780 --> 00:02:12,610
state of this vehicle sharing here.

33
00:02:12,800 --> 00:02:14,920
So we have the velocity it made as a second.

34
00:02:14,920 --> 00:02:17,170
We have the heading of the vehicle in degrees.

35
00:02:17,470 --> 00:02:19,020
We have the X and Y position.

36
00:02:19,660 --> 00:02:22,050
The next section here is the field of state.

37
00:02:22,390 --> 00:02:27,580
So this is what the current state estimates of the field are estimating, the vehicle state to be.

38
00:02:27,880 --> 00:02:32,650
And of course, ideally, we want the field of state to the estimated state to be as close as possible

39
00:02:32,650 --> 00:02:34,120
to the vehicle state up here.

40
00:02:34,900 --> 00:02:40,780
Now, as the simulation runs, we calculate the error between these two states here and we accumulated

41
00:02:40,780 --> 00:02:43,610
and this is the main square arrows down here.

42
00:02:44,020 --> 00:02:47,920
So as you can see here, since the field is not running because we actually haven't written it, it

43
00:02:47,920 --> 00:02:50,710
just starts off at a initial state set here.

44
00:02:51,190 --> 00:02:56,320
So as we drive and move around, you can see that this error for the simulation is actually increasing.

45
00:02:56,320 --> 00:03:01,210
So the X and Y position errors are increasing, the heading error is increasing as we're running.

46
00:03:01,210 --> 00:03:03,850
And of course, the errors are increasing as we're running.

47
00:03:05,020 --> 00:03:11,040
Moving back up to the top here, you can see these are the status of the sensors in the simulation.

48
00:03:11,050 --> 00:03:13,320
So we have GPS sensor running at one hurt.

49
00:03:13,600 --> 00:03:18,460
We have a leader sensor running at 10 hertz and we have the gyroscope sensor running at 10 hertz.

50
00:03:18,850 --> 00:03:21,430
So we can see if we change it to profile one.

51
00:03:21,790 --> 00:03:24,370
You can see that we don't have the law doesn't say so.

52
00:03:24,370 --> 00:03:25,420
It's in the off state.

53
00:03:25,750 --> 00:03:30,250
However, if we change your profile for you can see the water is running.

54
00:03:31,600 --> 00:03:34,350
So let's have a look at what's actually shown on the screen here.

55
00:03:34,360 --> 00:03:36,900
So obviously we have the current vehicle.

56
00:03:36,910 --> 00:03:40,510
So this is the vehicle is this symbol here.

57
00:03:41,050 --> 00:03:47,440
And the green cross is where the current vehicle location is, the history of the vehicle shown as is

58
00:03:47,440 --> 00:03:48,490
a dark green line.

59
00:03:48,490 --> 00:03:53,850
So as a vehicle moves around, it drops this line here to show you the history of the vehicle position.

60
00:03:54,880 --> 00:03:57,850
Now, these white crosses here are the GPS measurements.

61
00:03:57,850 --> 00:04:03,490
So as a vehicle run, you can say GPS measurements get generated and you can see the errors on the sensor.

62
00:04:03,490 --> 00:04:04,310
There is some noise.

63
00:04:04,310 --> 00:04:08,980
So they're generating these positions, but they're not exactly at the true vehicle position.

64
00:04:09,170 --> 00:04:11,360
So these are the typical sensor measurements.

65
00:04:11,410 --> 00:04:14,590
These are what we want to filter out and try to estimate this position of the state.

66
00:04:15,670 --> 00:04:21,160
These triangles over here, these yellow triangles over here are the Leida landmarks, which we're going

67
00:04:21,160 --> 00:04:22,110
to try to estimate.

68
00:04:22,120 --> 00:04:27,970
So if we run with a profile that uses Leida, such as profile number five, you can see that the yellow

69
00:04:27,970 --> 00:04:31,060
lines here are the light of measurements to each of the landmarks.

70
00:04:36,590 --> 00:04:42,290
And if we look closely when we first run the simulation, this Red Cross here is the current estimated

71
00:04:42,290 --> 00:04:43,070
state of the field.

72
00:04:43,190 --> 00:04:47,690
So since the field is not running, it just stays at the origin or where we've initialized it.

73
00:04:48,140 --> 00:04:52,850
So now let's switch over to one of the actual completed solutions.

74
00:04:54,320 --> 00:04:57,420
So here's a simulation running with a completely filled up.

75
00:04:57,920 --> 00:05:01,100
So now we can see that the common field of state is moving.

76
00:05:01,100 --> 00:05:04,090
So we can see that the Red Cross is moving with time.

77
00:05:04,100 --> 00:05:07,630
We can see the dark red line is the history of the filter.

78
00:05:08,120 --> 00:05:11,210
And then this bright red circle here are Ellipse.

79
00:05:11,210 --> 00:05:13,050
Here is the uncertainty bound.

80
00:05:13,090 --> 00:05:16,430
So this is how how much uncertainty there is inside the estimates.

81
00:05:16,430 --> 00:05:20,120
And ideally, we want our green cross to stay with inside the circle.

82
00:05:20,750 --> 00:05:25,430
So I can say now when we have the filter working, you can see the field of state is getting estimated

83
00:05:25,700 --> 00:05:30,190
and you can see that our main square errors are staying fairly small.

84
00:05:30,590 --> 00:05:35,210
So you can see this is what a working example or the filter should look like.

85
00:05:37,710 --> 00:05:39,870
We can also test it out when we have the lighter.

86
00:05:39,900 --> 00:05:42,050
So this is a working light diversion as well.

87
00:05:42,060 --> 00:05:47,670
So if using in the light of measurements, so now we can see we get some very small position and velocity

88
00:05:47,670 --> 00:05:50,810
errors and you can see that our uncertainty is very small.

89
00:05:50,820 --> 00:05:54,000
So it tracks the true vehicle state very well.

90
00:05:58,180 --> 00:06:03,910
So this has just been a quick overview of the simulation environment and how we can actually use it

91
00:06:03,910 --> 00:06:06,700
to analyze the different filters that you come up with.

92
00:06:07,910 --> 00:06:12,590
So you would have seen that the simulation supports a number of different profiles that we can use to

93
00:06:12,590 --> 00:06:18,770
test the filter out on the first full profiles, build up the basic maneuvers in different situations,

94
00:06:18,770 --> 00:06:23,980
while the next four profiles are just copies of the original first four.

95
00:06:23,990 --> 00:06:27,470
But we're adding the law d'ascenzo into them as well so we can test it out.

96
00:06:28,010 --> 00:06:33,890
The final two profiles are for the capstone project, so the first profile just looks at a constant

97
00:06:33,890 --> 00:06:40,850
speed and heading manoeuver with the zero initial conditions and using the GPS and jarra measurements.

98
00:06:41,180 --> 00:06:43,160
The second profile is a bit more interesting.

99
00:06:43,160 --> 00:06:48,980
We start at a non-zero initial condition, but again, we just have a constant speed and heading.

100
00:06:49,220 --> 00:06:53,480
And again, this is with the GPS and gyro profiles.

101
00:06:53,510 --> 00:06:55,490
Three and four get a bit more challenging.

102
00:06:55,820 --> 00:07:00,140
Even though we start off at zero initial conditions, we're actually going to have one case.

103
00:07:00,140 --> 00:07:05,810
It has changing things while it's driving and the other one has a changing speed and changing headings

104
00:07:05,810 --> 00:07:06,650
while it's driving.

105
00:07:07,160 --> 00:07:12,860
So the first for the basic scenario is that we can test out all the different types of filters on because

106
00:07:13,100 --> 00:07:15,800
it can be represented as a linear or nonlinear problem.

107
00:07:16,730 --> 00:07:22,490
The next four profiles, again, are the same profiles, but we turn on the light on measurements so

108
00:07:22,490 --> 00:07:26,230
the Leida measurements can only be processed on a nonlinear filter.

109
00:07:26,240 --> 00:07:30,230
So they are only relevant for the extended computer and unscented common filter.

110
00:07:32,090 --> 00:07:37,700
Profiles nine and zero are going to be the two profiles that we use for the capstone project, so they're

111
00:07:37,700 --> 00:07:40,610
basically a big combination of all the different conditions.

112
00:07:40,610 --> 00:07:42,440
They're going to have nonzero initial conditions.

113
00:07:42,440 --> 00:07:44,740
They can have changing speeds, changing headings.

114
00:07:45,410 --> 00:07:47,270
It's going to use all senses.

115
00:07:47,630 --> 00:07:50,180
But we're also going to add different errors onto the sensors.

116
00:07:50,180 --> 00:07:52,370
So the GPS measurements might be faulty.

117
00:07:52,640 --> 00:07:55,040
We might get of measurements out of 40.

118
00:07:55,760 --> 00:07:58,120
We're going to have biases on the gyroscope.

119
00:07:59,000 --> 00:08:03,020
So there's going to be a lot more things that you have to compensate for inside the filter, which is

120
00:08:03,020 --> 00:08:08,060
more like typical Real-Life scenario situations, profile number zero.

121
00:08:08,270 --> 00:08:12,110
It's just again, a copy of profile nine, except now there's a major change.

122
00:08:12,350 --> 00:08:15,080
We don't have any Leida data association.

123
00:08:15,500 --> 00:08:21,710
So in all the profiles before that use Leida, we have a bit of a cheat, though I don't know is what

124
00:08:21,710 --> 00:08:24,380
Landmark is looking at in real life.

125
00:08:24,380 --> 00:08:26,060
He might not actually know this information.

126
00:08:26,060 --> 00:08:31,640
So you'll have to do some sort of optimization procedure to work out what Landmark is actually saying.

127
00:08:32,000 --> 00:08:34,340
So this is the data association problem.

128
00:08:34,340 --> 00:08:38,600
So Profile Zero has no true data association.

129
00:08:38,600 --> 00:08:44,690
So you have to write an optimization procedure or some procedure yourself to work out what landmarked

130
00:08:44,690 --> 00:08:48,620
Lawder is actually looking at before we actually do the filtering into the filter.

131
00:08:49,400 --> 00:08:53,330
So last two profiles of all the capstone project and they're the most challenging.

132
00:08:53,330 --> 00:08:56,090
You're going to have to think about the different solutions to the problem.

133
00:08:56,390 --> 00:08:58,780
And it's a bit of a challenge to see what you can come up with.