1
00:00:03,920 --> 00:00:08,990
Now, let's have a look at the second exercise for the uncynical Matura, so this is to implement the

2
00:00:08,990 --> 00:00:09,790
update step.

3
00:00:10,780 --> 00:00:17,000
So this exercise is to implement the unscented Somerfield update equations and the non-linear Lydda

4
00:00:17,000 --> 00:00:19,640
measurement model using the Ozanian transforms.

5
00:00:20,090 --> 00:00:26,270
So the first step in this process is to open up the last common felafel from the previous exercise in

6
00:00:26,270 --> 00:00:29,360
which you completed the prediction step again.

7
00:00:29,360 --> 00:00:35,810
You want to review the car that's already been written for the next step as part of the UK exercises.

8
00:00:37,650 --> 00:00:42,250
The second step for this exercise is now to implement the law, the measurement model function.

9
00:00:42,770 --> 00:00:46,020
So this is the same model that we implemented in the EAF.

10
00:00:46,020 --> 00:00:47,710
How are we going to make it slightly different?

11
00:00:47,730 --> 00:00:53,220
We're going to make it take in the augmented state so that we can augment the state for the unseen to

12
00:00:53,220 --> 00:00:53,840
transform.

13
00:00:54,510 --> 00:01:00,660
So inside our Leida measurement model, we're now going to take in a augmented state and we want to

14
00:01:00,660 --> 00:01:03,960
fill in the code here for the Lydda measurement model.

15
00:01:04,620 --> 00:01:07,190
So the argument state is just going to have this form here.

16
00:01:07,200 --> 00:01:12,150
So it's going to be the original state vector for the vehicle, but we're going to augment it with the

17
00:01:12,150 --> 00:01:12,560
noise.

18
00:01:12,590 --> 00:01:15,930
So this is the range noise and the relative bearing noise.

19
00:01:16,980 --> 00:01:21,540
The law, the measurement model is exactly the same as what we used inside the center fielder.

20
00:01:21,960 --> 00:01:27,660
However, we're now going to add the ability to add noise into the model based on the augmented state.

21
00:01:28,980 --> 00:01:32,260
So now let's have a look at the architecture of the system.

22
00:01:32,280 --> 00:01:37,890
So when the order produces measurements inside the simulation, it causes handle order measurements

23
00:01:37,890 --> 00:01:42,040
and it gets passed in a dataset of all the measurements made for that time step.

24
00:01:42,660 --> 00:01:48,930
This code here basically just loops, loops through each measurement and then causes had a lot of measurement.

25
00:01:49,470 --> 00:01:55,200
So this is the function that we want to modify and enter the code for the unscented coming through the

26
00:01:55,380 --> 00:01:55,860
next step.

27
00:01:56,160 --> 00:01:59,970
So between these two lines here, this is where we're going to add the code.

28
00:02:00,720 --> 00:02:09,450
And again, just like inside the extent of the law, the measurements at the moment have a true ID for

29
00:02:09,450 --> 00:02:16,560
the bacon inside the data set so we can use that idea to get the map taken, location, the bacon data,

30
00:02:16,770 --> 00:02:21,900
and then when this is going to give us the X and Y position of that bacon from the data set.

31
00:02:22,560 --> 00:02:29,220
So this is where we want to implement the next step of the update step, which is to implement the innovation

32
00:02:29,220 --> 00:02:30,120
calculations.

33
00:02:30,540 --> 00:02:35,130
So the first step is to augment the state and covariance matrix with the measurement noise.

34
00:02:35,550 --> 00:02:37,640
So we want to form these augmented matrix.

35
00:02:37,860 --> 00:02:43,890
So this augment the state vector is augmented covariance matrix so we can augment the vector with just

36
00:02:43,890 --> 00:02:49,740
two zeros because we know the noise should be zero mean and the covariance matrix, we're going to augment

37
00:02:49,740 --> 00:02:55,220
it with our constant range noise and our constant relative bearing noise.

38
00:02:56,340 --> 00:03:01,320
Once we've generated these covariance matrix and state vector for the augmented system, we want to

39
00:03:01,320 --> 00:03:03,060
generate the signal points and whites.

40
00:03:03,690 --> 00:03:08,370
Then we want to transform the sigma points with the light, a measurement model that we've completed.

41
00:03:08,860 --> 00:03:16,140
So we take we break our signal points into the state and the measurement uncertainty, put it into the

42
00:03:16,140 --> 00:03:18,440
measurement model to get the transformed measurement.

43
00:03:18,720 --> 00:03:25,200
So the way that we've written it up since the light of measurement model function takes in the augmented

44
00:03:25,200 --> 00:03:27,900
state, we don't need to break it up into two different vectors.

45
00:03:27,900 --> 00:03:29,220
We can just pass it directly in.

