1 00:00:04,080 --> 00:00:09,540 In the last few videos, we looked at a few of the differently square solutions, we first started off 2 00:00:09,540 --> 00:00:15,660 with normal a square solution which has the solution that looks like this, then we extended the solution 3 00:00:15,660 --> 00:00:17,590 for our way to the least squares solution. 4 00:00:17,610 --> 00:00:21,750 So this is what happens when we have a bit more information about the noise and the measurements. 5 00:00:22,080 --> 00:00:27,030 We can include that in the estimation process and we end up with a solution that looks like this. 6 00:00:27,490 --> 00:00:32,130 So I can see the difference here between the solutions that we've now included this time. 7 00:00:32,580 --> 00:00:36,540 So this is the measurement uncertainty into the into the solution. 8 00:00:37,630 --> 00:00:43,620 And since we actually know the uncertainty of the Y measurements due to the noise, we can then use 9 00:00:43,620 --> 00:00:47,940 this equation here to work out the uncertainty in the estimates in X. 10 00:00:48,360 --> 00:00:51,850 So this is the estimate, uncertainty or covariance matrix. 11 00:00:53,250 --> 00:00:56,580 Next, what they're going to look at is called recursively squares. 12 00:00:57,840 --> 00:01:02,930 So in the past, we've learned how to calculate a constant with a square solution. 13 00:01:02,940 --> 00:01:07,720 We build up a model matrix from all the different measurements and we solve the equation. 14 00:01:08,370 --> 00:01:12,560 This requires all the measurements to be available at time that we calculate the solution. 15 00:01:12,990 --> 00:01:14,100 What happens if we don't? 16 00:01:14,880 --> 00:01:18,580 What happens if we want to update the estimate as measurements become available? 17 00:01:19,350 --> 00:01:23,250 Do we keep storing all the measurements and add new euros into the Matrix? 18 00:01:23,850 --> 00:01:28,590 A better way is actually to use a recursive update every time a new measurement is available. 19 00:01:29,250 --> 00:01:33,510 Doing this would mean that we don't have to keep a history of all the previous measurements and we would 20 00:01:33,510 --> 00:01:35,880 only have to keep the best estimate from last time. 21 00:01:36,180 --> 00:01:37,860 So there are huge advantages in. 22 00:01:37,860 --> 00:01:42,290 This is also more applicable in situations where you always want an estimate. 23 00:01:42,300 --> 00:01:47,930 So say you have a sense of measurement coming in at a very low rate, like once an hour, once a day. 24 00:01:48,420 --> 00:01:51,470 You don't want to have to wait a few days to get an estimate. 25 00:01:51,480 --> 00:01:53,370 You want to have an estimate every time. 26 00:01:54,600 --> 00:01:56,700 So we can use recursively squares to do this. 27 00:01:57,360 --> 00:02:02,040 So we're going to look at how we actually calculate this recursively square solution. 28 00:02:04,130 --> 00:02:06,320 So let's have a look at the problem statement. 29 00:02:06,560 --> 00:02:09,820 So first off, we have our measurement equation, which we've seen before. 30 00:02:09,830 --> 00:02:14,990 So this is for a single measurement, not for all measurements, just a single one. 31 00:02:15,350 --> 00:02:18,190 And again, we know properties of the noise vector here. 32 00:02:18,740 --> 00:02:25,670 So we know that first it's going to have a certain variance given by R is going to be a zero Amane expected 33 00:02:25,670 --> 00:02:29,650 value if you sum up all the different noise values from all the different measurements. 34 00:02:30,140 --> 00:02:34,610 And we also know that each of the measurements is going to be each of the noise on the measurement is 35 00:02:34,610 --> 00:02:35,680 going to be uncorrelated. 36 00:02:35,690 --> 00:02:36,750 It's going to be independent. 37 00:02:36,770 --> 00:02:38,330 So there's going to be no correlation. 38 00:02:41,410 --> 00:02:47,740 So for recursive estimation to work, we first start off with a guess, initial guess or a good best 39 00:02:47,740 --> 00:02:50,390 estimate of what our solution is going to be. 40 00:02:51,220 --> 00:02:55,540 Then when we get a measurement, what we do is that we fuse it into the solution and we come up with 41 00:02:55,540 --> 00:02:58,600 our next best estimate using this information here. 42 00:02:59,560 --> 00:03:04,690 Once we got done that, then we get another measurement in and then we update our estimate again so 43 00:03:04,690 --> 00:03:09,080 we recursively step forward the solution every time we get a new measurement. 44 00:03:09,610 --> 00:03:11,200 So he just follows the same process. 45 00:03:12,790 --> 00:03:17,800 Now, you can sort of visualize this as this process here, so we have an initial estimate of the state 46 00:03:18,070 --> 00:03:23,800 estimate, we get information, we get the measurement information, we use it in to update the estimate 47 00:03:24,370 --> 00:03:26,590 to the current time, to the current measurement. 48 00:03:27,100 --> 00:03:31,350 And then this measurement here comes back to become the previous one. 49 00:03:31,360 --> 00:03:32,820 And then we get a new measurement in. 50 00:03:32,830 --> 00:03:35,560 So it just recursively goes around the circle. 51 00:03:35,710 --> 00:03:38,040 The current estimate becomes the previous estimate. 52 00:03:38,050 --> 00:03:43,000 Every time we get a new measurement, we update the estimate and we just keep going through this, so 53 00:03:43,000 --> 00:03:43,400 forth. 54 00:03:44,290 --> 00:03:49,570 Now, how we can actually write this in mathematical terms is that we can use a recursive estimation 55 00:03:49,570 --> 00:03:50,110 equation. 56 00:03:51,550 --> 00:03:52,270 Same here. 57 00:03:52,870 --> 00:03:59,690 So here, this is going to be our best estimate for all the measurements up to and including the K measurement. 58 00:04:00,640 --> 00:04:02,840 This is going to be a previous best estimate. 59 00:04:02,860 --> 00:04:08,500 So this is going to be a best estimate without the current measurement used in and then we're going 60 00:04:08,500 --> 00:04:10,040 to have this recursive term here. 61 00:04:10,600 --> 00:04:15,880 So this term here is the error between the current measurement and what the predicted measurement should 62 00:04:15,880 --> 00:04:16,120 be. 63 00:04:16,150 --> 00:04:18,340 So this is the measurement model metrics. 64 00:04:19,060 --> 00:04:20,570 This is the previous estimate. 65 00:04:20,860 --> 00:04:24,120 So if we multiply these two together, we get a predicted estimate. 66 00:04:24,340 --> 00:04:28,750 And the difference between this predicted estimate of the measurement and the actual true measurement 67 00:04:28,750 --> 00:04:34,690 that we get is going to be an error term that we can feed back to work out how much we have to update 68 00:04:34,690 --> 00:04:38,470 our best estimate from the previous times up to the current. 69 00:04:39,940 --> 00:04:44,360 And we can do this by multiplying this amount of error through this game matrix here. 70 00:04:44,650 --> 00:04:46,780 So this is going to be a correction game. 71 00:04:46,960 --> 00:04:52,960 And this is Kresh, again, is the game that minimizes the square cost function for all the previous 72 00:04:52,960 --> 00:04:56,260 solutions, for all the previous measurements that we've had. 73 00:04:57,280 --> 00:05:03,010 So to actually work out the least squares solution, we have to come up with how we calculate this game 74 00:05:03,010 --> 00:05:03,360 matrix. 75 00:05:03,360 --> 00:05:09,880 Okay, because this is going to affect how much we need to update our previous estimate to get our current 76 00:05:09,880 --> 00:05:11,950 estimate based on our current measurement. 77 00:05:12,700 --> 00:05:18,820 So if we work out how to solve this equation or how to solve this case for it to be the solution to 78 00:05:18,820 --> 00:05:22,690 these squares, then we can just using the information, using this equation here. 79 00:05:22,690 --> 00:05:28,330 And every time we get a new measurement, we can put it in here, calculate an update and get a better 80 00:05:28,330 --> 00:05:29,080 estimate here. 81 00:05:31,180 --> 00:05:36,310 So the first part of the solution, we're going to look at how to ride out the because of estimation 82 00:05:36,310 --> 00:05:36,620 error. 83 00:05:37,210 --> 00:05:42,220 So this is the residual error for the current measurement and it's going to be the difference between 84 00:05:42,220 --> 00:05:47,580 the true state X and our estimate at the current state EXC. 85 00:05:49,560 --> 00:05:56,260 So we can substitute in our recursive solution into this time here to end up with this equation here. 86 00:05:56,910 --> 00:06:06,150 We can also substitute in our mathematical model for our measurement work, and we end up with an equation 87 00:06:06,150 --> 00:06:06,930 that looks like this. 88 00:06:09,030 --> 00:06:15,420 So this can be rearranged into a final solution here, so we now we can say that our current estimation 89 00:06:15,420 --> 00:06:21,560 error is a function of the previous estimation error, plus our current noise. 90 00:06:22,500 --> 00:06:27,090 So this is going to be a recursive estimation of our estimation error. 91 00:06:28,690 --> 00:06:30,710 Now, I can say this is an interesting term here. 92 00:06:30,730 --> 00:06:35,780 This is the identity matrix time minus the game matrix times. 93 00:06:37,090 --> 00:06:42,520 Now, if this was the one and this number was less than one, you can see that the estimation error 94 00:06:42,760 --> 00:06:45,220 is always going to be less afterwards. 95 00:06:45,250 --> 00:06:50,350 So if this is one term here, I mean, multiply it by number less than one, it means that this is going 96 00:06:50,350 --> 00:06:50,890 to be smaller. 97 00:06:52,130 --> 00:06:58,130 So this means that this sort of it shows that as we put more and more estimates into the solution, 98 00:06:58,340 --> 00:07:00,650 ideally this should always be getting less. 99 00:07:00,650 --> 00:07:04,120 They should be always be less estimation error after we do each measurement. 100 00:07:04,910 --> 00:07:07,610 But this relies on the fact that work has to be set up. 101 00:07:07,610 --> 00:07:10,070 Such a Katamatite is going to be less than one. 102 00:07:13,050 --> 00:07:18,870 We also want to have a look at our recursive equation for the uncertainty so we know that the uncertainty 103 00:07:18,870 --> 00:07:24,240 is just going to be the covariance matrix and it's going to be the covariance matrix of the measurement 104 00:07:24,240 --> 00:07:24,960 residuals. 105 00:07:25,230 --> 00:07:32,580 So using this equation here that we've seen before, we can substitute in this equation above so we 106 00:07:32,580 --> 00:07:34,440 can substitute it in and expand it. 107 00:07:35,040 --> 00:07:37,150 We end up with an equation that looks like this. 108 00:07:37,710 --> 00:07:40,140 So this should be sort of a bit familiar. 109 00:07:40,170 --> 00:07:47,160 You can see that we have a sort of matrix times and invariants times matrix again, but transposed and 110 00:07:47,160 --> 00:07:47,920 you can see here. 111 00:07:48,390 --> 00:07:54,930 So this is just how we can transpose the measurement, uncertainty or the covariance from one frame 112 00:07:54,930 --> 00:07:55,450 to another. 113 00:07:55,470 --> 00:08:02,040 So we have the initial covariance here and multiply it by some transition matrix and in that transition 114 00:08:02,040 --> 00:08:07,860 matrix to transpose the uncertainty in a different frame or in after an operation. 115 00:08:08,220 --> 00:08:09,900 And we can say here we do the same thing here. 116 00:08:09,930 --> 00:08:15,530 So the Matrix pretty much maps the uncertainty in the measurement to the uncertainty in a state estimate. 117 00:08:16,860 --> 00:08:19,470 Now we can look at the cost function that we want to minimize. 118 00:08:20,280 --> 00:08:27,270 So the cost function that we're minimizing in recursive estimation is pretty much the we want to minimize 119 00:08:27,270 --> 00:08:31,660 the residuals squared for all the different measurements up to and including K. 120 00:08:32,010 --> 00:08:33,950 So this is a cost function for K. 121 00:08:35,010 --> 00:08:42,390 Now the difference between the true state and the estimated state can be just be written out as the 122 00:08:42,600 --> 00:08:43,920 measurement residuals squared. 123 00:08:45,570 --> 00:08:48,230 And again, they should be looking familiar. 124 00:08:48,720 --> 00:08:56,050 We can rewrite this into a vector equation of just measurement residual transposed time to measure residuals. 125 00:08:57,590 --> 00:09:00,140 Now, this is also very similar. 126 00:09:00,200 --> 00:09:05,440 This is almost the covariance matrix, but this is going to end up giving us a scalar. 127 00:09:06,170 --> 00:09:12,850 But what we can do is that we can actually link this scale to be the trace of the covariance matrix. 128 00:09:13,070 --> 00:09:18,320 So the convergence matrix is just the measurement residual transpose time to measure residuals. 129 00:09:18,320 --> 00:09:19,580 So that's the opposite of this. 130 00:09:20,240 --> 00:09:24,520 But the trace of this matrix is just going to be equal to the cost function. 131 00:09:25,520 --> 00:09:31,760 Therefore, we know that minimizing the solution to solution uncertainty also minimizes the cost function. 132 00:09:32,480 --> 00:09:39,950 So if we find a solution that minimizes this uncertainty matrix p we also finding a solution that minimizes 133 00:09:39,950 --> 00:09:40,820 this cost function. 134 00:09:45,480 --> 00:09:47,850 Define the minimum so that we can find a solution. 135 00:09:47,880 --> 00:09:53,870 What we have to do is we have to minimize our OK, we have to minimize the solution uncertainty. 136 00:09:55,170 --> 00:09:57,960 So we do this by minimizing cost function with respect. 137 00:09:57,990 --> 00:10:05,560 OK, so this time, instead of working at our X, our estimate, we're using recursive estimation. 138 00:10:05,600 --> 00:10:11,370 So now we have to work out the optimum game matrix that minimizes the solution so we differentiate our 139 00:10:11,370 --> 00:10:12,060 cost function. 140 00:10:12,960 --> 00:10:20,190 So if we work out Alpay matrix on the previous slide, we differentiate that with respect to K, we 141 00:10:20,190 --> 00:10:22,080 end up with this solution here. 142 00:10:23,040 --> 00:10:25,970 And in the past we know that we want to find the minimum point. 143 00:10:25,980 --> 00:10:31,500 So we have to find a stationary point by setting the derivative to zero and then we guarantee that this 144 00:10:31,500 --> 00:10:33,680 is a minimum since is only a single minimum. 145 00:10:34,500 --> 00:10:35,650 So we set it to zero. 146 00:10:35,790 --> 00:10:40,260 We rewrite the equation and we end up with this being the optimum cost function. 147 00:10:41,310 --> 00:10:46,420 So this function here minimizes the cost function for the given K. 148 00:10:46,860 --> 00:10:53,610 So this is how we can calculate K that we can put into a recursive solution that always minimizes the 149 00:10:53,760 --> 00:10:54,480 squared error. 150 00:10:56,490 --> 00:11:01,860 So to put that in practice, we have an initial guest of our state or estimate the thing that we want 151 00:11:01,860 --> 00:11:04,990 to estimate and we know how accurate our estimate is. 152 00:11:05,010 --> 00:11:07,200 So we have an initial covariance matrix. 153 00:11:08,100 --> 00:11:13,440 So, you know, initial guess, we might not know these exactly like we might have no information about 154 00:11:13,440 --> 00:11:15,570 what our initial estimate is. 155 00:11:15,910 --> 00:11:18,140 So they might just started off with zero. 156 00:11:18,780 --> 00:11:22,620 And if we do that, well, then we can say that we have no idea what our estimate is. 157 00:11:22,650 --> 00:11:27,780 So we start off our covariance to be some really, really large number, which means we pretty much 158 00:11:27,780 --> 00:11:29,550 have no faith in the current estimate. 159 00:11:30,750 --> 00:11:36,660 But once we've set up our initial estimate of the state at our covariance of the estimate, we then 160 00:11:36,810 --> 00:11:38,970 look at our recursively square solution. 161 00:11:39,120 --> 00:11:43,370 So every time we get a new measurement, we can calculate this game matrix, okay? 162 00:11:44,220 --> 00:11:47,710 We can use that game matrix inside of our updated equation. 163 00:11:47,760 --> 00:11:53,550 So this is at least squares solution that takes in our previous estimate to calculate our new estimate. 164 00:11:53,760 --> 00:11:55,560 And you can say it's a function of this game. 165 00:11:55,560 --> 00:11:59,650 Matrix K is also a function of our measurement here. 166 00:12:00,150 --> 00:12:03,750 So using the measurement information, we can update our current best estimate. 167 00:12:04,230 --> 00:12:08,040 And then when we do that, we can update our solution uncertainty. 168 00:12:09,030 --> 00:12:11,930 So this is a recursive really square solution. 169 00:12:12,690 --> 00:12:15,630 We go through this process every time we get a new update. 170 00:12:16,290 --> 00:12:20,940 And by doing this, we're always going to be getting the best update possible using all the information 171 00:12:20,940 --> 00:12:22,910 up to and including that measurement. 172 00:12:23,520 --> 00:12:30,360 And this happens because all the previous uncertainty and effects of each of those measurements is stored 173 00:12:30,360 --> 00:12:32,370 in this covariance matrix here. 174 00:12:32,940 --> 00:12:37,980 So this convergence matrix always keeps internally a history of all the measurements that have been 175 00:12:37,980 --> 00:12:38,700 made before.