1
00:00:03,930 --> 00:00:08,490
In this video, we're going to look at the first exercise that you have to complete for the linnear

2
00:00:08,490 --> 00:00:08,790
common.

3
00:00:09,360 --> 00:00:13,090
And this is to implement the prediction step of the common filter.

4
00:00:14,310 --> 00:00:19,590
So this step is going to involve implementing the linnear common filter prediction equations and the

5
00:00:19,590 --> 00:00:21,670
2D vehicle process model.

6
00:00:22,710 --> 00:00:29,580
So the first step in the process is to open up the common filter zip file when we compile this and run

7
00:00:29,580 --> 00:00:35,760
the simulation as is using the default profile, which is profile one, we can see that the car starts

8
00:00:35,760 --> 00:00:36,450
at the origin.

9
00:00:36,450 --> 00:00:43,370
So P p y equals zero zero and it moves at a 45 degree angle at five meters per second.

10
00:00:43,380 --> 00:00:49,110
So if we break it down into the V, X and Voila components is going to give us this equation here.

11
00:00:50,100 --> 00:00:55,650
So this is going to be the first profile that we're looking at for implementing the step equations and

12
00:00:55,650 --> 00:00:57,000
the 2D process model.

13
00:01:00,670 --> 00:01:05,700
The second step in the process is to set up the initial state and the covariance for the Linnear come

14
00:01:05,710 --> 00:01:06,210
and fill it up.

15
00:01:06,760 --> 00:01:12,280
So we are going to initially assume that the position is zero zero and the initial velocity is five

16
00:01:12,280 --> 00:01:15,230
meters a second with the heading of 45 degrees.

17
00:01:15,730 --> 00:01:18,910
We also can assume that the zero uncertainty initially.

18
00:01:18,910 --> 00:01:21,460
So we're going to use a zero covariance matrix.

19
00:01:22,360 --> 00:01:29,290
So to do this inside our function prediction step inside the common filter between these two lines here,

20
00:01:29,770 --> 00:01:34,890
we want to start to enter the code over here for setting up the initial condition for the filter.

21
00:01:35,320 --> 00:01:41,020
So to do this, what we're going to do is we're going to set up the initial state vector X being zero

22
00:01:41,020 --> 00:01:43,130
zero three three point five three point five.

23
00:01:43,150 --> 00:01:51,460
So inside our step vector, we have the form of the state's X Y for position and then X Y for velocity.

24
00:01:52,330 --> 00:01:57,020
We also want to set up the initial covariance matrix, the P matrix, just to be an identity.

25
00:01:57,040 --> 00:02:00,010
So just to be a zero matrix as shown here.

26
00:02:02,870 --> 00:02:06,460
The next step in the exercise is to implement the prediction step.

27
00:02:06,500 --> 00:02:09,070
This is the process model for the linear common filter.

28
00:02:09,740 --> 00:02:14,930
So to do this, we're going to use the time step and define the process, model, matrix according to

29
00:02:14,930 --> 00:02:18,510
the to the legal process model that we've covered in previous video.

30
00:02:19,370 --> 00:02:25,130
We're also going to use the linear model equation to update the state, and we're going to do this inside

31
00:02:25,130 --> 00:02:27,190
the function block as shown here.

32
00:02:27,200 --> 00:02:32,330
So inside our prediction step function, there's a function down here that has this code down here.

33
00:02:32,330 --> 00:02:34,880
And we want to fill in the details over here.

34
00:02:37,280 --> 00:02:42,740
So the metrics that we want to implement is shown here, as we've seen before, where data is going

35
00:02:42,740 --> 00:02:48,440
to be the time stamp and then the linear model equation is just simply our F X.

36
00:02:48,710 --> 00:02:49,880
So F is a process.

37
00:02:49,880 --> 00:02:52,660
Model X is all previous state to calculate the new state.

38
00:02:53,030 --> 00:02:55,820
And you can see that we have no other input here.

