1
00:00:06,660 --> 00:00:11,080
In the previous video, we looked at the extent to come a further update, step equations.

2
00:00:11,100 --> 00:00:14,220
So in this video, we're going to have a closer look at how we derive them.

3
00:00:14,970 --> 00:00:20,550
So we have seen that the state vector and the covariance matrix can be updated using these set of equations

4
00:00:20,550 --> 00:00:20,870
here.

5
00:00:21,390 --> 00:00:23,050
But where do these equations come from?

6
00:00:23,340 --> 00:00:29,010
How do we know that these are valid for when we're using the filter for non-linear systems?

7
00:00:31,120 --> 00:00:36,940
The first step in this process is to look at expanding the estimation era with the innovation correction

8
00:00:36,950 --> 00:00:37,270
step.

9
00:00:37,660 --> 00:00:43,570
So if you look at calculating the estimation error, the estimation error is Alex with the tilde symbol

10
00:00:43,570 --> 00:00:43,990
above it.

11
00:00:44,200 --> 00:00:47,870
And it's just going to be the difference between the true state and our estimated state.

12
00:00:48,960 --> 00:00:55,240
I can substitute in our correction step for our estimation error by using the common four step.

13
00:00:55,270 --> 00:00:57,780
So this is our update step here.

14
00:00:57,790 --> 00:01:05,200
We have our previous best assessment, plus our common gane times the innovation and we can also substitute

15
00:01:05,200 --> 00:01:07,370
in our definition for the innovation.

16
00:01:07,390 --> 00:01:14,230
So in one of the previous videos, we calculated the Taylor series expansion for the innovation and

17
00:01:14,230 --> 00:01:15,460
which is this time here.

18
00:01:15,490 --> 00:01:21,740
So we look at the Jacobins of the measurement model and we look at probation's away from the true state.

19
00:01:21,760 --> 00:01:24,810
So this is the area between the true state and the estimated state.

20
00:01:25,180 --> 00:01:30,190
And we look at how the process and we look at how the measurement model affects the process here.

21
00:01:30,730 --> 00:01:36,010
So taking this approximation for our innovation is shooting it into this equation here.

22
00:01:36,520 --> 00:01:41,740
We end up with these equations and again, having a closer inspection of this.

23
00:01:41,740 --> 00:01:45,850
We can say that this time here is the difference between the true state and the estimate, the state

24
00:01:46,240 --> 00:01:47,350
as well as over here.

25
00:01:47,980 --> 00:01:50,970
So these two terms here are just our estimation error.

26
00:01:51,730 --> 00:01:57,640
So we can rewrite this equations in terms of only the estimation error and rearrange it to get it into

27
00:01:57,640 --> 00:01:58,090
this form.

28
00:01:58,090 --> 00:02:00,910
Here we have the current estimated era.

29
00:02:00,940 --> 00:02:06,730
After we do, the update is going to be a function of the current estimation error before we do the

30
00:02:06,730 --> 00:02:09,460
update, as well as the amount of noise in the system.

31
00:02:09,460 --> 00:02:11,620
And then we have these two scaling matrices here.

32
00:02:12,070 --> 00:02:17,350
So this pretty much gives us every kind of relationship of how do you describe how the error changes

33
00:02:17,530 --> 00:02:19,760
before and after we do the update.

34
00:02:21,100 --> 00:02:27,700
So using the definition of the posterior error covariance, which is given by this equation here.

35
00:02:27,730 --> 00:02:30,660
So this is the error covariance after we do the update.

36
00:02:31,000 --> 00:02:35,890
So it's like a variance matrix of the error for TimeStep K given all the information up to TimeStep,

37
00:02:35,890 --> 00:02:40,900
K is just the expected value of our estimation.

38
00:02:40,900 --> 00:02:41,620
Error squared.

39
00:02:42,280 --> 00:02:47,200
So on the previous slide, we worked out the estimation error so we can go about just substituting it

40
00:02:47,200 --> 00:02:49,210
in our value for the estimation error.

41
00:02:49,930 --> 00:02:53,140
And we end up with this big set of equations here.

42
00:02:55,220 --> 00:03:00,350
So we're using the expectation of right here, but we know that the expected value of any concern is

43
00:03:00,350 --> 00:03:01,670
just going to be that content.

44
00:03:01,670 --> 00:03:06,080
So you can see that we've put out a set of concerns here instead of constant fear.

45
00:03:06,530 --> 00:03:09,410
And same thing here and here.

46
00:03:09,590 --> 00:03:15,530
And we just left with the expected value and we just left with the expectation operator on these two

47
00:03:15,530 --> 00:03:17,160
sets of parameters here.

48
00:03:17,660 --> 00:03:23,150
Now, we know that this is just the expected value of the estimation, R-squared, which is just another

49
00:03:23,150 --> 00:03:24,750
term for the covariance matrix.

