1
00:00:03,760 --> 00:00:08,500
As part of the linnear coming Philidor update exercise that we have just completed, we should have

2
00:00:08,500 --> 00:00:12,340
completed the GPS measurement update except for the I filled filter.

3
00:00:12,340 --> 00:00:13,910
So it should look something like this.

4
00:00:14,440 --> 00:00:19,780
So now in this video, we're going to go through a few concepts to explain what's happening and how

5
00:00:19,780 --> 00:00:24,280
we can work out whether it's working and a few concepts about common filtering in general.

6
00:00:24,820 --> 00:00:31,390
So the first exercise I want to do once we've completed the update step is to then set the initial state

7
00:00:31,390 --> 00:00:33,280
of the come and fill it out to be all zero.

8
00:00:33,280 --> 00:00:35,980
So zero for position, zero for velocity.

9
00:00:36,340 --> 00:00:42,590
And we are also going to set the initial covariance for the initial state here to be zero.

10
00:00:42,610 --> 00:00:46,840
So we're going to do this by setting the initial position and initial velocity, standard deviations

11
00:00:46,840 --> 00:00:50,640
dizzier and we're going to run it with zero process model noise.

12
00:00:51,280 --> 00:00:55,150
So when we run the simulation, we get a response that looks like this.

13
00:00:55,960 --> 00:00:59,860
So I can see that the estimated state, the Red Cross, is staying back at the Origin.

14
00:01:00,610 --> 00:01:06,800
The field of state is staying at zero and our error is increasing as the car is driving away.

15
00:01:06,820 --> 00:01:10,500
So this is what we sort of are expecting to happen.

16
00:01:10,810 --> 00:01:14,160
We've told the initial state to be zero, so it's staying at zero.

17
00:01:14,320 --> 00:01:16,310
And we said the initial covariance is zero.

18
00:01:16,330 --> 00:01:22,420
So the field, as far as the field is concerned, it has perfect information about the field of state,

19
00:01:22,420 --> 00:01:24,130
therefore, is not going to be updated.

20
00:01:25,000 --> 00:01:29,770
This is because there's no uncertainty inside the system for the GPS measurements to affect the system.

21
00:01:30,640 --> 00:01:34,720
So now let's actually change the initial uncertainty in the system.

22
00:01:34,750 --> 00:01:37,780
So now let's increase our key metrics.

23
00:01:38,170 --> 00:01:45,220
So let's say we have an initial velocity and position, uncertainty of at, say, 10, 10.

24
00:01:46,450 --> 00:01:48,430
So now let's rebuild this and run it.

25
00:01:49,390 --> 00:01:51,250
And now we can see what's happening.

26
00:01:51,310 --> 00:01:53,620
We can see that as the GPS measurements are made.

27
00:01:54,340 --> 00:02:00,580
It is updating the state of the computer and the computer is now tracking the position of the vehicle.

28
00:02:00,910 --> 00:02:06,400
So I can see now we have a small position, error and course the error as well.

29
00:02:06,580 --> 00:02:10,690
And we can see the red dot and the uncertainty is tracking where the true state is.

30
00:02:10,690 --> 00:02:16,450
So we can see that the true state, the green cross, is within the uncertainty bounds of the estimated

31
00:02:16,450 --> 00:02:16,870
state.

32
00:02:17,590 --> 00:02:22,000
The shape of the variance profile of time is pretty much controlled by two parameters.

33
00:02:22,000 --> 00:02:26,710
It is controlled by the key matrix and the Matrix along with the update.

34
00:02:26,710 --> 00:02:27,020
Right.

35
00:02:28,300 --> 00:02:33,190
So we've seen in the past from the prediction step, the key matrix controls the rate of increase of

36
00:02:33,190 --> 00:02:34,680
uncertainty inside the system.

37
00:02:34,690 --> 00:02:40,600
It shows you how quickly the uncertainty is going to increase with time while the Matrix is going to

38
00:02:40,600 --> 00:02:44,710
control how accurate the measurement is that we fuse into the system.

39
00:02:45,520 --> 00:02:50,710
So the larger the key matrix, the quicker the uncertainty grows between the updates, the smaller the

40
00:02:50,710 --> 00:02:54,400
R matrix is, the smaller is the updated covariance of the filter.

41
00:02:55,470 --> 00:03:01,770
So if we have a large key metrics and a large matrix or update, right, we might get a covariance profile

42
00:03:01,770 --> 00:03:02,570
that looks like this.

43
00:03:02,580 --> 00:03:07,860
So you can see overall the uncertainty in the system is increasing, even though we're getting some

44
00:03:07,860 --> 00:03:08,580
updates to it.

45
00:03:08,790 --> 00:03:12,990
The update doesn't provide enough information to constrain the uncertainty inside the system.

