1
00:00:04,010 --> 00:00:09,170
Now that we worked out the linear approximation of the system that we want to estimate, we can use

2
00:00:09,170 --> 00:00:14,690
this linear approximation along with the linear filter to estimate the dynamic states of the pendulum

3
00:00:14,690 --> 00:00:16,490
is in measurements of the angle theta.

4
00:00:17,180 --> 00:00:21,380
The common field of code will be exactly the same as the code that we've used in the past.

5
00:00:21,740 --> 00:00:28,080
But all we have to do is change the state vector, the process model and the measurement model.

6
00:00:28,640 --> 00:00:33,860
So in general, to apply the linear common filter to your estimation problem, you need to decide on

7
00:00:33,860 --> 00:00:40,190
the state vector X the process model F the system process uncertainty, matrix skew measurement, model

8
00:00:40,190 --> 00:00:46,010
H and the measurement uncertainty, covariance matrix are the rest of the calculations are basically

9
00:00:46,010 --> 00:00:47,970
the same for any estimation problem.

10
00:00:49,640 --> 00:00:57,140
So here is our continuous linear system of the form X equals X plus B you with the state's theta and

11
00:00:57,140 --> 00:01:03,860
theta dot, we will again assume that we have zero torque or zero input again, just like we did with

12
00:01:03,860 --> 00:01:09,590
the 2D track, for example, and will be doing this just because we are only interested in the free

13
00:01:09,590 --> 00:01:12,520
response to the system or unforced response of the system.

14
00:01:13,710 --> 00:01:18,390
Now we need to turn this continuous system into this great form, and we're going to do this by calculating

15
00:01:18,390 --> 00:01:23,800
the matrix, which we can do using the Matrix exponential relationship that we've derived in the past.

16
00:01:24,690 --> 00:01:30,480
We will also assume that we have a random unknown amount of input or talk given by this random parameter

17
00:01:30,480 --> 00:01:31,020
W.

18
00:01:32,090 --> 00:01:38,240
And this random input is only going to be affecting the theater, not state the size of this uncertainty,

19
00:01:38,240 --> 00:01:43,880
a process more noise will be controlled by the parameter sigma squared tau so we can set the process

20
00:01:43,910 --> 00:01:49,410
model noise, covariance matrix due to simply be this value here of our sigma squared value.

21
00:01:49,940 --> 00:01:55,190
So every time we do a prediction step, we add an extra uncertainty to the angular velocity state.

22
00:01:56,290 --> 00:01:58,490
This will cause the uncertainty in the system to grow.

23
00:01:58,510 --> 00:02:04,210
Every time we do a prediction that this increased uncertainty can help the system work in cases when

24
00:02:04,210 --> 00:02:09,880
we have bad approximations or approximations that aren't entirely correct, the measurement model,

25
00:02:09,880 --> 00:02:11,590
again, is a very simple model.

26
00:02:11,590 --> 00:02:16,810
We will simply take measurements of the angular position data and assume that there's some additive

27
00:02:16,810 --> 00:02:23,710
noise v where they is a random variable of a zero main Gaussian distribution with variance sigma squared

28
00:02:23,710 --> 00:02:24,160
theta.

29
00:02:25,240 --> 00:02:29,500
So our measurement model is simply a selection matrix for the first theta.

30
00:02:29,830 --> 00:02:34,900
And our measurement model covariance matrix is simply the variance sigma squared theta.

31
00:02:37,670 --> 00:02:43,430
We will be doing a simulation exercise like we did last time, your exercise will be to go through and

32
00:02:43,430 --> 00:02:46,420
implement the equations as explained on the previous slides.

33
00:02:46,700 --> 00:02:52,730
And once you've done that, that's all you need to do to have implemented a common filter for the pendulum

34
00:02:52,730 --> 00:02:53,690
dynamic system.

35
00:02:54,660 --> 00:03:00,660
So in the code for this assignment, there is the initialise function for the Kamasutra or you have

36
00:03:00,660 --> 00:03:08,640
to do is to go out and fill in the values for the F key Hajnal Matrixes and put them into our initialize

37
00:03:08,640 --> 00:03:10,050
function as shown here.

38
00:03:10,830 --> 00:03:12,810
So if we switch over to the code.

39
00:03:13,770 --> 00:03:17,010
You can see the function that we have to modify right here.

40
00:03:17,280 --> 00:03:20,880
So this is the assignment to underscore filtered up by code.

41
00:03:21,360 --> 00:03:26,890
So this is the code that we need to modify for you to have implemented the common filter for the Pentagon

42
00:03:26,910 --> 00:03:29,070
system, the rest of the field.

43
00:03:29,190 --> 00:03:35,190
So it's such as their prediction step and the update step of the code is pretty much exactly the same

44
00:03:35,190 --> 00:03:37,950
as the carry that we use for the previous filters.

