1 00:00:00,390 --> 00:00:01,290 ‫Welcome back. 2 00:00:01,920 --> 00:00:08,640 ‫So now that we have covered the prediction formula, let's go to the heart of the embassy controller. 3 00:00:09,000 --> 00:00:16,320 ‫The cost function, the cost function has the same format, like in the previous course, it's quadratic. 4 00:00:16,740 --> 00:00:21,520 ‫You see, this one was what we had in the previous course here. 5 00:00:22,440 --> 00:00:24,420 ‫And this is our cost function here. 6 00:00:24,430 --> 00:00:26,160 ‫You see, it's the same format. 7 00:00:26,430 --> 00:00:33,690 ‫The only difference now is that instead of the steering wheel angle instead of Delta, we have this 8 00:00:33,870 --> 00:00:35,760 ‫control input vector here. 9 00:00:36,600 --> 00:00:40,790 ‫And also in our case, the horizon period is four. 10 00:00:40,800 --> 00:00:47,430 ‫So that means that this and here this N equals four. 11 00:00:48,090 --> 00:00:54,300 ‫And so from the previous course, the steering wheel angle that control input was a scalar. 12 00:00:54,600 --> 00:01:01,030 ‫But in our case, in the other case, our control input is a vector. 13 00:01:01,050 --> 00:01:05,010 ‫It's a three by one vector. 14 00:01:05,970 --> 00:01:10,920 ‫And that's how this three by one control input vector looks like. 15 00:01:11,670 --> 00:01:19,140 ‫You have your you two and K plus i you three at kapos I and you for at Kaplans I. 16 00:01:19,920 --> 00:01:22,950 ‫And here we are back with our original notation. 17 00:01:23,310 --> 00:01:32,160 ‫So now if I write X Sub K, there is the state vector now. 18 00:01:32,340 --> 00:01:34,740 ‫OK, so that's your present state. 19 00:01:35,550 --> 00:01:47,820 ‫If I write X Sub K plus one, then it's a state vector 0.1 seconds later x k plus two. 20 00:01:48,480 --> 00:01:57,060 ‫That's the state vector, 0.2 seconds later and etc. And so let's draw a small time line here. 21 00:01:57,570 --> 00:02:00,300 ‫Well, actually, it's a sample time line. 22 00:02:01,380 --> 00:02:03,000 ‫You have your key here. 23 00:02:04,110 --> 00:02:11,370 ‫Then this is Kate plus one, Kate plus two K plus three. 24 00:02:11,490 --> 00:02:14,080 ‫And finally, Kate plus four. 25 00:02:15,060 --> 00:02:17,850 ‫So again, this is your present here. 26 00:02:18,990 --> 00:02:26,910 ‫This is your sample time interval, which is TTS equals 0.1 seconds. 27 00:02:27,940 --> 00:02:34,900 ‫And so the first term in the cost function, which is this one here. 28 00:02:36,060 --> 00:02:39,750 ‫This term is for this part here. 29 00:02:40,820 --> 00:02:48,140 ‫So if you're an equals four, then it would be K plus four since your horizon period is four, then 30 00:02:48,140 --> 00:02:55,670 ‫it would cover K plus one K plus two, K plus three and K plus four. 31 00:02:56,660 --> 00:03:08,450 ‫So again, the first term in the cost function is for the errors in the last time sample at K Plus four, 32 00:03:09,020 --> 00:03:19,640 ‫and the rest of the cost function is for the time samples from K up until K plus three. 33 00:03:20,780 --> 00:03:24,170 ‫So if I take this entire term here. 34 00:03:25,390 --> 00:03:31,960 ‫This entire term in purple will be for this from K. 35 00:03:31,990 --> 00:03:33,940 ‫Up until K plus three. 36 00:03:34,840 --> 00:03:48,370 ‫And you can see it as I in the summation sign will go from I equals to zero to I equals two and minus 37 00:03:48,370 --> 00:03:52,150 ‫one, which is four minus one, which is three. 38 00:03:53,200 --> 00:04:03,340 ‫Then in the autonomous vehicle case, the error vectors where two by one vectors two rows one column. 39 00:04:04,360 --> 00:04:09,040 ‫And so you had two rows because you had a CI variable. 40 00:04:09,220 --> 00:04:13,210 ‫And then the initial Y variable, they were also your outputs. 41 00:04:14,270 --> 00:04:23,360 ‫Remember, you had four states, however, out of those four states, you chose two outputs, so your 42 00:04:23,360 --> 00:04:29,450 ‫error vector, let's say at Cape Plus, I would look like this. 43 00:04:30,640 --> 00:04:33,950 ‫Error sigh at K plus I. 44 00:04:34,760 --> 00:04:47,260 ‫An error inertial wye at Cape Plus I, and since an error is reference value minus the true output value, 45 00:04:47,410 --> 00:04:50,800 ‫then this vector can also be rewritten like this. 46 00:04:51,860 --> 00:05:07,670 ‫CI are at Cape Plus I minus sy at K Plus I also the inertial Y reference at K plus i minus the true 47 00:05:07,670 --> 00:05:12,860 ‫Y output, the inertial y at K plus I. 48 00:05:13,640 --> 00:05:19,310 ‫So this vector here that you can also rewrite in this form. 49 00:05:19,700 --> 00:05:23,030 ‫That's how it looks like here and here. 50 00:05:23,030 --> 00:05:25,250 ‫It would simply be transposed. 51 00:05:25,260 --> 00:05:27,380 ‫You see you have a your transpose sign. 52 00:05:27,920 --> 00:05:32,000 ‫So here this vector would be one row and two columns. 53 00:05:32,960 --> 00:05:43,100 ‫And of course, if instead of I, you had here and then we would be talking about this vector here in 54 00:05:43,100 --> 00:05:50,750 ‫the first term and therefore our weight matrices Q and S. 55 00:05:52,000 --> 00:05:54,790 ‫They were two by two diagonal matrices. 56 00:05:55,820 --> 00:06:07,910 ‫So, Q was Q1, Q2 and then you had the zero here and here, and the same thing with S S1, S2 and then 57 00:06:07,910 --> 00:06:09,110 ‫zero and zero. 58 00:06:10,190 --> 00:06:17,390 ‫Also, you only had one control input, you had your steering wheel angle, your delta. 59 00:06:18,400 --> 00:06:21,760 ‫Therefore, you are was simply a scalar. 60 00:06:22,800 --> 00:06:30,420 ‫So in this term here in the previous course, it was like this, you had your steering wheel input, 61 00:06:31,230 --> 00:06:32,460 ‫which was a scalar. 62 00:06:33,390 --> 00:06:34,740 ‫Then you had your scalar. 63 00:06:34,740 --> 00:06:39,600 ‫Ah, and then you had the same scalar steering wheel input. 64 00:06:40,320 --> 00:06:48,810 ‫This transpose sine didn't matter there because if you have scalar transpose, well, that equals the 65 00:06:48,810 --> 00:06:50,940 ‫same scalar. 66 00:06:52,130 --> 00:06:55,610 ‫So that was our autonomous vehicle. 67 00:06:56,650 --> 00:07:00,580 ‫However, in the UAV case, the dimensions are different. 68 00:07:01,360 --> 00:07:05,770 ‫The error vectors are three by one vectors. 69 00:07:06,730 --> 00:07:16,330 ‫Remember, we had six states and out of these six states we would choose three outputs Phi Theta and 70 00:07:16,450 --> 00:07:24,100 ‫CI, because we have to compare them to the reference values of PHI, our CTA, R and R. 71 00:07:25,250 --> 00:07:33,260 ‫So the error vector that we have here at Kate plus I, that we have here in our cost function, it will 72 00:07:33,260 --> 00:07:34,130 ‫look like this. 73 00:07:35,140 --> 00:07:47,110 ‫Error fi at K Plus I error SETA at Cape Plus AI and error CI at Cape Plus AI and in the same way, like 74 00:07:47,110 --> 00:07:48,400 ‫here you can rewrite it. 75 00:07:48,400 --> 00:08:01,120 ‫Like this fly R at K plus AI minus the true fi as the output at K plus ai seta r at K plus ai minus 76 00:08:01,450 --> 00:08:11,740 ‫true seta at K plus ai ci are at Cape Plus AI minus the true CI at K plus AI. 77 00:08:12,640 --> 00:08:15,490 ‫So that's how the vector can be rewritten. 78 00:08:16,610 --> 00:08:19,790 ‫Then this here would be your control input vector. 79 00:08:20,180 --> 00:08:29,030 ‫And again, this is a three by one vector, three rows one column that means that the weight matrices 80 00:08:29,030 --> 00:08:32,690 ‫que se and are. 81 00:08:33,880 --> 00:08:40,030 ‫They are all three by three diagonal mattresses. 82 00:08:41,140 --> 00:08:42,830 ‫There will be a queue matrix. 83 00:08:43,840 --> 00:08:45,610 ‫That would be your SE matrix. 84 00:08:46,540 --> 00:08:52,900 ‫And this year, our matrix here, they can all be different, but they're all three by three diagonal 85 00:08:52,900 --> 00:08:53,560 ‫matrices. 86 00:08:54,190 --> 00:09:04,660 ‫And of course, the reason why we had one half in the cost function here and also here you see you have 87 00:09:04,660 --> 00:09:13,150 ‫your one half in this term and in this term, the reason for that was that in the cost function, when 88 00:09:13,150 --> 00:09:20,140 ‫we took its gradient, then you would end up with a constant to over to. 89 00:09:21,080 --> 00:09:23,630 ‫Which you could cancel out, because it would be one. 90 00:09:24,500 --> 00:09:28,250 ‫So you see in the previous course this was your cost function here. 91 00:09:29,230 --> 00:09:36,850 ‫And when you took the gradient of it, then you managed to have it in this form that you have one here 92 00:09:36,850 --> 00:09:41,020 ‫and one here in front of these two terms. 93 00:09:41,920 --> 00:09:44,620 ‫And of course, these ones, you don't have to write them here. 94 00:09:44,620 --> 00:09:46,900 ‫So it makes this expression easier. 95 00:09:48,130 --> 00:09:52,930 ‫And here I would simply like to show you the cost function terms again. 96 00:09:53,470 --> 00:09:55,400 ‫So let's start with this one. 97 00:09:55,480 --> 00:10:00,370 ‫If we take this term here, then if you write it out, it looks like this. 98 00:10:01,330 --> 00:10:06,550 ‫So this one would be the error vector at Cape Bliss End, but transposed. 99 00:10:07,390 --> 00:10:10,570 ‫This would be the error vector at Cape Plus End. 100 00:10:11,560 --> 00:10:18,760 ‫And this would be your best weight matrix, so you can choose different weights here for S1 S2 in this 101 00:10:18,790 --> 00:10:19,180 ‫three. 102 00:10:19,960 --> 00:10:23,470 ‫Then this term here it's this one. 103 00:10:24,400 --> 00:10:33,070 ‫So the error vector transposed at Cape Plus I this would be your error vector at Cape Plus I this would 104 00:10:33,070 --> 00:10:34,690 ‫be your cue weight matrix. 105 00:10:35,770 --> 00:10:39,610 ‫Again, you can choose different weights for your cu weight matrices. 106 00:10:40,420 --> 00:10:44,530 ‫And finally, this term here looks like this one. 107 00:10:45,510 --> 00:10:55,590 ‫That's your control input vector transposed, this is your control input vector and this is your arm 108 00:10:55,590 --> 00:10:56,550 ‫matrix here. 109 00:10:57,360 --> 00:11:01,890 ‫And of course, you can choose your different are one or two and are three. 110 00:11:03,160 --> 00:11:16,480 ‫So these two terms here, in other words, these two here they are for I equals zero, I equals one, 111 00:11:16,840 --> 00:11:23,830 ‫I equals two and I equals three, and I'm talking about this. 112 00:11:23,830 --> 00:11:25,330 ‫I hear this. 113 00:11:25,330 --> 00:11:30,310 ‫I goes from zero to three and minus one four minus one. 114 00:11:30,310 --> 00:11:31,090 ‫That's three. 115 00:11:31,960 --> 00:11:37,030 ‫And then finally, this term, here is this one. 116 00:11:38,110 --> 00:11:42,250 ‫And that's for when and equals four. 117 00:11:43,300 --> 00:11:44,890 ‫That's for this one here.