46
00:03:13,530 --> 00:03:16,380
So this is a bad example of what we want to be.

47
00:03:16,740 --> 00:03:21,750
This filter is actually helping us all that much, because the longer we run the filter, the more uncertainty

48
00:03:21,750 --> 00:03:22,800
in the system we're going to get.

49
00:03:22,830 --> 00:03:25,260
So it's not going to be helping us to estimate anything.

50
00:03:25,830 --> 00:03:31,200
The ideal case, what we want is when we have a small key matrix and a small R matrix or a fast update.

51
00:03:31,200 --> 00:03:31,500
Right.

52
00:03:31,770 --> 00:03:36,540
And we'll get the uncertainty inside the system looking like this where even though we increasing the

53
00:03:36,540 --> 00:03:41,940
uncertainty during the predictions, whenever we get an update, it brings the uncertainty down a greater

54
00:03:41,940 --> 00:03:44,140
amount than increased during the prediction step.

55
00:03:44,520 --> 00:03:49,830
So over time, we can see that the uncertainty is going to be converging and it's going to converge

56
00:03:49,830 --> 00:03:52,620
down to a steady state value.

57
00:03:52,920 --> 00:03:58,380
And this steady state value is going to be based on the ratio of the Q and Matrixes, as well as the

58
00:03:58,380 --> 00:03:59,040
update rate.

59
00:04:00,510 --> 00:04:05,640
An interesting thing to see from the op ed equations here is that the final covariance P is a function

60
00:04:05,640 --> 00:04:11,970
of our common game matrix K, which is a function of our covariance innovation, is now you can see

61
00:04:11,970 --> 00:04:16,290
during these questions here, none of these actually has to do with the current state of the system,

62
00:04:16,680 --> 00:04:20,070
the current state of the system X does not appear in any of these equations.

63
00:04:20,670 --> 00:04:26,420
So basically what that is saying is that our covariance with time is independent of the current state.

64
00:04:26,910 --> 00:04:33,240
The more updates we get, the smaller the P matrix is going to be in ideal conditions and since took

65
00:04:33,240 --> 00:04:38,700
a variance with time does not depend on the current state X, it is possible to predict how accurate

66
00:04:38,700 --> 00:04:41,220
outfielder estimates are going to be over the long term.

67
00:04:41,500 --> 00:04:47,400
So if we run the field for long enough, our covariance is going to converge down to a constant non-zero

68
00:04:47,400 --> 00:04:50,640
value and that value is going to be based on our Q and our matrix.

69
00:04:52,240 --> 00:04:56,980
So if we know the amount of process, more noise in the filter and we know how good our measurements

70
00:04:56,980 --> 00:05:01,410
are, we can work out overall how good our state estimates are going to be.

71
00:05:02,230 --> 00:05:07,420
So we can use this information in the engineering design process and work out an area budget so we can

72
00:05:07,420 --> 00:05:11,290
work out how good our sensors have to be to make the level performance we want to.

73
00:05:11,460 --> 00:05:15,580
So depending on our application, we might find out that we don't need a really high quality sensor.

74
00:05:15,580 --> 00:05:19,270
We might be able to get away with using a low quality sensor at lower cost.

75
00:05:19,510 --> 00:05:25,300
If the ratio of the metrics to key metrics are such that the estimated state covariance as we run the

76
00:05:25,300 --> 00:05:28,540
filter is adequate enough for the solution that we want.

77
00:05:30,170 --> 00:05:35,180
So you can say even though the film starts off by jumping around, once the uncertainty inside the system

78
00:05:35,180 --> 00:05:41,800
starts to converge, the uncertainty and the velocity uncertainty start to converge to the true value.

79
00:05:41,960 --> 00:05:43,850
And we can say we're getting a lot less jumps.

80
00:05:43,860 --> 00:05:48,500
So even though the GPS measurements might be all over the place, the common filler is doing what it

81
00:05:48,500 --> 00:05:53,240
should be doing and filtering it down to come up with a best estimate of the position and velocity.

82
00:05:53,240 --> 00:05:56,270
Therefore, we're going to get some smaller errors over here.

83
00:05:56,280 --> 00:06:01,340
So rather than the estimates, jumping between all the GPS measurements is going to use the information

84
00:06:01,340 --> 00:06:05,150
contained inside the process model and it's going to start to converge down towards.

85
00:06:05,220 --> 00:06:10,030
The truth is obviously this won't work when we have a more dynamic situation.

86
00:06:10,040 --> 00:06:14,990
So when the car turns by having zero process more noise, the field is going to become inconsistent

87
00:06:15,620 --> 00:06:19,370
because the estimated position is not close to the true position.

