1
00:00:04,080 --> 00:00:09,270
In this video, we're going to have a look at the initial conditions for the Linnear and Fulda, so

2
00:00:09,270 --> 00:00:13,500
we're going to see different ways of setting up this initial conditions and what the responses is going

3
00:00:13,500 --> 00:00:13,890
to be.

4
00:00:14,100 --> 00:00:19,290
And then we're going to look at you doing an exercise, setting up the initial conditions, using the

5
00:00:19,290 --> 00:00:24,540
first measurement, rather than assuming initial state for the simulation that we've been working on.

6
00:00:26,240 --> 00:00:31,310
So you should have seen from the simulations that the best performance obtained, which means the minimum

7
00:00:31,310 --> 00:00:36,620
remains squared error for the whole profile is when the initial conditions being the state and the covariance

8
00:00:36,620 --> 00:00:38,690
are as close to the truth as possible.

9
00:00:38,720 --> 00:00:42,550
And this makes sense from the simulations that you've already run.

10
00:00:42,560 --> 00:00:45,980
You should have seen that when we start the initial state below zero.

11
00:00:45,990 --> 00:00:52,850
So zero for the origin and zero for the velocity of the coming for us to perform fairly well on the

12
00:00:52,850 --> 00:00:53,730
first profile.

13
00:00:53,780 --> 00:00:59,810
So if we run the first profile, so you can see that the estimate starts at the origin and which is

14
00:00:59,810 --> 00:01:01,590
where the car starts as well.

15
00:01:01,610 --> 00:01:06,710
And you can see that the further quickly converges down to track the true position of the car.

16
00:01:07,520 --> 00:01:12,410
However, when we run with the second profile for the same configurations, the car does not start at

17
00:01:12,410 --> 00:01:13,300
the origin anymore.

18
00:01:13,310 --> 00:01:15,680
However, the filter still starts at the origin.

19
00:01:15,980 --> 00:01:20,550
And you can see it takes quite a while for the filter to try to catch up with the true position.

20
00:01:21,020 --> 00:01:23,660
This is because the field starts very inconsistently.

21
00:01:23,900 --> 00:01:28,050
The state estimates thinks is at zero, whereas the car is not at zero.

22
00:01:28,070 --> 00:01:33,050
So it takes a long time for the uncertainty to grow such large enough that the filter can shift its

23
00:01:33,050 --> 00:01:35,900
position and catch up with the true state of the system.

24
00:01:36,440 --> 00:01:41,300
And it can also say that once we run this profile, we end up with a very large maze with error for

25
00:01:41,300 --> 00:01:42,150
the whole profile.

26
00:01:42,470 --> 00:01:45,740
And this is because the filter did not start consistently.

27
00:01:45,950 --> 00:01:50,120
The initial state of the system was nowhere near the true state of the system.

28
00:01:52,040 --> 00:01:57,710
However, this raises a question, how do we actually know what the initial conditions are when the

29
00:01:57,710 --> 00:02:02,960
system is actually initialized from a non-zero value is an example of this is when we use profile to

30
00:02:02,960 --> 00:02:07,050
inside the simulator and the car starts at a different position rather than the origin.

31
00:02:07,490 --> 00:02:10,730
So how do you know what the initial conditions are going to be in events?

32
00:02:10,940 --> 00:02:15,530
And obviously in most conditions, we don't know what the initial conditions are going to be in events.

33
00:02:15,980 --> 00:02:17,780
So then we're going to have two options.

34
00:02:17,780 --> 00:02:23,450
We're either going to have to assume zero or some other concern like we've done already, or we're going

35
00:02:23,450 --> 00:02:28,010
to wait until we get the first measurement or we know enough about the current state to actually start

36
00:02:28,010 --> 00:02:29,440
the initialization process.

37
00:02:29,900 --> 00:02:32,150
And this is what we're going to do in this exercise.

38
00:02:33,990 --> 00:02:38,910
Now, luckily, as we've seen, the linnear common filter has nice property that it will not diverge

39
00:02:38,910 --> 00:02:40,540
even if the air is too large.

40
00:02:40,800 --> 00:02:46,620
So if we start with really large era because we have a non-zero initial condition, the update step

41
00:02:46,620 --> 00:02:49,680
will always be updating to minimize the estimation error.

42
00:02:49,890 --> 00:02:51,990
The filter is not going to start to diverge.

43
00:02:52,000 --> 00:02:54,360
The air is always going to be decreasing.

44
00:02:55,560 --> 00:03:01,230
So inside this exercise, we want to implement the filter state and covariance initialization on the

45
00:03:01,230 --> 00:03:02,070
first measurement.

46
00:03:02,550 --> 00:03:07,830
So to this first, we want to open up, become a filter file from the previous exercise in which we've

47
00:03:07,830 --> 00:03:10,450
had the prediction and the update steps completed.