46
00:03:29,970 --> 00:03:33,910
Once we've done that, we want to calculate the main of the transform sigma points.

47
00:03:33,930 --> 00:03:38,480
So this is to get the estimated measurement so we can use this equation down here.

48
00:03:38,880 --> 00:03:41,250
We calculate, we use the weights that we've calculated.

49
00:03:41,550 --> 00:03:48,750
We multiply them by the each of the predicted measurements and we sum it up to get the predictive measurement

50
00:03:49,560 --> 00:03:51,670
once we're done that we can calculate the innovation.

51
00:03:51,690 --> 00:03:59,340
So this is just the difference between our range and relative bearing measurement compared to the predicted

52
00:03:59,340 --> 00:04:00,110
measurement here.

53
00:04:01,620 --> 00:04:06,060
Then the last step is to calculate the covariance of the Transform Sigma points.

54
00:04:06,390 --> 00:04:14,220
So this is just again using the weighted covariance, looking at each sigma point in, looking at each

55
00:04:14,220 --> 00:04:20,160
sigma point in turn and the difference away from the predicted main which have calculated over here.

56
00:04:22,620 --> 00:04:27,090
Now, this is where it starts to get a bit different, we have to calculate the cross covariance.

57
00:04:27,360 --> 00:04:31,000
So this is the covariance between the state and the measurement.

58
00:04:31,320 --> 00:04:37,380
So since we already have the sigma points from the prediction step and from the updates that we now

59
00:04:37,380 --> 00:04:39,630
want to calculate the cross covariance.

60
00:04:39,660 --> 00:04:42,370
So what that means is that we want to implement this equation here.

61
00:04:43,080 --> 00:04:48,730
So this time here is going to be a difference in the state between the main state and the single point

62
00:04:48,750 --> 00:04:49,090
state.

63
00:04:49,530 --> 00:04:53,120
And over here, this is going to be the difference between the predicted measurement.

64
00:04:53,130 --> 00:04:57,000
So I said hat and our signal points for the measurement.

65
00:04:57,610 --> 00:05:03,060
So we use the same rating functions as we've done before, but we use them in this equation here.

66
00:05:03,090 --> 00:05:07,410
So this is going to give us the cross covariance between the state and the measurements.

67
00:05:08,550 --> 00:05:14,490
Now, when we're using the original sigma points for the state, we also want to make sure that we normalise

68
00:05:14,490 --> 00:05:19,930
the heading angle difference so we can use the help of functions we covered in the previous exercise.

69
00:05:20,640 --> 00:05:25,200
We also want to make sure when we calculate the bearing, measurement, innovation over here that we

70
00:05:25,200 --> 00:05:27,210
also normalize the innovation.

71
00:05:27,750 --> 00:05:33,090
We have to do that in this step over here rather than the other steps to make sure that the covariance

72
00:05:33,090 --> 00:05:34,700
matrix is appropriate.

73
00:05:35,040 --> 00:05:37,270
We representing the heading era.

74
00:05:38,520 --> 00:05:44,040
And then finally, last step in this process is to implement the unscented common filter updates to

75
00:05:44,040 --> 00:05:44,610
the equation.

76
00:05:44,630 --> 00:05:51,450
So we implement the calculation of the gain we use again to update the current state to get the new

77
00:05:51,450 --> 00:05:51,840
state.

78
00:05:52,050 --> 00:05:56,160
And we can also update the covariance matrix so that these equations should be pretty straightforward

79
00:05:56,160 --> 00:05:57,030
for you to implement.

80
00:05:59,530 --> 00:06:04,600
Once we've done that, we can then run the simulation and test out the different profiles so we can

81
00:06:04,600 --> 00:06:10,840
test out profiles one through to eight, and we can look at how much error we get as we run the simulations

82
00:06:11,470 --> 00:06:16,750
so we can compare the simulations between the linear computer, the extend, the current filter and

83
00:06:16,750 --> 00:06:19,200
now the uncertainty from filter.

84
00:06:19,570 --> 00:06:22,060
And we can do that with and without the slider.

85
00:06:22,450 --> 00:06:25,530
So profile one to four and then profiles five to eight.

86
00:06:26,230 --> 00:06:32,280
So looking at this, what do you expect will happen between the accuracy of the different simulations

87
00:06:32,860 --> 00:06:34,950
now I just put on the screen here.

88
00:06:34,960 --> 00:06:41,360
So these are the uncertainties I get for the uncynical midfielder and this is for the UN, for the extended

89
00:06:41,360 --> 00:06:42,010
cut midfielder.

90
00:06:43,690 --> 00:06:48,850
And you can have a look at the results and actually have a look to see if any of these results are surprising.

91
00:06:49,150 --> 00:06:51,550
And then what's the actual cause of this happening?

92
00:06:51,790 --> 00:06:52,710
So what's the difference?