‫Welcome back.
‫So now we're going to start with the inner loop that starts from here.
‫So our position control gave us our fire.
‫Our thoughts are and you won, you won, went straight into the plant.
‫But then by ah and Sittar went into the NPC controller along with ah, that came from the planner.
‫And so all that will happen here inside this inner for loop.
‫And then at the very beginning of this inner for loop, we're going to go into this function here,
‫LPT continuous discrete function.
‫And so it's going to happen here.
‫You see LPT continuous discrete, it's going to take in the state values and then also the omegle values,
‫the angular velocities of the propeller.
‫And in return you will get the discrete A, B, C and D matrices and also you will get the updated X,
‫Y and Z dot values here.
‫And the same thing with Phi Phi Darsey tying the seat on the UN and the sign side that.
‫So let's see how it looks like.
‫So this function starts from here and then I'm going to take the initial constants that I need from
‫the net function.
‫I'm also going to take the states that I need at this point.
‫I don't need X, Y and Z states in this function.
‫So that's why you see, I go from five to nine.
‫I don't need to extract the X, Y and Z states.
‫And then here again, I have my rotation matrix and then I take my U VW variables and then I compute
‫my X, Y and Z dot.
‫Now, in reality, you don't really need to perform this operation here because this operation will
‫also be performed in the plant.
‫However, I simply built my code in such a way that at some point in the middle of the inner loop,
‫I want to record my velocity values so my X, Y and Z dot so I will not accumulate them in the middle
‫of my inner loop.
‫And that's why in order to get the latest values, I update my X, Y and Z dot in this function and
‫then this LPT function will give me the updated Eckstut without incident and then I can accumulate them
‫all in this velocity X, Y, Z, total vector.
‫And now back to our OPV function.
‫And what I do need to do for sure is that I need to take this transfer matrix.
‫You see this T matrix and I define it here.
‫This is now the transfer matrix or not the rotation matrix, but the transfer matrix that we had derived
‫in Section two.
‫And then I take my angular velocity in the body frame, then I multiply that transfer matrix by my peak.
‫You are variables and then I get my fi that theta that inside that.
‫And now this entire block here, up until here, this entire block, this is to create these matrices
‫here.
‫All right.
‫So you see this is your a matrix here, which is a six by six matrix.
‫And so I define it here, six by six matrix and then my B matrix is six by three, then my C matrix,
‫and then we'll milady's matrix with zero.
‫And then I start feeling this a matrix in.
‫So for example, here I have a then three comma one since in Python you start counting from zero then
‫actually is the fourth row and then second column.
‫So let's see fourth row and second column.
‫So zero.
‫One, two, three.
‫So fourth row and then second column.
‫And then zero and one.
‫So it's this element here.
‫So is this one a three one.
‫And then if you look at it then a three one.
‫It's this expression here, minus one at all times.
‫The mass moment of inertia of the propeller divided by the mass movement of inertia of the drone about
‫the body frame y axis.
‫So this expression here.
‫It's this one here, and so I fill in the B matrices in the same way, you see, these are the relevant
‫terms here.
‫And then I put these relevant terms in the right position in my B matrix.
‫And, well, the C matrix mainly consists of the zeros and then you have three ones.
‫They're there to extract some states and say that they are outputs and then, well, the The Matrix
‫was a three by three zero matrix.
‫And remember that these things, they were continues matrices, but we needed discrete matrices in order
‫to start working with the NPC controller.
‫And so we used the forward order method.
‫And so the democratization happens here.
‫So once I have my A, B, C and D matrices, then I make them discrete here and then I take my discrete
‫A, B, C and D matrices along with other variables that I needed to update in this function.
‫And then I give them to the main file.
‫So they're here, you see.
‫And then this one here, this is where I augment my system, if you remember.
‫Then augmenting meant that you expanded your system.
‫These were your states here, and then you also took your control inputs from one sample time before,
‫and you also declared them as states.
‫And that's because you wanted the control input increment to be as your control input in your cost function.
‫And so you performed this augmentation operation.
‫You see, that's how we performed our augmentation operation.
‫So we had new matrices here, A till the B till then and C till then.
‫Then D till then, then was zero.
‫And then that became our control input delta use of K and then the original control inputs themselves.
‫We put them here in this state vector.
‫And then if you look at this row here, then this is exactly what I do.
‫So this is my augmented state vector, this is my original state vector.
‫And then I take my original control inputs and then I concatenate them, I make them one vector and
‫then here I create this R vector that pretty much looks like this fire, rather then theta ref, then
‫ref, then next fire up until the end of how long your horizon period is.