88
00:06:19,910 --> 00:06:22,130
So this is going to become trouble later on.

89
00:06:22,130 --> 00:06:24,210
We're actually trying to use the output of the filter.

90
00:06:25,100 --> 00:06:30,860
So in this case, let's set the acceleration standard deviation three point one, and let's rebuild

91
00:06:30,860 --> 00:06:32,040
this and run it.

92
00:06:32,270 --> 00:06:33,980
So let's run this profile three.

93
00:06:34,010 --> 00:06:39,170
So now I can say that as the prediction happens, the uncertainty inside the system is inflating.

94
00:06:39,170 --> 00:06:40,530
So the circle is getting larger.

95
00:06:40,850 --> 00:06:46,940
You can see that when we turn the vehicle, the estimated state catches up with the true state a lot

96
00:06:46,940 --> 00:06:50,320
quicker than it did when we had the zero process model uncertainty.

97
00:06:50,750 --> 00:06:54,650
But we do see overall that there's lots of jumping around with the estimate.

98
00:06:54,680 --> 00:06:58,760
This is because the amount of uncertainty in the system is growing quite rapidly.

99
00:06:59,510 --> 00:07:04,490
That is going to try to rely on the GPS measurements more than the smooth prediction model.

100
00:07:05,370 --> 00:07:10,460
And you can see on the corners it has still has a very large time to catch up.

101
00:07:10,490 --> 00:07:18,920
So if we increase the acceleration process model in a more, so now we get a larger and certainly growth

102
00:07:19,010 --> 00:07:19,670
with time.

103
00:07:19,940 --> 00:07:23,920
You can say that on the corners it takes a short amount of time for it to catch up.

104
00:07:24,350 --> 00:07:30,950
But we can say that the measurement, the but we can see that the common field and now the state estimates

105
00:07:30,950 --> 00:07:36,980
follow the white GPS measurement a lot more because it's going to cost them a lot more because it's

106
00:07:36,980 --> 00:07:39,370
got less uncertainty inside the prediction model.

107
00:07:39,890 --> 00:07:43,960
So I can say that we get in this quite ragged path as we're doing our predictions.

108
00:07:44,300 --> 00:07:49,490
So this is because it's jumping between measurement to measurement rather than using the prediction

109
00:07:49,490 --> 00:07:49,830
model.

110
00:07:50,420 --> 00:07:51,590
So this is a trade off.

111
00:07:51,830 --> 00:07:57,560
We can get it to update very quickly to the measurements by having a lot of process model noise and

112
00:07:57,560 --> 00:08:03,330
a small measurement model noise, but it counteracts having actually a nice, smooth estimate.

113
00:08:03,620 --> 00:08:09,890
So if we ran this profile, one you can say now we are jumping all over the place from side to side

114
00:08:09,890 --> 00:08:11,580
of the actual estimated position.

115
00:08:11,990 --> 00:08:17,570
So even though the field is staying consistent, the true position, the Green Cross is within this

116
00:08:17,570 --> 00:08:18,410
uncertainty circle.

117
00:08:18,630 --> 00:08:23,150
You can say that our estimates are jumping all over the place and you can say we're getting some reasonably

118
00:08:23,150 --> 00:08:25,570
large velocity and position errors.

119
00:08:26,240 --> 00:08:31,040
So there's going to be a trade off if we want a nice, smooth response that trust the prediction model

120
00:08:31,220 --> 00:08:34,600
more, we want a lower noise value for the acceleration.

121
00:08:34,940 --> 00:08:39,830
However, if we want to let the car turn different directions and the process models to try to keep

122
00:08:39,830 --> 00:08:45,410
up, we're going to need a larger acceleration, standard deviation to let the filter update quickly

123
00:08:45,410 --> 00:08:46,370
for the new response.

124
00:08:46,640 --> 00:08:51,530
This will mean that the estimates jump around a lot more, making them less suitable for some control

125
00:08:51,530 --> 00:08:52,160
purposes.

126
00:08:52,790 --> 00:08:58,010
Overall, this is a limitation of using a linear come and fill it up with the velocity characterized

127
00:08:58,010 --> 00:08:59,630
in the x y velocity.

128
00:08:59,990 --> 00:09:05,990
When we start looking at using the extended material for a nonlinear system, we can start estimating

129
00:09:05,990 --> 00:09:08,990
the heading of the vehicle and then the velocity in that heading.

130
00:09:09,470 --> 00:09:14,960
Then we can start to include gyroscope information to estimate how the vehicle heading changes so it

131
00:09:14,960 --> 00:09:20,870
can more rapidly change and update with the changing conditions without artificially increasing the

132
00:09:20,870 --> 00:09:23,900
acceleration, noise or uncertainty of the process model.