39
00:02:55,820 --> 00:02:57,440
So we don't have a B you.

40
00:02:58,310 --> 00:03:01,430
All we're doing here is just assuming that we know the precise model.

41
00:03:01,460 --> 00:03:06,410
There's zero input and then we're going to use the noise of the system, the uncertainty of the system

42
00:03:06,410 --> 00:03:10,580
to update the acceleration and allow for a change in velocity.

43
00:03:13,220 --> 00:03:17,980
The fourth step is then going to be to implement the prediction step for the covariance.

44
00:03:18,320 --> 00:03:23,840
And so to do this, we're going to define the key metrics as a function of the variable acceleration,

45
00:03:23,840 --> 00:03:24,630
standard deviation.

46
00:03:24,650 --> 00:03:26,540
So this is a global concern that you can use.

47
00:03:27,290 --> 00:03:33,320
And then we're going to define the metrics as a function of the timestep, as we've covered in the previous

48
00:03:33,320 --> 00:03:33,730
video.

49
00:03:34,850 --> 00:03:38,360
And then we want to implement the covariance prediction set equations.

50
00:03:40,190 --> 00:03:44,990
We're going to assume that the process, model noise, acceleration, standard deviation is initially

51
00:03:44,990 --> 00:03:45,450
zero.

52
00:03:45,650 --> 00:03:51,260
So we're going to set constant value, this acceleration, standard deviation to be zero so that we

53
00:03:51,260 --> 00:03:56,750
can carry out the verification without any extra noise in the system so we can understand what exactly

54
00:03:56,750 --> 00:03:57,420
is going on.

55
00:03:58,940 --> 00:04:04,310
So again, inside our prediction step block of code down here, we want to implement the key metrics.

56
00:04:04,310 --> 00:04:10,220
So our key metrics is just going to be this metric is going to be a two by two metrics with the acceleration

57
00:04:10,220 --> 00:04:12,850
noise index and acceleration noise in the Y.

58
00:04:13,310 --> 00:04:17,780
And for this example, we can assume that both of these are just going to be equal to this constant

59
00:04:18,050 --> 00:04:19,910
acceleration, standard deviation.

60
00:04:21,170 --> 00:04:22,850
We're going to implement the EL metrics.

61
00:04:22,850 --> 00:04:27,310
And again, this is just going to be based on the TimeStep data so we can define this matrix here.

62
00:04:27,830 --> 00:04:33,620
And then lastly, we can implement the prediction set for the covariance, which is shown in this equation

63
00:04:33,620 --> 00:04:33,920
here.

64
00:04:36,410 --> 00:04:41,960
So once we've implemented the state prediction and covariance prediction, we can then go about verifying

65
00:04:41,960 --> 00:04:43,120
the work that we've actually done.

66
00:04:43,670 --> 00:04:49,160
So the next step is to run the simulation and we want to check that the diction follows the truth fairly

67
00:04:49,160 --> 00:04:49,720
closely.

68
00:04:50,120 --> 00:04:55,400
And this is going to work because we've initialized the filter with the truth, position and velocity.

69
00:04:55,820 --> 00:04:59,990
And then we can see what happens if we use profiles two to four.

70
00:05:00,350 --> 00:05:05,380
So profiler's no one is going to should track fairly well for two to four.

71
00:05:05,390 --> 00:05:08,000
We have changing conditions so we can actually see what happens.

72
00:05:09,810 --> 00:05:16,310
So on profile one, you can see that our estimate is state the red line, the Red Cross being the current

73
00:05:16,310 --> 00:05:19,100
state, is going to follow the car very nicely.

74
00:05:19,520 --> 00:05:24,560
We're going to get a very small position error and frostier because we've started the simulation from

75
00:05:24,560 --> 00:05:27,580
the true condition and we have zero input.

76
00:05:27,590 --> 00:05:33,230
The car is just traveling at a constant speed with no other noise inside the system.

77
00:05:33,440 --> 00:05:35,570
So it should track the truth very well.

