1
00:00:03,410 --> 00:00:08,480
In this video, we're going to go over a few hints to help you out with the challenges inside the capstone

2
00:00:08,480 --> 00:00:13,210
project, and the first one is to deal with the non-zero initial conditions.

3
00:00:13,640 --> 00:00:18,650
So the first sort of hints that I can give you is that we want to use the GPS position measurements

4
00:00:18,650 --> 00:00:26,000
to initialize the X and Y position states within, then want to use a GPS position and LEIDA measurements

5
00:00:26,000 --> 00:00:29,960
to landmarks when they're available to estimate the heading of the vehicle.

6
00:00:31,250 --> 00:00:35,990
Now, once we have these two pieces of information, we can make an initial estimate of the velocity

7
00:00:35,990 --> 00:00:41,780
and jarra bias so we can ever assume them to be zero or we could use some other process to estimate

8
00:00:41,780 --> 00:00:42,040
them.

9
00:00:42,500 --> 00:00:47,870
Once we have these three bits of information, we can combine the outputs of the three processes to

10
00:00:47,870 --> 00:00:52,400
generate the full initial condition for the filter and then start running the common filter.

11
00:00:54,500 --> 00:01:01,040
Now, to deal with the gyroscope sense of bias, what we can do is a gyroscope by state to the state

12
00:01:01,040 --> 00:01:02,560
vector and vehicle model.

13
00:01:02,810 --> 00:01:07,880
And what that looks like in terms of the mathematics is that we end up with a vehicle process model

14
00:01:07,880 --> 00:01:08,800
that looks like this.

15
00:01:09,290 --> 00:01:13,080
So the top section is just the normal process model.

16
00:01:13,130 --> 00:01:19,390
However, we've now added the JavaScript bias states to the filter, so we have a constant bias with

17
00:01:19,400 --> 00:01:20,060
zero derivative.

18
00:01:20,060 --> 00:01:26,180
So you can see that the bias term for the current timestep is just a call to the bottom of the previous

19
00:01:26,180 --> 00:01:26,740
timestep.

20
00:01:27,410 --> 00:01:33,050
However, inside the JavaScript model, when we are calculating the rate of the vehicle, we're also

21
00:01:33,050 --> 00:01:35,070
subtracting this bias term here.

22
00:01:35,300 --> 00:01:43,550
So this is how we get the bias term inside the vehicle dynamics next to deal with the faulty GPS measurements

23
00:01:43,550 --> 00:01:44,180
that we're getting.

24
00:01:44,600 --> 00:01:51,530
What we want to do is not fuse any faulty GPS measurements so we can first evaluate each measurement,

25
00:01:51,890 --> 00:01:57,290
such as using our innovation checking process, and then we only fuse the measurements that pass the

26
00:01:57,290 --> 00:01:57,800
test.

27
00:01:58,790 --> 00:02:04,070
And more mathematically, concretely, what we can do is we can calculate the normalized innovation

28
00:02:04,070 --> 00:02:04,580
squared.

29
00:02:04,680 --> 00:02:11,630
So if we have the GPS innovation as the music to here and we have the covariance matrix for the innovation,

30
00:02:11,780 --> 00:02:17,120
we can use this equation to calculate the Naess, the normalized innovation squared, and then we can

31
00:02:17,120 --> 00:02:20,710
test the normalize innovation squared against a preset threshold.

32
00:02:21,080 --> 00:02:25,040
And if this test pass, then we confuse the measurement into the filter.

33
00:02:25,490 --> 00:02:30,950
If the NS is larger than that threshold, then it must be a 40 cents A measurement so we can just ignore

34
00:02:30,950 --> 00:02:32,760
it and not fuse it into the filter.

35
00:02:34,070 --> 00:02:40,460
And then lastly, to deal with a lot of data association problem, what we can do is we can use the

36
00:02:40,460 --> 00:02:47,010
estimated position and maximum leida range to find all possible landmarks that could be in view.

37
00:02:47,420 --> 00:02:51,530
So we have the complete set of landmarks map in the database.

38
00:02:51,530 --> 00:02:57,770
And what we can do is we can use the estimated position from the filter and we can first then filter

39
00:02:57,770 --> 00:03:00,730
out all landmarks, which could not be possibly in view.

40
00:03:01,010 --> 00:03:05,230
And this allows us to get a smaller set of landmarks that could be in view.

41
00:03:05,360 --> 00:03:08,980
And then we can do data processing on top of these landmarks.

42
00:03:08,980 --> 00:03:14,840
The subset to work out what we're saying, we can use the current estimate, the position and heading

43
00:03:14,840 --> 00:03:19,080
to best match up the landmarks to find the data associations.

44
00:03:19,640 --> 00:03:25,460
So from this reduced set of landmarks, we can then search through all of them and see which ones match

45
00:03:25,460 --> 00:03:27,560
up with the best heading estimate.

46
00:03:29,400 --> 00:03:34,740
Now, if we don't know the current estimated headings, such as during initialization, then you might

47
00:03:34,740 --> 00:03:37,430
need to find the best heading that fits the data.

48
00:03:37,470 --> 00:03:44,490
So we might need another optimization process to find out what best fits the current situation for the

49
00:03:44,490 --> 00:03:45,280
given position.

50
00:03:46,290 --> 00:03:51,300
So hopefully all this gives you some hints about how you go about solving this capstone project and

51
00:03:51,300 --> 00:03:55,000
the challenges that arise from dealing with this situation.