1
00:00:03,810 --> 00:00:07,830
So in this video, we're going to have a look at a few of the things that you should have noticed when

2
00:00:07,830 --> 00:00:12,510
we're running the two vehicle field, a prediction set for the antenna come and filter.

3
00:00:14,040 --> 00:00:18,570
Once you've finished the prediction, step for the antenna, become a filter and then you can run the

4
00:00:18,570 --> 00:00:22,680
simulation, you should be able to see that the filter works quite well.

5
00:00:22,680 --> 00:00:24,720
For the first couple of profiles.

6
00:00:25,090 --> 00:00:30,420
You can see that the nonlinear model of actually having the heading inside the vehicle and then driving

7
00:00:30,420 --> 00:00:36,150
that using the gyroscope information AIDS in the estimation of the vehicle state, just like it did

8
00:00:36,150 --> 00:00:37,380
in the extended filter.

9
00:00:37,920 --> 00:00:42,510
So you can say that as the vehicle turns, you can see that the uncertainty ellipse turns more rapidly

10
00:00:42,510 --> 00:00:44,080
because the state vector is changing.

11
00:00:44,490 --> 00:00:49,650
This allows the filter to track the vehicle position a lot quicker, a lot more responsively than it

12
00:00:49,650 --> 00:00:51,180
did in the model.

13
00:00:51,750 --> 00:00:55,150
So you can say overall the system is working quite well.

14
00:00:55,320 --> 00:01:01,230
You can see that the true state of the vehicle, so the green cross stays with inside the estimated

15
00:01:01,230 --> 00:01:02,130
state of the vehicle.

16
00:01:02,130 --> 00:01:05,400
So the Red Cross and is inside the uncertainty ellipse.

17
00:01:06,000 --> 00:01:10,890
So you can see on this profile before where we have variable speed, you can see that even though the

18
00:01:10,890 --> 00:01:17,310
vehicle turns and accelerates and slows down, the estimated position is always pretty close to the

19
00:01:17,310 --> 00:01:18,090
vehicle truth.

20
00:01:18,810 --> 00:01:25,170
So this is one of the advantages of using a non-linear model and setting up process model in such a

21
00:01:25,170 --> 00:01:31,640
way where you can also have a look at when we start the filter from a non-zero initial condition.

22
00:01:31,980 --> 00:01:38,490
So if we run it with profiling, number two, you can see that we start off the filter at this non-zero

23
00:01:38,520 --> 00:01:39,000
ignition.

24
00:01:39,000 --> 00:01:42,800
So initializes of Filatov on the first GPS measurement, which is going to be here.

25
00:01:42,810 --> 00:01:46,360
So the initial position error is going to be very small.

26
00:01:46,920 --> 00:01:52,290
However, since we start the field up with no velocity information, we assume that velocity zero zero

27
00:01:52,740 --> 00:01:56,160
is not going to have very good velocity information or heading information.

28
00:01:56,550 --> 00:02:02,960
So we can say up here the vehicle heading is actually a negative 135 degrees, while the filter at this

29
00:02:02,970 --> 00:02:04,850
moment thinks is about 48 degrees.

30
00:02:04,860 --> 00:02:07,350
So this is why we get this large heading arrow already.

31
00:02:07,980 --> 00:02:13,350
So you can say as the filter runs, even though it's tracking the position quite well, in this case,

32
00:02:13,350 --> 00:02:15,450
we were just traveling in a single direction.

33
00:02:15,900 --> 00:02:19,500
We can see that there's a large heading era that we've developed.

34
00:02:19,890 --> 00:02:23,700
So the heading is actually 180 degrees away from what it really is.

35
00:02:24,300 --> 00:02:29,850
So even though we have a small position error in this case, if the vehicle actually starts to turn,

36
00:02:29,850 --> 00:02:32,520
the gyroscope information is going to be very incorrect.

37
00:02:32,520 --> 00:02:36,630
It's going to be turning the wrong way because we're 180 degrees out of phase.

38
00:02:37,350 --> 00:02:38,730
So this is one problem.

39
00:02:38,730 --> 00:02:45,420
When we initialize the field with unknown information, we're initializing with unknown velocity information

40
00:02:45,420 --> 00:02:46,310
or heading information.

41
00:02:46,710 --> 00:02:53,040
So this means that the filter can diverge if we actually start to turn during any time here.

42
00:02:53,070 --> 00:02:56,010
The filter would diverge and the errors would grow even larger.