78
00:05:37,180 --> 00:05:43,870
So this shows that the system will be working and the prediction equations are working well, the next

79
00:05:43,870 --> 00:05:45,370
step is to check the position.

80
00:05:45,370 --> 00:05:46,810
Covariance prediction is working.

81
00:05:46,960 --> 00:05:52,810
And to do this, we want to set the initial position for the X and Y covariance to be basically at,

82
00:05:52,810 --> 00:05:54,340
say, five meters squared.

83
00:05:54,940 --> 00:06:00,310
We also want to run the simulation and see that a three sigma position uncertainty stays at approximately

84
00:06:00,330 --> 00:06:02,470
plus or minus 15 meters.

85
00:06:02,770 --> 00:06:07,450
And we can compare this with the grid size because that grid size is at twenty five meters.

86
00:06:08,050 --> 00:06:13,090
So what this means is that when we run it, the simulation, we can see that the uncertainty circle,

87
00:06:13,090 --> 00:06:17,260
the covariance here is going to stay at a constant size and it's going to be staying centered on the

88
00:06:17,260 --> 00:06:17,760
vehicle.

89
00:06:19,660 --> 00:06:24,880
Once you check the position covariance, we can look at the velocity covariance and make sure that the

90
00:06:24,880 --> 00:06:28,060
prediction step is working correctly to do this.

91
00:06:28,180 --> 00:06:33,760
Again, we want to set the initial state to be all zero so that the estimate stays at the origin.

92
00:06:34,750 --> 00:06:39,790
Now we want to set the initial position covariance to be zero, but we want to set the initial velocity

93
00:06:39,790 --> 00:06:45,100
X and Y covariance to be five, divided by three squared meters per second.

94
00:06:45,700 --> 00:06:50,550
So what this is going to do is going to allow us when we run the simulation, we're going to see that

95
00:06:50,560 --> 00:06:56,590
the three sigma error for position grows at the same rate as the car position changes.

96
00:06:56,950 --> 00:06:58,390
So we don't run the simulation.

97
00:06:58,390 --> 00:07:04,600
You can see down here the origin, we run the simulation and the car drives off at forty five degrees

98
00:07:04,600 --> 00:07:05,490
at five minutes.

99
00:07:05,510 --> 00:07:11,830
And second, we should see that the three sigma uncertainty basically moves at or expands at the same

100
00:07:11,830 --> 00:07:13,120
rate as a car moves.

101
00:07:13,390 --> 00:07:18,580
So it should stay fairly close to the car seat of origin as it expands.

102
00:07:18,970 --> 00:07:21,520
And this is showing us that we can correctly model.

103
00:07:21,730 --> 00:07:26,890
I've lost the uncertainty inside the system and see that it actually tracks what the true system is

104
00:07:26,890 --> 00:07:30,840
doing as the position changes with a constant velocity.

105
00:07:32,500 --> 00:07:38,290
The last step in the verification process is to verify that the acceleration covariance prediction is

106
00:07:38,290 --> 00:07:38,750
working.

107
00:07:39,400 --> 00:07:42,430
So we want to set the initial state back to the original value.

108
00:07:43,120 --> 00:07:46,060
We also want to set the initial covariance or to be zero.

109
00:07:46,480 --> 00:07:50,390
But now we want to set the process model acceleration to constant.

110
00:07:50,410 --> 00:07:53,660
So the acceleration standard deviation to be point one.

111
00:07:55,300 --> 00:08:00,160
Now, when we run the simulation, we can see that three sigma velocity, the uncertainty grows over

112
00:08:00,160 --> 00:08:04,560
time and the position uncertainty is going to grow quite radically with time.

113
00:08:04,840 --> 00:08:08,850
So we can see as the car moves out, uncertainty is going to start small.

114
00:08:09,250 --> 00:08:15,340
But as we as time goes on, is going to grow even larger and it's going to start growing exponentially

115
00:08:15,340 --> 00:08:18,490
larger as we as time goes on.