50
00:03:24,890 --> 00:03:28,460
So this is just the covariance matrix without the current measurement applied.

51
00:03:28,700 --> 00:03:32,950
And this covariance matrix over here is just the measurement noise, covariance matrix.

52
00:03:32,960 --> 00:03:34,090
So the matrix.

53
00:03:35,000 --> 00:03:40,700
So we've come up with an equation that links the covariance matrix before the update to the covariance

54
00:03:40,700 --> 00:03:42,010
matrix after the update.

55
00:03:42,230 --> 00:03:46,850
And it's also going to be a function of the current uncertainty in the system from the measurement.

56
00:03:49,530 --> 00:03:54,750
So where the magic happens is that we want to find the solution for the common for the game matrix k

57
00:03:54,900 --> 00:04:02,190
that minimizes the trace of the updated covariance matrix P and then this will give us the optimum solution

58
00:04:02,190 --> 00:04:05,190
to minimize the conditional mean squared estimation error.

59
00:04:05,460 --> 00:04:07,990
So this is the whole process of the common filter.

60
00:04:08,160 --> 00:04:14,160
So this is where the computer calculates the optimal value of the K matrix to provide the optimal solution.

61
00:04:15,910 --> 00:04:20,620
The first step in this equation is to define a cost function and we're going to define the cost function

62
00:04:20,620 --> 00:04:24,280
l to just be the trace of the covariance matrix.

63
00:04:24,700 --> 00:04:30,250
So the covariance matrix, the equation that we worked out on the last slide, we can substituted into

64
00:04:30,250 --> 00:04:35,980
this equation here, calculate the trace of it, and now we want to minimize the cost function.

65
00:04:36,250 --> 00:04:41,340
And we do this by looking at the derivative of this cost function with respect to a common field again.

66
00:04:41,710 --> 00:04:45,980
So this is basically going to calculate the common field again that minimizes this function here.

67
00:04:46,810 --> 00:04:50,980
So to do that, we want to differentiate our cost function with respect to common field again.

68
00:04:50,980 --> 00:04:55,930
And we end up with this equation here and define the minimum of this equation.

69
00:04:55,940 --> 00:04:57,540
We want to set it equal to zero.

70
00:04:58,300 --> 00:05:03,400
Once we do that, it's just a matter of rearranging the equation to work out what K is.

71
00:05:03,580 --> 00:05:09,730
And when we do that, we end up with this common field or update function here that calculates the coming

72
00:05:09,730 --> 00:05:10,720
for the game matrix.

73
00:05:11,560 --> 00:05:17,500
So you can see that we have our inverse s and again, we know what this is, is just the innovation

74
00:05:18,160 --> 00:05:19,110
covariance matrix.

75
00:05:19,120 --> 00:05:23,950
So this is just our Jacobsen's times, our previous covariance transformation.

76
00:05:23,950 --> 00:05:26,110
And then the same thing for the measurement model noise.

77
00:05:27,650 --> 00:05:33,080
So this is where we get the comment for the game matrix from, so comment for the game Matrix K is the

78
00:05:33,080 --> 00:05:38,990
optimum solution to minimize the trace of the covariance matrix, which minimizes the conditional mean

79
00:05:39,000 --> 00:05:40,310
squared estimation error.

80
00:05:42,600 --> 00:05:46,800
So now that we have the comment for the game, we can actually go through the update step.

81
00:05:47,340 --> 00:05:52,370
So the solution for the update step equation is just this set of equations here.

82
00:05:52,380 --> 00:05:58,650
So we have our updated state estimate from our previous estimate, plus the common field of game at

83
00:05:58,670 --> 00:06:00,060
risk times in innovation.

84
00:06:00,720 --> 00:06:05,430
So this is what we started with initially and this is the definition of the common thought, the game

85
00:06:05,440 --> 00:06:05,930
matrix.

86
00:06:05,940 --> 00:06:07,500
So this is how we calculate K.

87
00:06:08,430 --> 00:06:11,690
Now, the solution for the covariance is also easily recovered.

88
00:06:11,700 --> 00:06:15,240
So we have our covariance matrix update set.

89
00:06:16,110 --> 00:06:18,100
So we have our covariance equation here.

90
00:06:18,390 --> 00:06:26,220
We can substitute in our definition for the optimum common gain matrix K and we end up with this equation

91
00:06:26,220 --> 00:06:26,560
here.

92
00:06:26,580 --> 00:06:33,590
So basically we have a function of the previous covariance matrix minus this K as K transpose.

93
00:06:33,600 --> 00:06:40,440
So this transforms the innovation covariance into the way it looks at how much we have to update the

94
00:06:40,440 --> 00:06:41,580
covariance matrix by.

95
00:06:42,390 --> 00:06:49,140
And if we substitute in our previous equation for S, we can actually rearrange this equation and times

96
00:06:49,260 --> 00:06:51,570
cancel out and the left with this set.