1
00:00:03,770 --> 00:00:07,470
In this video, we're going to have a look at a like measurement model.

2
00:00:07,490 --> 00:00:12,440
Now, this is going to be slightly different to the linear model, such as the GPS model that we looked

3
00:00:12,440 --> 00:00:18,400
at inside the linear coming filter, because this lighter model is going to be a nonlinear model.

4
00:00:18,680 --> 00:00:23,840
So to implement this model, we're going to have to use a non-linear filter, such as the extent and

5
00:00:23,850 --> 00:00:24,230
filter.

6
00:00:25,130 --> 00:00:30,490
So to recap what we did for the linear, come and fill it up, we had a GPS measurement model.

7
00:00:30,500 --> 00:00:34,780
So basically the model produced a measurement of the position of the vehicle.

8
00:00:34,790 --> 00:00:37,570
So it produced IPEX in a P position.

9
00:00:38,180 --> 00:00:44,270
So for the state vector that described the vehicle in the linear frame, we had the position X, Y and

10
00:00:44,270 --> 00:00:46,340
then we had the velocity in the X and Y.

11
00:00:46,760 --> 00:00:51,950
The measurement vector basically gave us a measurement of the position of the vehicle in the X and Y,

12
00:00:52,340 --> 00:00:56,190
and it was a linear model such as it could be written in this form here.

13
00:00:56,960 --> 00:01:03,080
So expanding out the measurement vector, the position, the X and Y was just basically a modification

14
00:01:03,080 --> 00:01:08,730
of this election matrix C, which selects out the position X and the position in the Y from the state

15
00:01:08,750 --> 00:01:12,160
vector, and it also had additive noise added onto it.

16
00:01:12,590 --> 00:01:18,590
So this was for japes like SANZAR directly measuring the position of the vehicle, which just directly

17
00:01:18,590 --> 00:01:20,930
pulled out the position from the state vector.

18
00:01:23,090 --> 00:01:27,380
So now let's consider the non-linear 2D range and bearing measurement model.

19
00:01:27,500 --> 00:01:31,590
So this is going to be a type of measurement that gets returned by a law to model.

20
00:01:32,150 --> 00:01:34,540
So let's consider we have a vehicle here.

21
00:01:34,550 --> 00:01:37,310
So it's located at position X and Y.

22
00:01:37,670 --> 00:01:43,280
It has a heading of PSI and also has a velocity V in the direction of the heading.

23
00:01:44,070 --> 00:01:46,920
But this is not going to be important for this measurement model.

24
00:01:47,540 --> 00:01:51,280
So the measurements that it produces, it produces a range measurement.

25
00:01:51,290 --> 00:01:54,800
So the range between the vehicle location and the landmark location.

26
00:01:55,040 --> 00:01:56,910
So we can say that we have a landmark here.

27
00:01:57,830 --> 00:02:02,000
This is going to be the position of the landmark and the X and Y for Landmark I.

28
00:02:02,570 --> 00:02:07,210
So we're going to get a range I, which is going to be the range from the vehicle to Landmark.

29
00:02:07,550 --> 00:02:09,560
We're also going to get a relative bearing.

30
00:02:09,570 --> 00:02:14,450
So this is the bearing between the heading of the vehicle and the landmark direction.

31
00:02:14,870 --> 00:02:17,630
So this is going to be denoted as our Theta I.

32
00:02:18,980 --> 00:02:23,810
So the state picked up for this non-linear model is going to be, again, a velocity heading of the

33
00:02:23,810 --> 00:02:27,180
vehicle and a position in the X and Y, the measurement vector.

34
00:02:27,200 --> 00:02:32,450
So these are the measurements we're going to get from the Leida model are going to be the Range I and

35
00:02:32,450 --> 00:02:33,050
Theta I.

36
00:02:33,200 --> 00:02:38,730
This is going to be the range and relative bearing to the landmark inside the visible range.

