1
00:00:03,440 --> 00:00:07,290
So let's have a look at the calculation of an optimistic example.

2
00:00:07,580 --> 00:00:13,970
So continue on our innovation example using the lead measurement model and without without 2D nonlinear

3
00:00:13,970 --> 00:00:19,640
vehicle model, we have the following data that we've calculated in us here in the series of previous

4
00:00:19,640 --> 00:00:25,380
videos we have that the current state, we have the current covariance, we have our measurements.

5
00:00:25,400 --> 00:00:28,090
So this is our predicted measurement from a measurement model.

6
00:00:28,370 --> 00:00:32,340
And let's assume that we have a sense measurement that gives these values here.

7
00:00:32,360 --> 00:00:34,160
So this is what we have from the sensor.

8
00:00:34,490 --> 00:00:36,520
This is what we have from our prediction.

9
00:00:37,130 --> 00:00:41,870
We have the innovation, which is just going to be a difference between the two and we have our innovation

10
00:00:41,870 --> 00:00:43,810
covariance such we've calculated before.

11
00:00:44,150 --> 00:00:46,280
And lastly, we have our Jacobins.

12
00:00:46,710 --> 00:00:51,620
So after we've collected all this information, we can go through and actually apply the comerio to

13
00:00:51,620 --> 00:00:52,730
apply ISTEP equations.

14
00:00:54,840 --> 00:00:58,820
So the first step in the process is to calculate a common field of game metrics K.

15
00:00:59,120 --> 00:01:02,120
And again, this is fairly straightforward now that we have all the metrics.

16
00:01:02,240 --> 00:01:05,670
So we have our previous covariance matrix p showing here.

17
00:01:06,030 --> 00:01:07,920
We have the Giacomini metrics.

18
00:01:07,920 --> 00:01:12,340
So Hech metrics here and we have our metrics as shown here.

19
00:01:12,750 --> 00:01:19,380
So to calculate K, we just have our covariance times, the transpose of our Jacobean metrics multiplied

20
00:01:19,380 --> 00:01:22,700
by the inverse of our innovation covariance.

21
00:01:22,980 --> 00:01:28,080
And if we go through do the operations mathematically expanding at the magic series and performing the

22
00:01:28,080 --> 00:01:31,750
inverse, we end up with this comment for the game matrix here.

23
00:01:32,520 --> 00:01:40,050
So if we have a look at what this is saying, it means that this top term, this top line of here corresponds

24
00:01:40,050 --> 00:01:42,120
to how much we can update the velocity.

25
00:01:42,510 --> 00:01:45,450
This is how much we can update the heading information.

26
00:01:45,720 --> 00:01:47,900
This is how much we can update the exposition.

27
00:01:47,910 --> 00:01:55,630
And this is how much we can update the Y position, given errors in the based on the innovation.

28
00:01:57,090 --> 00:02:02,520
So when we actually do the innovation update step here, we can say that we have the current state this

29
00:02:02,600 --> 00:02:10,980
hour, minus the innovation and our common field again matrix so we can go through and we can multiply

30
00:02:10,980 --> 00:02:12,600
the innovation by a common game.

31
00:02:12,960 --> 00:02:17,170
So this is going to calculate how much we need to update our best estimate vector.

32
00:02:17,550 --> 00:02:23,400
So when we expand this out, we end up with our updated vector so we can see that from the common gain

33
00:02:23,400 --> 00:02:24,390
that we haven't updated.

34
00:02:24,390 --> 00:02:30,240
I've lost the arrow because there's no information about the velocity, but we can see that we've come

35
00:02:30,240 --> 00:02:35,250
up with a better estimate for the heading now because we've had we had a bit of hitting error in the

36
00:02:35,310 --> 00:02:37,470
we had a bit of relative bearing error from the measurement.

37
00:02:37,620 --> 00:02:40,380
So we've now updated our heading estimate for the vehicle.

38
00:02:40,830 --> 00:02:46,770
You can also say that we've slightly updated our position in the X and position in the wall in the Y

39
00:02:47,040 --> 00:02:51,400
because we had a bit of measurement error for the range.

40
00:02:52,830 --> 00:02:55,890
So this is how we calculate the updated state estimate.

41
00:02:56,700 --> 00:02:59,120
We also have to update the covariance time.

42
00:02:59,130 --> 00:03:04,380
So looking at this equation here, all we have to do is calculate the scaling term and multiply it by

43
00:03:04,380 --> 00:03:07,300
our previous covariance to get the updated covariance.

44
00:03:08,160 --> 00:03:13,890
So basically we multiply our common gain by how GIACOBBI and subtract the identity matrix of the same

45
00:03:13,890 --> 00:03:14,610
size of it.

46
00:03:14,820 --> 00:03:17,700
And that becomes the scaling time and again.

47
00:03:17,700 --> 00:03:19,440
We have the Matrix's down here.

48
00:03:19,470 --> 00:03:24,330
So basically all we do is we fill in the different matrices and we end up with this expanded equation

49
00:03:25,050 --> 00:03:30,120
with the old covariance matrix on the end, and then we multiply it out and then we get the new updated

50
00:03:30,120 --> 00:03:31,110
covariance matrix.

51
00:03:31,470 --> 00:03:36,090
So you can actually see now we haven't had any improvement in velocity because we haven't actually had

52
00:03:36,090 --> 00:03:37,100
any information there.

53
00:03:37,680 --> 00:03:40,870
But you can see now heading estimate, it's only a bit smaller.

54
00:03:41,220 --> 00:03:44,700
We can see that our position variances are also smaller as well.

55
00:03:44,830 --> 00:03:48,810
So we've improved the quality of our estimates of the state vector.