1 00:00:04,130 --> 00:00:10,370 In this video, we're going to look at calculating the Lider measurement model innovation, so continuing 2 00:00:10,370 --> 00:00:14,190 on with our vehicle example, we have the measurement model that looks like this. 3 00:00:14,210 --> 00:00:20,540 So this is the measurement model for a lot of like sensor that produces range and relative bearing measurements 4 00:00:20,810 --> 00:00:22,880 to a known landmark position. 5 00:00:24,690 --> 00:00:30,270 We can find the measurement prediction al-Said hat by substituting in our current state estimate our 6 00:00:30,540 --> 00:00:36,670 hat and assuming zero measurement noise, so we assume that our noise vector V is just going to be zero. 7 00:00:36,960 --> 00:00:39,540 So these terms over here cancel out. 8 00:00:39,540 --> 00:00:42,080 So we just get left with this measurement model here. 9 00:00:43,710 --> 00:00:48,870 So let's have a look at how we actually go about calculating the innovation and our sense of measurement 10 00:00:48,870 --> 00:00:50,730 for the slide measurement model. 11 00:00:51,150 --> 00:00:56,280 First off is that we have our current state, so we going to assume that we have a current state that 12 00:00:56,280 --> 00:00:56,900 looks like this. 13 00:00:56,910 --> 00:00:59,790 So we have a diversity of 2.5 meters a second. 14 00:01:00,060 --> 00:01:07,440 We have a heading of point seven radians exposition of 10 meters and a Y position of negative five meters. 15 00:01:08,400 --> 00:01:13,470 We also have the landmark location, so let's assume that we can observe landmark number one. 16 00:01:13,890 --> 00:01:19,800 So the landmark location that we know is going to be at 12 meters for the exposition and at two meters 17 00:01:19,800 --> 00:01:20,970 for the Y position. 18 00:01:22,170 --> 00:01:27,150 So we can take these numbers here for the current state and the landmark position, substitute them 19 00:01:27,150 --> 00:01:31,390 into our Lider measurement model and we end up with this set of equations here. 20 00:01:31,740 --> 00:01:39,500 So basically, wherever we have our L, X and Y, we substitute in our values for the landmark locations. 21 00:01:39,500 --> 00:01:44,730 So we have a wall here for Elex and we have a two here for a L y. 22 00:01:46,320 --> 00:01:51,300 So when we substitute in our current state, we end up with these equations here and we just go through 23 00:01:51,300 --> 00:01:56,790 and do the calculations, so expanding them out and we end up with these numbers here. 24 00:01:57,270 --> 00:02:03,060 So this is the current predictive measurement for landmark one using the current state best estimate. 25 00:02:03,450 --> 00:02:08,130 So this is what we predict that the leader should be showing when we actually do the measurement. 26 00:02:10,270 --> 00:02:15,700 Now, the next step into calculating the innovation is that we need to find the innovation covariance, 27 00:02:15,700 --> 00:02:18,970 and to do this, we need the Jacobean matrixes. 28 00:02:19,090 --> 00:02:23,070 Now to start off, we need the a.e matrix of the sensor model. 29 00:02:23,080 --> 00:02:25,480 So this is our little model in this example. 30 00:02:25,600 --> 00:02:32,410 And we want to get its partial derivatives with respect to the Vector X and we want to evaluate it at 31 00:02:32,410 --> 00:02:33,280 our best estimate. 32 00:02:34,540 --> 00:02:39,190 We also want to calculate that you came in for the measurement model with respect to the measurement 33 00:02:39,190 --> 00:02:41,710 noise out of our vector here. 34 00:02:41,950 --> 00:02:48,100 So this is just the derivative of these functions up here with respect to all our noise inputs or our 35 00:02:48,110 --> 00:02:50,060 vector, which is these components here. 36 00:02:51,730 --> 00:02:55,450 So to start off, let's have a look at the measurement model state Jacobean. 37 00:02:55,450 --> 00:03:02,200 So this one over here, we can calculate this Jacobean by taking the partial derivatives of these two 38 00:03:02,200 --> 00:03:06,900 functions here, one for a range measurement, one for our relative bearing information. 39 00:03:07,180 --> 00:03:12,300 We can take the derivatives of these two parameters with respect to the state vector parameters. 40 00:03:12,310 --> 00:03:18,970 So our velocity heading position X and Y, and when we do that, we end up with this set of equations 41 00:03:18,970 --> 00:03:19,270 here. 42 00:03:19,960 --> 00:03:24,160 So if you remember, we had a state vector being our velocity. 43 00:03:24,400 --> 00:03:27,770 So you can see that the velocity does not come into any of these equations up here. 44 00:03:27,780 --> 00:03:31,310 So for the range or the relative bearing, so we get zero zero. 45 00:03:31,330 --> 00:03:32,890 So there's no effect on velocity. 46 00:03:33,580 --> 00:03:38,460 Now, the next day it was all heading and we can see the this top equation here headings not in it. 47 00:03:38,480 --> 00:03:41,070 So we should get zero for the derivative, which we do. 48 00:03:41,500 --> 00:03:46,570 But down here for a relative bearing, we have a negative side so we can see we're going to have a derivative 49 00:03:46,570 --> 00:03:51,940 of negative one when we take the derivative of this whole function with respect to SA so we can see 50 00:03:51,940 --> 00:03:52,840 we get a negative one. 51 00:03:54,420 --> 00:04:00,920 We can do the same thing for our position in the exercise if we differentiate our arrangement with respect 52 00:04:00,920 --> 00:04:04,530 to our position, yes, we end up with this term here. 53 00:04:04,550 --> 00:04:10,340 So this is just a difference in the position divided by our total distance, which is just given by 54 00:04:10,340 --> 00:04:11,160 this equation here. 55 00:04:11,660 --> 00:04:18,440 So this so these components are just a partial derivatives of these two functions with respect to the 56 00:04:18,440 --> 00:04:19,580 position derivatives. 57 00:04:21,260 --> 00:04:23,760 So we're going to put some numbers to these examples here. 58 00:04:24,080 --> 00:04:31,280 So given our current state showing here and our current landmark given by his and our previous predictive 59 00:04:31,280 --> 00:04:35,990 measurement, which we calculated a few years ago, to give these numbers here, we can now evaluate 60 00:04:35,990 --> 00:04:38,450 our Jacobean for this time period. 61 00:04:38,990 --> 00:04:44,900 So what we want to do is we basically want to fill in our parameters here for our current state and 62 00:04:45,200 --> 00:04:48,920 landmark into this matrix here and just evaluate what it is. 63 00:04:48,980 --> 00:04:51,540 So when we do that, we can substitute in the numbers. 64 00:04:51,560 --> 00:04:55,760 So here we have PEX and we know that 10 we have minus Elix. 65 00:04:55,910 --> 00:04:57,260 We know that Alex is 12. 66 00:04:57,270 --> 00:05:00,690 So we get ten minus 12 divided by our distance. 67 00:05:00,710 --> 00:05:06,100 So this range measurement here, which is just going to be our seven point two eight. 68 00:05:06,110 --> 00:05:07,660 So it's divided by seven point to it. 69 00:05:08,090 --> 00:05:13,460 And we just do that for all the components and we end up with these numbers here when we actually evaluate 70 00:05:13,460 --> 00:05:13,820 it out. 71 00:05:14,300 --> 00:05:18,870 So this is the measurement model describing with respect to the state vector. 72 00:05:19,850 --> 00:05:22,940 Now we also want to find the measurement model noise Jacobean. 73 00:05:22,940 --> 00:05:28,900 So this is coming up here and this is just a partial derivatives of our sense of measurement model. 74 00:05:28,910 --> 00:05:35,390 So these parameters here with respect to the input, noise measurement noise are very vector, which 75 00:05:35,390 --> 00:05:37,070 in this model is over here. 76 00:05:37,610 --> 00:05:42,070 And we don't have to do anything fancy for this because we can see that this is just additive noise. 77 00:05:42,080 --> 00:05:45,140 So all we end up with is just an identity matrix. 78 00:05:45,950 --> 00:05:48,640 So this is just showing that we have additive noise for this model. 79 00:05:48,650 --> 00:05:51,880 So we just expect this to be an identity matrix. 80 00:05:52,610 --> 00:05:57,350 So if we calculate the predictive measurement, we also want to calculate our innovation covariance. 81 00:05:57,800 --> 00:06:01,690 So let's assume that we have a current covariance matrix that looks like this. 82 00:06:01,700 --> 00:06:05,590 So we have an uncertainty of ten point one and two two. 83 00:06:05,870 --> 00:06:09,860 So we just have assumed that we have a diagonal matrix for the current covariance. 84 00:06:10,670 --> 00:06:14,000 We also assume that our sensor is going to have this uncertainty here. 85 00:06:14,000 --> 00:06:17,210 So we assume that we have a convergence of 10 and negative one. 86 00:06:17,210 --> 00:06:23,510 So 10 for the range measurements and three point one for the heading estimate or relative bearing estimate. 87 00:06:25,180 --> 00:06:31,150 The Jacobins that we've calculated are these ones here, and we what we want to do is want to evaluate 88 00:06:31,150 --> 00:06:35,870 the innovation covariance, which is given by this certainly equations here. 89 00:06:36,730 --> 00:06:39,760 So knowing all these elements here is pretty straightforward. 90 00:06:39,790 --> 00:06:45,130 What we have to do, we just have to substitute in the different matrices into this equation and then 91 00:06:45,130 --> 00:06:46,780 expand it out and evaluate it. 92 00:06:47,380 --> 00:06:50,950 So if we just substitute in the matrixes, we end up with this set of equations. 93 00:06:50,950 --> 00:06:57,820 So you can say this term here is how do you tribunal for the with respect to the state vector, this 94 00:06:57,820 --> 00:07:00,610 matrix here we have our current covariance matrix. 95 00:07:01,120 --> 00:07:07,140 So this matrix here into here and again, we have our Jacobean matrix transpose. 96 00:07:07,140 --> 00:07:08,920 So that's just this matrix here. 97 00:07:08,920 --> 00:07:10,170 And we transpose it here. 98 00:07:10,780 --> 00:07:15,090 And again, this term here is going to be the sensor noise covariance. 99 00:07:15,100 --> 00:07:20,740 So we know that these two matrices here are just going to be identity matrix, as we've calculated that 100 00:07:20,740 --> 00:07:21,140 before. 101 00:07:21,160 --> 00:07:25,720 So identity and identity transpose, which is just another identity matrix. 102 00:07:25,990 --> 00:07:32,210 And we have our measurement noise covariance matrix R, which is just this matrix here. 103 00:07:33,070 --> 00:07:38,230 So expanding it all out, doing all the matrix multiplications, we end up with this matrix for The 104 00:07:38,560 --> 00:07:39,070 Matrix. 105 00:07:39,100 --> 00:07:41,640 So this is going to be our innovation covariance. 106 00:07:42,070 --> 00:07:45,700 So this is how much error we expect on our innovation. 107 00:07:47,760 --> 00:07:52,800 So this was a fairly straightforward example of how we actually go about calculating the predictive 108 00:07:52,800 --> 00:07:57,680 measurement and the innovation covariance for the measurement model that we've been showing. 109 00:07:57,690 --> 00:08:05,370 So this is for using our vehicle process model of state vector, having vissi positioning position in 110 00:08:05,370 --> 00:08:12,040 the way and with all the measurement model producing range and relative bearing measurement information. 111 00:08:13,080 --> 00:08:15,480 So there's nothing too fancy, nothing complicated going on. 112 00:08:15,480 --> 00:08:20,160 It's just a matter of understanding equations and filling in the relative calculations and then just 113 00:08:20,160 --> 00:08:22,170 going through and doing the calculations.