48
00:03:11,280 --> 00:03:14,090
Now we want to run the simulation with profile number two.

49
00:03:14,100 --> 00:03:18,960
So this is a profile of the non-zero initial conditions and you should see some issues.

50
00:03:19,620 --> 00:03:24,630
When we actually run this simulation profile, you should see that the initial step and covariance are

51
00:03:24,630 --> 00:03:25,340
inconsistent.

52
00:03:25,770 --> 00:03:30,510
So the true state is not within the uncertainty of the filter estimate.

53
00:03:30,560 --> 00:03:36,360
So the red circle and if we look at the estimation error for the whole profile, you can see that it's

54
00:03:36,360 --> 00:03:38,520
quite large compared to the other profiles.

55
00:03:39,970 --> 00:03:45,460
So what you want to do is that we want to change the way the filter gets initialized instead of initializing

56
00:03:45,460 --> 00:03:51,220
on the first prediction, as we've done for the last two examples, using a constant state and covariance

57
00:03:51,220 --> 00:03:55,800
set by these parameters here, we are going to change this flag up here to be false.

58
00:03:56,590 --> 00:03:59,320
So the filter is no longer going to initialize on.

59
00:03:59,320 --> 00:04:03,340
The first prediction is going to wait until it gets initialized in the update step.

60
00:04:03,350 --> 00:04:08,940
So inside the handle GPS measurement function, there's going to be a bit of suction down here.

61
00:04:09,130 --> 00:04:14,350
And this is where we're going to write the initialization for the state vector and covariance matrix.

62
00:04:14,350 --> 00:04:17,060
And we're going to use the current GPS measurement to do this.

63
00:04:18,040 --> 00:04:23,260
So what we're doing in the past, we've been setting the initial state to bring all zeros, always said

64
00:04:23,260 --> 00:04:28,300
it to be five meters per second at 45 degrees because we actually knew and what the profile was going

65
00:04:28,300 --> 00:04:28,710
to be.

66
00:04:29,800 --> 00:04:36,250
And we've set the covariance for the initial state just to be a diagonal matrix with the first two terms

67
00:04:36,250 --> 00:04:41,590
being an initial position uncertainty and then the last two terms being initial velocity, uncertainty.

68
00:04:41,890 --> 00:04:43,600
And we've set these large enough.

69
00:04:43,990 --> 00:04:46,690
That encapsulates what the truth is going to be.

70
00:04:47,710 --> 00:04:51,130
Another way is to instead of initializing at time equals zero.

71
00:04:51,130 --> 00:04:56,770
We want to initialize on the first update and this first update is going to be the GPS position measurement.

72
00:04:57,250 --> 00:05:02,350
So basically we want to do is we want to set Vector X, not to be the GPS measurement.

73
00:05:02,360 --> 00:05:04,390
So this is the position in the X and Y.

74
00:05:04,630 --> 00:05:09,160
And then since we don't have any information about the velocity, all we're going to do the same thing.

75
00:05:09,160 --> 00:05:14,470
We're going to assume it starts off at zero, but we're going to set a initial velocity variance such

76
00:05:14,470 --> 00:05:19,150
that this is going to be large enough that it will actually encapsulate what the true velocity is.

77
00:05:19,630 --> 00:05:24,730
And then for the position states and the position uncertainty, we're just going to set the uncertainty

78
00:05:24,730 --> 00:05:27,250
to be the R matrix from the GPS measurement.

79
00:05:28,720 --> 00:05:32,660
So we want to change the variable in it on first prediction to be false.

80
00:05:33,040 --> 00:05:38,080
So this is going to not initialize the state vector and it's going to wait until we get the first measurement.

81
00:05:38,920 --> 00:05:44,620
Then inside the measurement function for the GPS, we want to initialize the field of state and covariance

82
00:05:44,620 --> 00:05:48,780
on the first update, and we want to do it according to these equations here.

83
00:05:48,790 --> 00:05:51,910
So we want to set the position states to be the GPS measurements.

84
00:05:52,270 --> 00:05:58,330
We will assume zero velocity and then we're going to set the position uncertainty inside the covariance

85
00:05:58,330 --> 00:06:04,120
to be the measurement, uncertainty and then aid and uncertainty for the velocity, which is going to

86
00:06:04,120 --> 00:06:10,630
be based on the constant again, which allows us to set a large variance for velocity and let the field

87
00:06:10,810 --> 00:06:12,520
converge on the true velocity.

88
00:06:13,360 --> 00:06:17,350
Once we've done that, then we can rerun the simulation with profile too, and we should be able to

89
00:06:17,350 --> 00:06:17,700
notice.

90
00:06:17,710 --> 00:06:23,680
And the response and overall the whole profile should have a smaller profile error compared to if we

91
00:06:23,680 --> 00:06:28,330
didn't actually use this delayed initialization and we just initialize it from a zero condition.