43
00:02:56,010 --> 00:02:58,770
The filter would no longer accurately track the position.

44
00:02:59,580 --> 00:03:03,930
So this is one of the things that we're going to be careful of is how to set up the initial condition

45
00:03:04,350 --> 00:03:06,690
when we don't know what the initial condition is.

46
00:03:07,170 --> 00:03:10,650
So you can say even in this case here we use the unsane common filter.

47
00:03:11,580 --> 00:03:17,430
It's no better than the extended to come a filter when it comes to the initial conditions, the initial

48
00:03:17,430 --> 00:03:21,090
conditions that we set in the filter still have to be close to the truth.

49
00:03:21,090 --> 00:03:22,700
Otherwise, the onset didn't come.

50
00:03:22,710 --> 00:03:27,120
A filter and the extended economy filter will not work as accurately as I could.

51
00:03:29,470 --> 00:03:35,020
So the unscented common filter is still sensitive to the initial conditions and this is being the state

52
00:03:35,020 --> 00:03:41,140
and the covariance, so the state has to be close to the true state and the covariance have to it has

53
00:03:41,140 --> 00:03:45,930
to be an accurate representation of how much error is inside the system state.

54
00:03:46,690 --> 00:03:53,410
So the unscented common filter is not guaranteed to converge, just like in the same case as the extended

55
00:03:53,410 --> 00:03:53,980
common filter.

56
00:03:54,700 --> 00:03:57,390
However, this is different to the linnear common filter.

57
00:03:57,400 --> 00:04:02,020
If we use a linear common filter on, a linear problem is guaranteed to converge.

58
00:04:02,410 --> 00:04:07,510
But for the extended common filter and unscented coming through when we're operating on a non-linear

59
00:04:07,510 --> 00:04:12,220
model, there's no guarantee that the filter will converge and there's no guarantee that the filter

60
00:04:12,220 --> 00:04:15,280
will not diverge and give very incorrect estimates.

61
00:04:16,480 --> 00:04:17,380
So they extend.

62
00:04:17,380 --> 00:04:21,220
The common filter can also diverge if the state is far away from the truth.

63
00:04:21,460 --> 00:04:25,180
And this is because the filter itself still uses an approximation.

64
00:04:25,480 --> 00:04:31,300
Even though we're using the nonlinear model to project the Sigma points, we're still using an approximation

65
00:04:31,300 --> 00:04:36,340
to fit a Gaussian to the probability distribution after we've done the transformation.

66
00:04:36,910 --> 00:04:39,940
So the uncertainty coming filter is still going to be using.

67
00:04:39,940 --> 00:04:43,120
An approximation is not a true nonlinear filter.

68
00:04:43,270 --> 00:04:49,750
It's just an approximation that we can apply to a nominee system to get some estimates out of it.

69
00:04:52,290 --> 00:04:56,500
We want to always try to initialize the full state and covariance from the measurement data.

70
00:04:57,150 --> 00:05:01,390
We can't just assume a zero state with a very large uncertainty.

71
00:05:01,980 --> 00:05:07,180
This is just because since we have a nonlinear model, we can't assume there's a linear dynamics.

72
00:05:07,290 --> 00:05:13,020
So a large uncertainty can cause issues when we try to update the system using some linear approximations

73
00:05:13,020 --> 00:05:16,680
or assuming there's a simple relationship between the uncertainty and the state.

74
00:05:18,790 --> 00:05:25,420
So in this case, when we initialize the field with zero velocity and zero heading, you can see that

75
00:05:25,420 --> 00:05:26,080
full profile.

76
00:05:26,110 --> 00:05:28,420
Number two, the filter did not converge.

77
00:05:28,420 --> 00:05:31,000
It stayed 180 degrees out of phase.

78
00:05:31,000 --> 00:05:36,600
And this will cause issues when we try to also include any maneuvers inside the vehicle.

79
00:05:37,360 --> 00:05:44,920
So it's always best what was always ideal, to initialize the full state of the full state vector,

80
00:05:44,950 --> 00:05:49,330
rather than just assuming that some of the states have zero when they win, they're not always going

81
00:05:49,330 --> 00:05:49,970
to be zero.

82
00:05:50,410 --> 00:05:55,960
And of course, we want the covariance to be representative of how much error is inside the system.

83
00:05:56,200 --> 00:06:02,830
And ideally, we want that error to be as small as possible to keep the filter operating near the linnear

84
00:06:02,830 --> 00:06:07,540
condition that we've operated, that we've linear rise to our approximated the filter around.