1
00:00:04,410 --> 00:00:09,510
In this video, we're going to cover the second exercise for the Linnear coming here, and that is to

2
00:00:09,510 --> 00:00:13,210
implement the update step for the filter and to do this.

3
00:00:13,230 --> 00:00:18,450
We're going to implement an update step for a GPS like measurement into our simulation environment.

4
00:00:19,460 --> 00:00:25,280
So we're going to begin the video by covering a simple GPS measurement model, so the measurement model

5
00:00:25,280 --> 00:00:26,540
is going to be a linear model.

6
00:00:26,570 --> 00:00:29,300
We're going to assume a form as shown here.

7
00:00:29,310 --> 00:00:35,520
So we're going to have a model matrix much the state vector X, and we're going to assume some additive

8
00:00:35,540 --> 00:00:37,310
random noise into the system.

9
00:00:38,060 --> 00:00:41,800
So the measurement model for the GPS is just going to be a very simple model.

10
00:00:41,870 --> 00:00:47,030
All we're going to do is we're going to assume that the matrix, the measurement matrix up here is just

11
00:00:47,030 --> 00:00:52,490
a simple selection matrix that simply selects out the current state that we're interested in.

12
00:00:52,880 --> 00:00:58,370
So this matrix here just flicks out the position in the X and the position in the Y states from the

13
00:00:58,370 --> 00:00:59,030
state vector.

14
00:00:59,390 --> 00:01:02,840
So our measurement vector is going to be an API.

15
00:01:03,540 --> 00:01:09,050
So we're going to assume that the sensor just gives us these two measurements, which is drek copies

16
00:01:09,050 --> 00:01:10,240
of the state over here.

17
00:01:10,700 --> 00:01:15,650
And then looking at noise the system, we're going to assume that the additive noise here this week

18
00:01:15,650 --> 00:01:21,740
is just going to be from a normal distribution with zero main and known covariance R and the covariance

19
00:01:21,740 --> 00:01:27,650
matrix R is just going to be a diagonal matrix with a standard deviation for the exposition and a standard

20
00:01:27,650 --> 00:01:29,720
deviation for the Y position measurements.

21
00:01:30,020 --> 00:01:35,100
And in our simple case, we're just going to assume that they have equal uncertainty on each of these.

22
00:01:35,100 --> 00:01:38,220
So we are going to replace it by Sigma GPS.

23
00:01:39,290 --> 00:01:44,030
So this is the simple measurement model that we're going to be implementing inside this exercise for

24
00:01:44,030 --> 00:01:45,140
the linear algebra.

25
00:01:45,920 --> 00:01:49,160
So now let's go over the actual exercise itself.

26
00:01:49,820 --> 00:01:55,070
So in this exercise, we want to implement the common for the upper equations and the GPS measurement

27
00:01:55,070 --> 00:01:57,130
model, which we just covered on the previous slide.

28
00:01:57,710 --> 00:02:02,720
The first step is to open up your last column for the fall from the previous exercise where we did the

29
00:02:02,720 --> 00:02:03,620
prediction step.

30
00:02:04,010 --> 00:02:07,640
The next step is to set up the Hajj and our matrixes.

31
00:02:07,700 --> 00:02:09,380
So we see the matrix here.

32
00:02:09,410 --> 00:02:16,220
So this is a sense of measurement matrix and the matrix, which is the uncertainty inside the GPS measurement.

33
00:02:16,700 --> 00:02:22,180
We're going to assume that the position measurement, uncertainty is this GPS positions, derivation.

34
00:02:22,190 --> 00:02:23,130
So this is a constant.

35
00:02:23,510 --> 00:02:29,040
And what we're going to do is just use this value here inside these matrix terms here.

36
00:02:29,060 --> 00:02:31,410
So this is how we're going to form the matrix.

37
00:02:32,060 --> 00:02:35,580
So we're going to do all this inside the handle GPS measurement function.

38
00:02:35,990 --> 00:02:40,640
So in between these two lines of code here, we want to set up the matrix and we want to set up The

39
00:02:40,880 --> 00:02:41,480
Matrix.

40
00:02:43,530 --> 00:02:48,750
The next step is that we want to implement the common thought update equation, so we're seeing these

41
00:02:48,750 --> 00:02:49,440
sort of equations.

42
00:02:49,440 --> 00:02:56,290
Here is the linear common step study of using the measurement model to calculate the innovation.

43
00:02:56,350 --> 00:03:02,220
So the difference between the GPS measurement, which is this GPS measurement up here and the measurement

44
00:03:02,220 --> 00:03:06,310
model for the GPS say this is the measurement model and then the current vector.

45
00:03:07,170 --> 00:03:10,900
This here is the equation for the measurement innovation.

46
00:03:10,950 --> 00:03:14,670
This is the metrics that we have defined for the GPS sensor measurement.

47
00:03:15,780 --> 00:03:18,860
We use that inside to calculate the common again.

48
00:03:18,900 --> 00:03:25,320
Then we use a common gain down in these two equations here to update the state estimates and to update

49
00:03:25,320 --> 00:03:26,700
the covariance estimates.

50
00:03:28,680 --> 00:03:33,710
Once we have implemented these equations, we can then run the simulation with the following configurations.

51
00:03:34,080 --> 00:03:39,510
So the first off, we want to set the initial state and covariance to be zero along with the process

52
00:03:39,510 --> 00:03:45,510
model noise, we should see that the common filter estimate does not change or is not updated at all.

53
00:03:45,910 --> 00:03:52,160
This is because we're saying that we trust the initial state completely and we are ignoring any measurements.

54
00:03:52,740 --> 00:03:55,710
Eyes only means that we have no uncertainty in the system.

55
00:03:55,950 --> 00:04:02,340
So if we set the initial state and covariance to be zero, then we are saying that there is no uncertainty

56
00:04:02,340 --> 00:04:03,260
inside the system.

57
00:04:03,270 --> 00:04:06,480
Therefore, any measurement we get is just going to be in order.

58
00:04:06,510 --> 00:04:09,870
So if this happens, then the field is working correctly for this test.

59
00:04:10,470 --> 00:04:16,320
Next, we want to change the initial velocity, standard deviation to be about 10, and we want to rerun

60
00:04:16,320 --> 00:04:18,300
the simulation and actually see the response.

61
00:04:18,330 --> 00:04:23,580
So now when we do that, we should be able to see the common filter is updating the state estimates

62
00:04:23,580 --> 00:04:29,970
with precision as we now have an increasing uncertainty inside the system and the GPS measurements are

63
00:04:29,970 --> 00:04:33,900
using are using themselves to constrain the uncertainty in the system.

64
00:04:35,220 --> 00:04:40,290
So once we see that is working correctly, then we want to test out using different values of the initial

65
00:04:40,290 --> 00:04:45,660
position, the initial Vosta uncertainties, and then the noise, uncertainty for the acceleration.

66
00:04:46,050 --> 00:04:52,530
And we want to do this when we run in the different profiles so we can try to run profiles one, two,

67
00:04:52,530 --> 00:04:59,010
three and four and actually see how these changing how changing these different parameters affect the

68
00:04:59,010 --> 00:05:02,670
uncertainty of the system and affect the overall estimation performance.

69
00:05:04,360 --> 00:05:08,620
So once we've done that and played around with the different tunings, we can have a bit more of a feeling

70
00:05:08,620 --> 00:05:13,480
of how the common Filaret is using this information to update the state estimates.

71
00:05:13,480 --> 00:05:19,150
And we have a bit more understanding of how the uncertainties inside the system affect the estimation

72
00:05:19,150 --> 00:05:20,340
performance of the system.