37
00:02:39,230 --> 00:02:44,300
So we're going to get Modibo measurement vectors for each of the landmarks that the LIDAR model can

38
00:02:44,300 --> 00:02:44,660
see.

39
00:02:45,560 --> 00:02:49,560
And the measurement model is going to be represented as this nonlinear model here.

40
00:02:49,580 --> 00:02:53,990
So we're going to have the nonlinear measurement Model H, which is going to be a function of the current

41
00:02:53,990 --> 00:02:58,580
state, the current noise, and it's going to produce the current measurement vector.

42
00:02:59,210 --> 00:03:01,710
Now, this measurement model can be risen like this.

43
00:03:02,360 --> 00:03:04,940
So basically the first component is going to the range.

44
00:03:04,940 --> 00:03:09,500
So we can say here we get the difference between the landmark location and the vehicle location and

45
00:03:09,500 --> 00:03:15,140
the X squared, plus the landmark location and position of the vehicle in the Y squared.

46
00:03:15,440 --> 00:03:16,700
And then we get the square root of this.

47
00:03:17,210 --> 00:03:19,580
So this is just Pythagoras Theorem.

48
00:03:19,590 --> 00:03:24,080
It's just going to get the distance between this point and this point, the difference between the vehicle

49
00:03:24,080 --> 00:03:27,860
location and the landmark location for the landmark.

50
00:03:29,580 --> 00:03:31,720
Down here, we're going to get the relative bearings.

51
00:03:31,740 --> 00:03:36,790
This is basically going to take the angle between this point and the landmark location.

52
00:03:37,350 --> 00:03:39,690
This is going to give us this tunnel angle here.

53
00:03:39,720 --> 00:03:42,140
So this is the Eytan component down here.

54
00:03:42,690 --> 00:03:46,560
So that's going to give us the angle between the X axis and the landmark.

55
00:03:47,130 --> 00:03:49,070
However, we want the relative bearings.

56
00:03:49,080 --> 00:03:54,570
So as the heading of the vehicle changes, we also want to take into account how that changes its relative

57
00:03:54,570 --> 00:03:55,060
bearing.

58
00:03:55,380 --> 00:04:02,160
So if we have this tunnel angle here to the landmark and we subtract from this angle our heading, we're

59
00:04:02,160 --> 00:04:04,200
going to get left with this relative bearing.

60
00:04:04,590 --> 00:04:06,820
So this is going to be input into the system.

61
00:04:07,680 --> 00:04:12,410
So this is going to be our nonlinear measurement model for our little sensor.

62
00:04:12,780 --> 00:04:17,340
So the measurement vector is going to be a normal function of the state vector and we're just going

63
00:04:17,340 --> 00:04:18,670
to have some additive noise.

64
00:04:19,050 --> 00:04:20,790
So this is a lot of like sensor.

65
00:04:20,790 --> 00:04:23,870
So it measures the relative position in polar coordinates.

66
00:04:24,420 --> 00:04:30,090
And this is going to be important because using this model here, we get information about the position

67
00:04:30,090 --> 00:04:35,040
of the vehicle, but we also get information about the heading of the vehicle as well, because the

68
00:04:35,040 --> 00:04:36,990
heading becomes part of the measurement model.

69
00:04:37,770 --> 00:04:43,200
So this is going to be a more powerful way of estimating what the state of the vehicle is as compared

70
00:04:43,200 --> 00:04:45,920
to the GPS sensor, the GPS sensor.

71
00:04:45,930 --> 00:04:51,390
And he gives us information about the position of the vehicle while this sensor gives us information

72
00:04:51,390 --> 00:04:52,610
about the position of the vehicle.

73
00:04:52,620 --> 00:04:55,680
But it also gives us information about the heading of the vehicle.

74
00:04:56,100 --> 00:05:01,890
So we can see later on that this has the potential to dramatically increase the precision of the vehicle

75
00:05:01,890 --> 00:05:02,760
state estimates.