1 00:00:00,560 --> 00:00:01,310 ‫Welcome back. 2 00:00:01,970 --> 00:00:09,140 ‫And so now that we have our control inputs, we have our new one that came from the position controller 3 00:00:09,540 --> 00:00:14,590 ‫and then now we have our YouTube, Q3 and Q4. 4 00:00:15,470 --> 00:00:23,510 ‫And now what we need to do, we need to find our omega's, our angular velocities for our propeller's. 5 00:00:24,530 --> 00:00:33,350 ‫If you remember then, these were the relationships between you want you to Q3 and Q4 and then Omega 6 00:00:33,350 --> 00:00:40,250 ‫one, omega to omega three and Omega four, and then what we did, we divided. 7 00:00:40,250 --> 00:00:47,630 ‫You want you to use three or four by these concerns here, the trust factor, the trust factor times 8 00:00:47,630 --> 00:00:51,280 ‫the length here as well, and then the drag factor. 9 00:00:51,890 --> 00:00:58,310 ‫And so after having done that, we put it in this vector and then we called the new star. 10 00:00:59,150 --> 00:01:01,940 ‫And in the code, that's this part here. 11 00:01:02,510 --> 00:01:12,020 ‫You see here I perform the divisions and then here I put it in the vector, then this one here, that's 12 00:01:12,020 --> 00:01:13,100 ‫this matrix. 13 00:01:13,970 --> 00:01:20,930 ‫So all these operations where I first define a four by four zero matrix and then I assign some ones 14 00:01:20,930 --> 00:01:23,540 ‫and minus ones in certain elements. 15 00:01:24,350 --> 00:01:27,650 ‫I'm doing that in order to create this matrix. 16 00:01:28,580 --> 00:01:35,470 ‫Then the next step was to find the inverse of this matrix, which is this one here. 17 00:01:36,230 --> 00:01:44,560 ‫And then once I had my inverse matrix, then I could find my Omega squared like this with this equation. 18 00:01:45,440 --> 00:01:50,420 ‫And so it looks like this here and in the code. 19 00:01:50,600 --> 00:01:52,210 ‫That's this part here. 20 00:01:52,940 --> 00:02:01,610 ‫And then once I have my omega squared, then I can take a square root of them and then I get my omega 21 00:02:01,610 --> 00:02:04,580 ‫one, omega two, omega three and Omega four. 22 00:02:05,150 --> 00:02:10,850 ‫And then in order to find the total omega, then we use this formula for that. 23 00:02:11,540 --> 00:02:15,800 ‫So Omega one minus Omega two plus omega three minus omega four. 24 00:02:15,950 --> 00:02:22,850 ‫Remember that you have had your minus signs here because Omega two and Omega four, they had negative 25 00:02:22,850 --> 00:02:23,480 ‫rotation. 26 00:02:24,260 --> 00:02:26,980 ‫And in the code it's this one here. 27 00:02:27,410 --> 00:02:35,990 ‫So you see I first check if at least one of them is less than zero, that means that there is no real 28 00:02:35,990 --> 00:02:37,190 ‫solution for that. 29 00:02:37,920 --> 00:02:46,550 ‫However, if all of them are bigger than zero, then I use these equations here and then I have my omega 30 00:02:46,550 --> 00:02:49,100 ‫one, omega two, omega three and Omega four. 31 00:02:49,790 --> 00:02:55,250 ‫And then I put them all here into this variable. 32 00:02:55,250 --> 00:03:00,800 ‫So I collect them all throughout the simulation in this matrix. 33 00:03:01,340 --> 00:03:07,340 ‫And then I can use this vector later to graph my Omega's as a function of time. 34 00:03:08,030 --> 00:03:17,870 ‫And then here I compute my Omega total, you see with this formula here and then I enter into my last 35 00:03:17,870 --> 00:03:20,060 ‫function in the support file. 36 00:03:20,930 --> 00:03:29,110 ‫So now I'm here planned integration and now I'm entering into this function, open loop new states. 37 00:03:30,050 --> 00:03:33,740 ‫So let's see how it looks like it starts here. 38 00:03:34,520 --> 00:03:40,970 ‫And then I take in the existing states, then my omega total. 39 00:03:42,020 --> 00:03:43,460 ‫There's this one here. 40 00:03:43,610 --> 00:03:44,920 ‫This is my omega total. 41 00:03:45,860 --> 00:03:55,010 ‫Then I also take in my existing you one, you two, you three and you for my latest control inputs from 42 00:03:55,010 --> 00:03:56,060 ‫the init function. 43 00:03:56,360 --> 00:03:58,580 ‫I take the constants that I need. 44 00:03:59,450 --> 00:04:00,980 ‫Then remember the states. 45 00:04:00,980 --> 00:04:03,950 ‫It's a twelfth element vector. 46 00:04:04,220 --> 00:04:10,970 ‫So I take all my 12 states out of this vector right here you see. 47 00:04:11,960 --> 00:04:19,910 ‫I go from U v, w, eqr to X, Y, Z and five feet up sigh So that's this block here. 48 00:04:20,840 --> 00:04:25,940 ‫These three lines are for the animation purposes to make the movie smoother. 49 00:04:26,840 --> 00:04:35,190 ‫Then in case I want to use the drag force version, then I also take some of the values from the net 50 00:04:35,210 --> 00:04:37,310 ‫function related to drag. 51 00:04:38,120 --> 00:04:44,720 ‫And remember that in this course we used wrong Akutan method to integrate our plans. 52 00:04:45,720 --> 00:04:56,070 ‫So this here, this was like our summary for Runga Kutta, this schematics showed us in a nutshell how 53 00:04:56,070 --> 00:05:05,130 ‫the procedure worked, how you calculate it for different slopes, and then you assigned some weights 54 00:05:05,130 --> 00:05:14,490 ‫to them and then you had your weighted average slope and then you use that in this integration. 55 00:05:15,360 --> 00:05:20,300 ‫So this entire procedure, that's this entire block here. 56 00:05:21,000 --> 00:05:24,090 ‫So this entire code, that's wrong. 57 00:05:24,140 --> 00:05:27,800 ‫Kutta, up until here, end of Runga Cutter method. 58 00:05:28,440 --> 00:05:38,370 ‫So here I define some of the variables and then I say that they have to be my origin variables, meaning 59 00:05:38,370 --> 00:05:48,540 ‫that these are the variables at K, not K plus one and not in the middle between K and K plus one. 60 00:05:49,530 --> 00:05:51,300 ‫But these variables here. 61 00:05:52,260 --> 00:06:00,030 ‫They are these variables here, you see, except this one, that's where you are right now, that's 62 00:06:00,030 --> 00:06:01,310 ‫your present state. 63 00:06:02,100 --> 00:06:09,600 ‫And then I call them like this, for example, w underscore or so it's like Origin, then I will need 64 00:06:09,600 --> 00:06:16,140 ‫this variable to be equal to to here I choose if I want the drag force or not. 65 00:06:17,010 --> 00:06:19,870 ‫These are my state's basic equations here. 66 00:06:20,730 --> 00:06:23,310 ‫This is where I compute my slope's. 67 00:06:24,090 --> 00:06:33,150 ‫And since I have to compute for then all that is in this for loop you see for J in range from zero to 68 00:06:33,150 --> 00:06:33,720 ‫four. 69 00:06:33,870 --> 00:06:36,720 ‫So J would be zero one, two and three. 70 00:06:37,410 --> 00:06:45,300 ‫Python will not count for here four here does not mean that J has to go to four for here means that 71 00:06:45,840 --> 00:06:49,530 ‫you need to have four iterations and you start counting from zero. 72 00:06:49,540 --> 00:06:52,230 ‫So it's going to be zero, one, two and three. 73 00:06:53,080 --> 00:06:55,230 ‫And here I have my rotation matrix. 74 00:06:56,250 --> 00:07:03,150 ‫Then here I compute my latest x y and see that using the latest you the end W. 75 00:07:04,120 --> 00:07:11,590 ‫Then I do the same thing with the attitude values, so I have my transfer matrix here and then I use 76 00:07:11,590 --> 00:07:21,560 ‫that transfer matrix to take my latest, pick your values and convert them into my latest FIDA theatergoer, 77 00:07:21,570 --> 00:07:23,220 ‫then decide that values. 78 00:07:24,190 --> 00:07:30,940 ‫And then remember, in Guta, we had to compute for Slope's and so I save them here. 79 00:07:31,750 --> 00:07:36,830 ‫My first slope's, they are saved here when J equals zero. 80 00:07:37,810 --> 00:07:48,190 ‫So you can imagine that these are my state base equations and then I take my newest states that I get 81 00:07:48,190 --> 00:07:49,630 ‫from here. 82 00:07:50,350 --> 00:07:54,670 ‫And then I put them in this equation and then I get my first slope's. 83 00:07:55,390 --> 00:07:59,500 ‫You write V that W dot and then P that Q Dot and our dot. 84 00:08:00,190 --> 00:08:13,170 ‫And then I use the rotation and transfer matrices in order to obtain my X, Y and Z dot and then find 85 00:08:13,400 --> 00:08:15,030 ‫Citadel, then decide that. 86 00:08:15,940 --> 00:08:22,780 ‫And so these are my 12 slope's that are the first slope's and then I save them here. 87 00:08:23,760 --> 00:08:27,660 ‫Then I skip the other blocks because it's an if statement. 88 00:08:28,740 --> 00:08:31,120 ‫Then I entered this blog here. 89 00:08:31,290 --> 00:08:39,570 ‫That's when I compute my new status, but remember, when I compute my new status in the first iteration, 90 00:08:39,580 --> 00:08:44,280 ‫then I don't go from K to K plus one. 91 00:08:45,270 --> 00:08:53,940 ‫I compute my new status in the middle, right in the middle between K and K plus one. 92 00:08:54,660 --> 00:09:03,240 ‫And that's why in this equation I say you equals USIP or which is my present state. 93 00:09:03,480 --> 00:09:15,750 ‫Plus you dot my first slope times the sample time interval, which is zero point one seconds divided 94 00:09:15,750 --> 00:09:23,380 ‫by this RTS suppos and this time suppose that's this one here. 95 00:09:24,240 --> 00:09:31,860 ‫So that means that instead of zero point one seconds I will have a zero point one divided by two. 96 00:09:31,860 --> 00:09:35,370 ‫So I will have zero point zero five seconds here. 97 00:09:36,430 --> 00:09:41,920 ‫Then I will compute my new status here in the middle between K and K plus one. 98 00:09:42,820 --> 00:09:51,700 ‫And that's how I compute my all new 12 states and then I skip this block because I've already been in 99 00:09:51,700 --> 00:09:57,120 ‫this block here and then I go to the beginning of my loop. 100 00:09:57,970 --> 00:10:02,170 ‫Now, my J equals one, not zero anymore, but one. 101 00:10:02,920 --> 00:10:10,390 ‫I take my new states that I have just computed, and then I'm going to put them here in this equation 102 00:10:10,780 --> 00:10:13,930 ‫and then I'm going to get my second slope's. 103 00:10:14,740 --> 00:10:21,100 ‫And then thanks to my rotation and transfer matrices, I'm going to be able to get my new X, Y and 104 00:10:21,100 --> 00:10:24,640 ‫Z dots and then Fifita and Dots. 105 00:10:25,450 --> 00:10:30,810 ‫And then I'm going to enter into this block here because my G equals one now. 106 00:10:31,690 --> 00:10:38,740 ‫And in this block I'm going to save my second slope's, then I'm going to skip the other blocks. 107 00:10:39,640 --> 00:10:42,430 ‫Then I will again enter this block here. 108 00:10:43,210 --> 00:10:47,980 ‫I'm going to compute my new states, but now using my second slope. 109 00:10:48,760 --> 00:10:57,250 ‫But still this person needs to be to because these new state values, they will be between K and K plus 110 00:10:57,250 --> 00:10:57,670 ‫one. 111 00:10:58,420 --> 00:11:06,340 ‫And then again I will skip the other block, then I will go back to the beginning of my loop and now 112 00:11:06,340 --> 00:11:07,630 ‫my G equals two. 113 00:11:08,530 --> 00:11:13,420 ‫I have my new state value so I can put them here and compute my third slope's. 114 00:11:14,350 --> 00:11:21,130 ‫And thanks to the rotation and transfer matrices, I can get my new slope's in the inertia frame as 115 00:11:21,130 --> 00:11:21,540 ‫well. 116 00:11:22,580 --> 00:11:31,250 ‫So I will have my 12 third slope's, then I check, OK, JQ zero, no, I skipped this block. 117 00:11:31,250 --> 00:11:32,240 ‫Jake was one. 118 00:11:32,240 --> 00:11:34,510 ‫I skipped this block, Jake was two. 119 00:11:34,670 --> 00:11:43,610 ‫I enter here, so I save my third slope's here and then I make my two supposed one. 120 00:11:44,610 --> 00:11:45,210 ‫Why? 121 00:11:46,050 --> 00:11:54,870 ‫Well, that's because since they now enter in this block again and then I want to compute my new state 122 00:11:54,870 --> 00:11:56,630 ‫values using the third slope. 123 00:11:57,450 --> 00:12:05,340 ‫But remember, when you have a third slope, then you want to go from K to K plus one. 124 00:12:06,090 --> 00:12:13,500 ‫So now when you use the third slope, you want your new state values to be computed at K plus one not 125 00:12:13,500 --> 00:12:16,290 ‫in the middle, but at K plus one. 126 00:12:17,400 --> 00:12:20,640 ‫So not here anymore, but literally here. 127 00:12:21,570 --> 00:12:29,670 ‫And so now I shouldn't divide my ties by two anymore, I should divide it by one because then this thing 128 00:12:29,670 --> 00:12:31,980 ‫becomes zero point one seconds. 129 00:12:33,000 --> 00:12:38,850 ‫And then I compute my new state value set, Kate plus one, I skipped this block again. 130 00:12:39,870 --> 00:12:46,320 ‫I go back to my loop and now J equals three, the last loop. 131 00:12:47,520 --> 00:12:56,640 ‫Using my existing state values to compute my fourth slope's in the body frame and also in the inertial 132 00:12:56,640 --> 00:13:05,100 ‫frame, I check the JS Jake zero no G equals one no equals two no else. 133 00:13:05,760 --> 00:13:06,350 ‫Yes. 134 00:13:07,230 --> 00:13:12,630 ‫So I enter into this block and that's where I save my fourth slope's. 135 00:13:13,530 --> 00:13:17,810 ‫So since I have four slope's now, then I can stop with the process. 136 00:13:18,570 --> 00:13:21,630 ‫I'm not going to enter this block any more. 137 00:13:21,690 --> 00:13:28,080 ‫I'm going to skip it because now my Jake was three, so I will skip this block because I would only 138 00:13:28,080 --> 00:13:35,700 ‫enter this block if Jake was a number that is less than three since my Jake was three. 139 00:13:35,700 --> 00:13:40,760 ‫Then I will enter this block here in this block here. 140 00:13:41,670 --> 00:13:43,560 ‫Well, that's this one here. 141 00:13:43,890 --> 00:13:46,590 ‫It's this equation, this entire equation. 142 00:13:47,520 --> 00:13:53,070 ‫So you see, if you look at this one here, then this is your weighted average slope. 143 00:13:54,030 --> 00:13:58,290 ‫You see one over six times and then your four slopes. 144 00:13:58,740 --> 00:14:06,750 ‫And then for the second and third slope, you put two here and here because they have stronger weights 145 00:14:07,620 --> 00:14:16,410 ‫and then you take your original state value plus the weighted average slope times. 146 00:14:16,870 --> 00:14:17,460 ‫Yes. 147 00:14:18,300 --> 00:14:22,570 ‫And then you get your states that leave the plant. 148 00:14:23,490 --> 00:14:27,300 ‫So these states here, they leave the plant and that's it. 149 00:14:27,300 --> 00:14:34,410 ‫We're done with that for loop and then this one here, that's for the animation purposes. 150 00:14:34,410 --> 00:14:39,090 ‫The same thing here, again, just to make the movie smoother. 151 00:14:39,900 --> 00:14:42,240 ‫So that's the end of the wrong method. 152 00:14:43,200 --> 00:14:46,080 ‫Then these states get from here. 153 00:14:46,290 --> 00:14:50,700 ‫I'm going to store them in this new states vector like this. 154 00:14:51,600 --> 00:14:59,760 ‫And then this new states vector along with states underscore unny and you underscore Arny, they will 155 00:14:59,760 --> 00:15:01,080 ‫go to the main file. 156 00:15:02,010 --> 00:15:04,050 ‫And so in the main file, I get them here. 157 00:15:04,050 --> 00:15:07,950 ‫States, states, underscore and underscore UNY. 158 00:15:08,910 --> 00:15:16,860 ‫And then what I do, I take my state's total matrix and then I add my new states vector to the states 159 00:15:16,860 --> 00:15:25,830 ‫total matrix, you see, I put it here, I concatenated that's how I keep track of all my states throughout 160 00:15:25,830 --> 00:15:26,370 ‫time. 161 00:15:27,180 --> 00:15:33,120 ‫And then I do the same thing with states total underscore scrutiny and Yuto underscore Arnie. 162 00:15:33,990 --> 00:15:36,900 ‫And in fact, that's the end of the simulation. 163 00:15:37,500 --> 00:15:42,240 ‫Everything that follows next is about animation and plotting. 164 00:15:43,250 --> 00:15:50,360 ‫But control engineering ends here because now what happens next is that you go back to the beginning 165 00:15:50,360 --> 00:15:55,790 ‫of your inner loop, which is here, and then you run the entire loop again. 166 00:15:56,240 --> 00:15:57,320 ‫And then again. 167 00:15:57,680 --> 00:15:58,700 ‫And then again. 168 00:15:59,750 --> 00:16:08,900 ‫And when you have run of four times, then you go to the beginning of your outer loop and then you run 169 00:16:08,900 --> 00:16:17,150 ‫your outlook again up until here and once you have run your outlook, then again you will run your inner 170 00:16:17,150 --> 00:16:18,280 ‫loop four times. 171 00:16:19,070 --> 00:16:24,800 ‫And so all that ends when you reach your time equals one hundred seconds.