1 00:00:00,590 --> 00:00:01,380 ‫Welcome back. 2 00:00:01,890 --> 00:00:06,300 ‫So now we're going to start with the inner loop that starts from here. 3 00:00:07,070 --> 00:00:10,520 ‫So our position control gave us our fire. 4 00:00:10,520 --> 00:00:15,530 ‫Our thoughts are and you won, you won, went straight into the plant. 5 00:00:15,530 --> 00:00:26,240 ‫But then by ah and Sittar went into the NPC controller along with ah, that came from the planner. 6 00:00:27,020 --> 00:00:32,210 ‫And so all that will happen here inside this inner for loop. 7 00:00:32,870 --> 00:00:39,860 ‫And then at the very beginning of this inner for loop, we're going to go into this function here, 8 00:00:40,010 --> 00:00:42,560 ‫LPT continuous discrete function. 9 00:00:43,220 --> 00:00:45,320 ‫And so it's going to happen here. 10 00:00:45,730 --> 00:00:54,110 ‫You see LPT continuous discrete, it's going to take in the state values and then also the omegle values, 11 00:00:54,110 --> 00:00:56,210 ‫the angular velocities of the propeller. 12 00:00:56,720 --> 00:01:08,390 ‫And in return you will get the discrete A, B, C and D matrices and also you will get the updated X, 13 00:01:08,720 --> 00:01:11,690 ‫Y and Z dot values here. 14 00:01:12,410 --> 00:01:17,560 ‫And the same thing with Phi Phi Darsey tying the seat on the UN and the sign side that. 15 00:01:17,900 --> 00:01:19,630 ‫So let's see how it looks like. 16 00:01:20,270 --> 00:01:26,900 ‫So this function starts from here and then I'm going to take the initial constants that I need from 17 00:01:26,900 --> 00:01:27,890 ‫the net function. 18 00:01:28,670 --> 00:01:33,410 ‫I'm also going to take the states that I need at this point. 19 00:01:33,410 --> 00:01:37,150 ‫I don't need X, Y and Z states in this function. 20 00:01:37,160 --> 00:01:41,150 ‫So that's why you see, I go from five to nine. 21 00:01:41,900 --> 00:01:44,870 ‫I don't need to extract the X, Y and Z states. 22 00:01:45,620 --> 00:01:55,850 ‫And then here again, I have my rotation matrix and then I take my U VW variables and then I compute 23 00:01:55,850 --> 00:01:58,060 ‫my X, Y and Z dot. 24 00:01:58,640 --> 00:02:04,850 ‫Now, in reality, you don't really need to perform this operation here because this operation will 25 00:02:04,850 --> 00:02:06,610 ‫also be performed in the plant. 26 00:02:07,310 --> 00:02:15,080 ‫However, I simply built my code in such a way that at some point in the middle of the inner loop, 27 00:02:15,350 --> 00:02:27,500 ‫I want to record my velocity values so my X, Y and Z dot so I will not accumulate them in the middle 28 00:02:27,500 --> 00:02:29,180 ‫of my inner loop. 29 00:02:29,510 --> 00:02:39,980 ‫And that's why in order to get the latest values, I update my X, Y and Z dot in this function and 30 00:02:39,980 --> 00:02:46,790 ‫then this LPT function will give me the updated Eckstut without incident and then I can accumulate them 31 00:02:46,790 --> 00:02:51,110 ‫all in this velocity X, Y, Z, total vector. 32 00:02:51,890 --> 00:02:54,500 ‫And now back to our OPV function. 33 00:02:55,130 --> 00:03:02,360 ‫And what I do need to do for sure is that I need to take this transfer matrix. 34 00:03:02,360 --> 00:03:05,120 ‫You see this T matrix and I define it here. 35 00:03:05,600 --> 00:03:12,410 ‫This is now the transfer matrix or not the rotation matrix, but the transfer matrix that we had derived 36 00:03:12,410 --> 00:03:13,340 ‫in Section two. 37 00:03:13,940 --> 00:03:23,180 ‫And then I take my angular velocity in the body frame, then I multiply that transfer matrix by my peak. 38 00:03:23,180 --> 00:03:31,310 ‫You are variables and then I get my fi that theta that inside that. 39 00:03:32,120 --> 00:03:44,150 ‫And now this entire block here, up until here, this entire block, this is to create these matrices 40 00:03:44,150 --> 00:03:44,630 ‫here. 41 00:03:44,810 --> 00:03:45,400 ‫All right. 42 00:03:46,190 --> 00:03:50,240 ‫So you see this is your a matrix here, which is a six by six matrix. 43 00:03:51,110 --> 00:03:58,430 ‫And so I define it here, six by six matrix and then my B matrix is six by three, then my C matrix, 44 00:03:58,970 --> 00:04:01,010 ‫and then we'll milady's matrix with zero. 45 00:04:01,910 --> 00:04:05,300 ‫And then I start feeling this a matrix in. 46 00:04:05,900 --> 00:04:14,180 ‫So for example, here I have a then three comma one since in Python you start counting from zero then 47 00:04:14,180 --> 00:04:18,860 ‫actually is the fourth row and then second column. 48 00:04:19,760 --> 00:04:22,300 ‫So let's see fourth row and second column. 49 00:04:22,670 --> 00:04:23,540 ‫So zero. 50 00:04:23,540 --> 00:04:25,230 ‫One, two, three. 51 00:04:25,370 --> 00:04:27,920 ‫So fourth row and then second column. 52 00:04:28,580 --> 00:04:31,190 ‫And then zero and one. 53 00:04:31,850 --> 00:04:33,800 ‫So it's this element here. 54 00:04:34,550 --> 00:04:36,910 ‫So is this one a three one. 55 00:04:37,610 --> 00:04:40,640 ‫And then if you look at it then a three one. 56 00:04:41,510 --> 00:04:45,560 ‫It's this expression here, minus one at all times. 57 00:04:46,490 --> 00:04:55,250 ‫The mass moment of inertia of the propeller divided by the mass movement of inertia of the drone about 58 00:04:55,250 --> 00:04:56,920 ‫the body frame y axis. 59 00:04:58,070 --> 00:04:59,870 ‫So this expression here. 60 00:05:00,810 --> 00:05:09,540 ‫It's this one here, and so I fill in the B matrices in the same way, you see, these are the relevant 61 00:05:09,540 --> 00:05:10,460 ‫terms here. 62 00:05:11,280 --> 00:05:15,870 ‫And then I put these relevant terms in the right position in my B matrix. 63 00:05:16,500 --> 00:05:21,720 ‫And, well, the C matrix mainly consists of the zeros and then you have three ones. 64 00:05:21,720 --> 00:05:31,110 ‫They're there to extract some states and say that they are outputs and then, well, the The Matrix 65 00:05:31,110 --> 00:05:33,500 ‫was a three by three zero matrix. 66 00:05:34,380 --> 00:05:43,380 ‫And remember that these things, they were continues matrices, but we needed discrete matrices in order 67 00:05:43,380 --> 00:05:46,460 ‫to start working with the NPC controller. 68 00:05:47,040 --> 00:05:49,500 ‫And so we used the forward order method. 69 00:05:50,190 --> 00:05:53,080 ‫And so the democratization happens here. 70 00:05:53,970 --> 00:06:04,590 ‫So once I have my A, B, C and D matrices, then I make them discrete here and then I take my discrete 71 00:06:04,590 --> 00:06:11,540 ‫A, B, C and D matrices along with other variables that I needed to update in this function. 72 00:06:12,300 --> 00:06:14,620 ‫And then I give them to the main file. 73 00:06:15,360 --> 00:06:17,480 ‫So they're here, you see. 74 00:06:18,390 --> 00:06:26,040 ‫And then this one here, this is where I augment my system, if you remember. 75 00:06:26,040 --> 00:06:30,330 ‫Then augmenting meant that you expanded your system. 76 00:06:31,200 --> 00:06:39,120 ‫These were your states here, and then you also took your control inputs from one sample time before, 77 00:06:39,150 --> 00:06:41,670 ‫and you also declared them as states. 78 00:06:42,450 --> 00:06:50,610 ‫And that's because you wanted the control input increment to be as your control input in your cost function. 79 00:06:51,330 --> 00:06:54,240 ‫And so you performed this augmentation operation. 80 00:06:55,030 --> 00:06:59,160 ‫You see, that's how we performed our augmentation operation. 81 00:06:59,170 --> 00:07:05,130 ‫So we had new matrices here, A till the B till then and C till then. 82 00:07:05,130 --> 00:07:07,110 ‫Then D till then, then was zero. 83 00:07:07,650 --> 00:07:15,090 ‫And then that became our control input delta use of K and then the original control inputs themselves. 84 00:07:15,090 --> 00:07:17,700 ‫We put them here in this state vector. 85 00:07:18,690 --> 00:07:22,390 ‫And then if you look at this row here, then this is exactly what I do. 86 00:07:22,620 --> 00:07:27,630 ‫So this is my augmented state vector, this is my original state vector. 87 00:07:27,630 --> 00:07:36,810 ‫And then I take my original control inputs and then I concatenate them, I make them one vector and 88 00:07:36,810 --> 00:07:44,460 ‫then here I create this R vector that pretty much looks like this fire, rather then theta ref, then 89 00:07:44,850 --> 00:07:52,530 ‫ref, then next fire up until the end of how long your horizon period is.