1 00:00:03,570 --> 00:00:08,310 In this video, we're going to look at the extent come and filter prediction step and how we actually 2 00:00:08,310 --> 00:00:11,850 go about deriving the equations that we use to implement the STIP. 3 00:00:12,920 --> 00:00:18,250 So we have seen that the state sector and covariance matrix can be predicted using the following equations, 4 00:00:18,470 --> 00:00:24,250 we have the state vector prediction equation and we have the covariance matrix prediction equations. 5 00:00:25,070 --> 00:00:27,270 But where do these equations actually come from? 6 00:00:27,620 --> 00:00:32,720 So we see we have seen that they're very similar to the linear computer, but are they still applicable 7 00:00:32,720 --> 00:00:34,120 for the extended computer? 8 00:00:34,340 --> 00:00:39,650 So we have to go through and actually draw these equations to actually prove that they're still applicable 9 00:00:39,650 --> 00:00:40,670 for the situation. 10 00:00:42,230 --> 00:00:47,990 So first off, we're going to start to look at a Taylor series expansion, a Taylor series expansion 11 00:00:47,990 --> 00:00:52,700 is one way of approximating a non-linear system with a linear system. 12 00:00:53,210 --> 00:00:58,760 So taking the Taylor series, expansion of the state process model as shown here, and we're going to 13 00:00:58,760 --> 00:01:03,410 take the Taylor series about the true state with zero process model noise. 14 00:01:03,980 --> 00:01:05,820 We get this equation here. 15 00:01:05,900 --> 00:01:11,360 So we take the state transition function here and we make a Taylor series approximation about this parameter 16 00:01:11,360 --> 00:01:17,180 here by the state vector by substituting in our best prediction state, expanding it out, we get this 17 00:01:17,180 --> 00:01:17,930 equation here. 18 00:01:18,260 --> 00:01:23,960 So this is pretty much saying the Taylor series expansion is the state process model evaluated at this 19 00:01:23,960 --> 00:01:30,740 position, plus the Jacobean matrix multiplied by the difference in positions between the true state 20 00:01:30,740 --> 00:01:35,530 and the estimated state, plus the Jakobshavn for the noise multiplied by noise. 21 00:01:35,780 --> 00:01:38,840 And then we're going to get all these higher extra order terms. 22 00:01:41,600 --> 00:01:46,760 Now, this equation can be limited to the first or the terms only, so get rid of all these higher order 23 00:01:46,760 --> 00:01:50,910 terms and we only get left with the first order times shown here. 24 00:01:51,350 --> 00:01:57,410 So these terms here, which are repeated down here, this is an approximation of what the true state 25 00:01:57,410 --> 00:01:58,200 is at times. 26 00:01:58,250 --> 00:02:05,240 OK, given the information at the best estimate, TimeStep K minus one and the true state a K minus 27 00:02:05,240 --> 00:02:05,570 one. 28 00:02:07,140 --> 00:02:10,080 So this is going to be a key identity that we use later on. 29 00:02:12,080 --> 00:02:19,220 So now if we look at the extended filter state predictions step, we can use a definition of the a prior 30 00:02:19,220 --> 00:02:19,710 estimate. 31 00:02:20,030 --> 00:02:25,070 So the April, I estimate, is basically saying we're getting the expected value of the true state at 32 00:02:25,070 --> 00:02:28,970 time k given all the information up to came on this one. 33 00:02:29,750 --> 00:02:31,250 And of course, we do not like this. 34 00:02:31,250 --> 00:02:37,310 So we have the negatives emoted, noting that this is the a priori because it's very easy information 35 00:02:37,310 --> 00:02:43,330 up to the previous timestep is four times HepC and it's the best estimate because it's hot. 36 00:02:43,340 --> 00:02:45,620 So it's the estimate of the state, Victor. 37 00:02:48,140 --> 00:02:54,620 We can substitute in an approximation for České, so the true state at times said, okay, we can use 38 00:02:54,620 --> 00:03:00,210 our Taylor series expansion, substituting it into this equation here to get this following equation. 39 00:03:01,340 --> 00:03:06,740 So what we have here is that we have taken the expectation of writer on side our first order Taylor 40 00:03:06,740 --> 00:03:07,730 series expansion. 41 00:03:08,510 --> 00:03:10,060 And we can have a look at this time here. 42 00:03:10,280 --> 00:03:17,090 And we know by definition this term is the previous a posterior estimate of the state vector. 43 00:03:18,140 --> 00:03:24,560 So if we substitute this in here, since this is also in the expectation of Rita and this is the expectation 44 00:03:24,560 --> 00:03:30,200 operator as well, we know that we can change this value here just to be our true state, a K minus 45 00:03:30,200 --> 00:03:30,530 one. 46 00:03:31,070 --> 00:03:36,800 So when we do that, we can say we have X K minus one, minus K minus one, which means that this whole 47 00:03:36,800 --> 00:03:39,050 term here pretty much gets canceled out. 48 00:03:40,480 --> 00:03:45,970 We can also have a look at this time over here, so we're looking at this time here we have our noize, 49 00:03:45,970 --> 00:03:51,970 Victor Omega, but since we're inside an expectation of radar, we also know the identity, the expected 50 00:03:51,970 --> 00:03:58,510 noise in the system is going to be zero because as we take the because we know that the noise should 51 00:03:58,510 --> 00:03:59,410 have zero remain. 52 00:03:59,410 --> 00:04:02,360 So if we take the expectation of radar on the noise, it should be zero. 53 00:04:02,770 --> 00:04:05,940 So this time here and this time here, both cancel out. 54 00:04:06,550 --> 00:04:09,040 So what we get left with is obvious implication. 55 00:04:09,040 --> 00:04:14,830 We get the extended coming field of state prediction, which is basically saying that the state estimate 56 00:04:15,040 --> 00:04:15,550 for Tompsett. 57 00:04:15,550 --> 00:04:21,550 K is pretty much just a state transition function for the previous best estimate predicted forward using 58 00:04:21,550 --> 00:04:25,720 the current control and using zero Maine for the noise. 59 00:04:26,200 --> 00:04:28,400 So this is what we expected it to be. 60 00:04:28,420 --> 00:04:33,310 So this should be fairly straightforward, but we've proved that this is valid using the Taylor series 61 00:04:33,310 --> 00:04:36,000 expansion for the extended Matura. 62 00:04:37,150 --> 00:04:38,300 So that's the prediction step. 63 00:04:38,320 --> 00:04:41,140 So now we can look at the covariance prediction step. 64 00:04:41,970 --> 00:04:46,620 So, again, let's have a look at the definition of the state prediction era, the state prediction 65 00:04:46,620 --> 00:04:51,240 error is basically the true state minus the estimated state, and that's going to give us a clearer 66 00:04:51,240 --> 00:04:51,660 state. 67 00:04:52,730 --> 00:04:58,760 So we can substitute in our Taylor series approximation for XQ that's we've developed before, and we 68 00:04:58,760 --> 00:05:04,490 can also now substitute in for our predicted using the common filter prediction step. 69 00:05:04,940 --> 00:05:08,010 So basically, when we do that, we end up with this equation here. 70 00:05:08,390 --> 00:05:14,450 So we have our Taylor series expansion, which is this first term here, subtracted it from our prediction 71 00:05:14,690 --> 00:05:16,730 estimate, which is this term here. 72 00:05:16,730 --> 00:05:19,050 This is the state transition process model. 73 00:05:20,180 --> 00:05:25,010 So when we look at this, what we can say is we can say that this term here and this term here, they 74 00:05:25,010 --> 00:05:26,240 both cancel themselves out. 75 00:05:27,800 --> 00:05:30,420 And we just left with this term over here. 76 00:05:30,860 --> 00:05:35,720 So this is going to be our time for our state prediction error relationship. 77 00:05:35,720 --> 00:05:42,110 So our predicted state era is just going to be the driving times, the predicted error from last time, 78 00:05:42,800 --> 00:05:44,740 plus the Jacobean tons of noise. 79 00:05:45,950 --> 00:05:49,430 We can also use the definition of the AIPA covariance. 80 00:05:49,820 --> 00:05:55,520 So this covariance matrix here is looking at the amount of error around our predicted state so we can 81 00:05:55,520 --> 00:06:00,580 substitute in our equation for our estimation error that we worked out from the last slide into here. 82 00:06:01,040 --> 00:06:03,740 We can see this time here and this time here. 83 00:06:03,890 --> 00:06:06,360 Both these are both the estimation error. 84 00:06:06,380 --> 00:06:11,240 So we can substitute this term in here for both of them and we end up with this equation here. 85 00:06:13,470 --> 00:06:18,930 Now, on closer inspection, we can say a few things, this here is pretty much the estimation error 86 00:06:18,930 --> 00:06:20,650 squared and we already know what that is. 87 00:06:21,000 --> 00:06:27,450 This term is just going to be the covariance matrix for the previous timestep as given this by this 88 00:06:27,450 --> 00:06:28,200 equation here. 89 00:06:28,860 --> 00:06:31,210 We also can look at these terms over here. 90 00:06:31,230 --> 00:06:36,450 So this is the process, the noise squared and what's inside the expectation of radar. 91 00:06:36,480 --> 00:06:42,150 So, again, this is just going to be the variance of the noise, which we already know is key matrix. 92 00:06:42,840 --> 00:06:48,510 So we can substitute in our various matrix here for the process model for the previous step and our 93 00:06:48,510 --> 00:06:49,820 noise covariance matrix here. 94 00:06:50,310 --> 00:06:56,610 And we end up with the common field of covariance prediction equation, which is shown here. 95 00:06:57,570 --> 00:07:02,760 So this equation is very similar to the linear computer, is that now we're using the Jacobean matrix 96 00:07:02,760 --> 00:07:04,820 instead of the F and L matrixes. 97 00:07:05,280 --> 00:07:10,560 And basically what this is doing, this is taking a first order approximation of the error in the system. 98 00:07:10,810 --> 00:07:15,960 And this is perfectly valid because we prove that it is valid when we taking a Taylor series expansion 99 00:07:15,960 --> 00:07:17,700 around the state estimate. 100 00:07:18,910 --> 00:07:23,500 However, this is, of course, going to have a few caveats that come with it when we take the Taylor 101 00:07:23,500 --> 00:07:28,560 series expansion, we're doing a first order linear approximation on the process model. 102 00:07:28,900 --> 00:07:35,530 So if we get outside the linear region or if the linear regression point is too far away from linear 103 00:07:35,530 --> 00:07:38,290 region, then this equation starts to break down. 104 00:07:38,680 --> 00:07:42,850 But for reasonably linear systems, this equation is perfectly valid. 105 00:07:45,170 --> 00:07:50,270 So in summary, we can get the predicted state estimate by using the process model here, putting in 106 00:07:50,270 --> 00:07:56,130 our previous state to get our new state, putting in our current control and put it in zero noise. 107 00:07:56,840 --> 00:08:02,150 We can also calculate a predicted covariance estimate, which is pretty much propagating the previous 108 00:08:02,160 --> 00:08:08,630 covariance, using the Jacobins for the process model and taking account the extra noise which are induced 109 00:08:08,630 --> 00:08:10,300 by the process model noise. 110 00:08:10,520 --> 00:08:16,550 So basically taking our key matrix and transforming it, using the process model Jacobins with respect 111 00:08:16,550 --> 00:08:24,110 to noise, transforming it into our state's space x so we basically transform it both into the state 112 00:08:24,110 --> 00:08:25,940 vector system frame. 113 00:08:28,550 --> 00:08:35,240 So this is where the assumptions get baked into the extent of your equations, we've used a Taylor series 114 00:08:35,240 --> 00:08:39,020 expansion about a linear ization point, about the best estimate. 115 00:08:39,350 --> 00:08:41,990 And because we've done that, it makes some assumptions. 116 00:08:41,990 --> 00:08:47,000 It assumes that we're always operating with a good estimate of the system that's close to the truth. 117 00:08:47,390 --> 00:08:53,390 And it is assuming that the system can be fairly well represented by this Taylor series expansion with 118 00:08:53,390 --> 00:08:55,280 only including the first order terms.