45
00:03:38,520 --> 00:03:42,260
The only thing we need to change is the system matrixes for the filter itself.

46
00:03:43,230 --> 00:03:48,620
So the simulation options for this assignment have already been set up correctly for this exercise.

47
00:03:48,930 --> 00:03:52,020
So the simulation has a timestep or or one.

48
00:03:52,410 --> 00:03:54,000
We run it for 10 seconds.

49
00:03:54,000 --> 00:03:55,260
We have a measurement.

50
00:03:55,260 --> 00:03:55,540
Right.

51
00:03:55,560 --> 00:03:58,350
So how often do we make measurements of theta at 10 hertz?

52
00:03:58,710 --> 00:04:03,090
Each of these measurements is going to have a random noise associated with it and it's going to be all

53
00:04:03,090 --> 00:04:05,430
of our standard deviation of point one degrees.

54
00:04:06,330 --> 00:04:11,510
We're always going to start at a same angle, which is going to be a small angle inside a linear range.

55
00:04:11,820 --> 00:04:14,780
And of course, we're going to draw the plots and do the animation.

56
00:04:15,570 --> 00:04:21,150
So the common filter, when we're running it, we're going to have a pretty small noise talk of point

57
00:04:21,330 --> 00:04:22,050
zero one.

58
00:04:22,530 --> 00:04:27,270
They're going to make sure our measurement standard deviation for the matrix is going to be the same

59
00:04:27,270 --> 00:04:29,330
as what we've actually used inside the simulation.

60
00:04:29,340 --> 00:04:30,540
So point one degrees.

61
00:04:31,470 --> 00:04:36,300
We're also going to initialize the filter on our first measurement.

62
00:04:37,200 --> 00:04:39,290
So we're not going to start the filter off at zero zero.

63
00:04:39,300 --> 00:04:44,100
We're going to take the first theta measurement and then we're going to set that to be our current theta

64
00:04:44,100 --> 00:04:47,640
state and we're going to set the velocity to be zero.

65
00:04:47,640 --> 00:04:51,990
But I have an uncertainty of about 10 degrees per second.

66
00:04:52,990 --> 00:04:56,040
So once this is done, we can run that and then we can see the results.

67
00:04:58,150 --> 00:05:01,970
So switching over to the answer, when we run this, you should see this result.

68
00:05:02,770 --> 00:05:09,190
We should see the pendulum moving back and forth and you can see that the system is tracking the truth

69
00:05:09,190 --> 00:05:09,880
very well.

70
00:05:09,910 --> 00:05:14,350
So the red one is the estimate, the blue one underneath is this system truth?

71
00:05:14,950 --> 00:05:21,130
If we have a look at our measurement innovations, you can see that our standard deviation, our standard

72
00:05:21,130 --> 00:05:28,210
deviation for our measurement, covariance, innovation, covariance, sorry, is about our point,

73
00:05:28,210 --> 00:05:28,610
too.

74
00:05:28,810 --> 00:05:31,630
We can see down here the opposition to the measurements.

75
00:05:31,630 --> 00:05:34,860
Innovation is point to seven.

76
00:05:34,870 --> 00:05:39,220
So they're very closely aligned and so they should be fairly well trained.

77
00:05:39,850 --> 00:05:46,660
If we have a look at our angles here, we can see the estimated theta along with the tree theta and

78
00:05:46,660 --> 00:05:48,570
we can see the same thing for our velocity.

79
00:05:49,030 --> 00:05:52,860
You can see that we start the initial condition of zero from the first measurement here.

80
00:05:53,230 --> 00:05:57,940
And you can see that we initialize our velocity to be zero because we don't have a measurement at that

81
00:05:57,940 --> 00:05:58,420
time.

82
00:05:58,780 --> 00:06:03,910
But you can see after one or two measurements, it pretty much converges to what the truth is.

83
00:06:04,900 --> 00:06:11,620
We can see as we run the simulation, we estimate the ferocity of the theta quite well, even though

84
00:06:11,620 --> 00:06:13,900
we're only getting position measurements for Theta.

85
00:06:14,770 --> 00:06:20,260
If we have a look at a balance here, we we can see that the theta position error is within a three

86
00:06:20,260 --> 00:06:20,980
sigma balance.

87
00:06:20,980 --> 00:06:22,210
So it's fairly well contained.

88
00:06:22,600 --> 00:06:25,390
And the same thing for our theta have lost the error.

89
00:06:25,600 --> 00:06:28,330
It's well within our three sigma bounds here.

90
00:06:29,200 --> 00:06:33,430
So once you've finished implementing your vision of the common filter, you should pretty much get the

91
00:06:33,430 --> 00:06:36,180
exact same results as Shane saying on the screen here.

92
00:06:36,820 --> 00:06:39,220
And once you've done that, then we can move on to the next video.

93
00:06:39,220 --> 00:06:42,250
Will we talk about turning the filter and the performance of the filter?