MPC and MHE implementation in Matlab using Casadi | Part 1

all right so yeah I think we can start now so good afternoon everyone and welcome to our first seminar with the title optimization based solution for control and state estimation in dynamical system and today we will be looking as at the mobile robots as an example of a dynamical system for those who don't know me my name is Mohammed and I'm a postdoc fellow and Mme here so I divided the presentation over three parts and the first part I will be talking a little bit about optimization just a brief introduction to optimization and the kind of optimization problems we are after today I'll give also a couple of examples on like very simple optimization problems and how to solve them using the software we're going to use today which is called Casati then in the second part I will talk about model predictive control which is an optimization based technique for control I'll talk about its problem formulation and then how to implement it for mobile robots as an example for a dynamical system using of course Kesari and in the last part i'll be talking about moving horizon estimation which is a state estimation technique that is based on optimization and again we're going to apply image II for a mobile robot and it turns out that we will recycle actually most of the things we will learn in the implementation of MPC and the implementation of mhe right so since we are talking about optimization based solution solutions for control and state estimation so why do we do optimization in general so generally we do optimization because it has many applications in business for example for resources allocations and logistics also in science for fitting system models to to data in machine learning for example you get some training data and you would like to fit some model to this training data and also in engineering for designing of technical systems like designing machine parts in mechanical systems or probably electronic parts in digital systems and any optimization problems actually is characterized by three main elements or components the first one being an objective function that we would like to either minimize or maximize so for example if we're looking at a curve fitting problem that we want to solve by optimization we would like to minimize the difference between the model we're using the output of the model and the training data right that's something we want to minimize and in a finance problem if we would like to if we have an objective to maximize maximize profit so this means that we have an objective function that we would like to maximize right so as you can see the optimization function has a variable called W and we call this variable the decision variable or sometimes the optimization variable this is the variable that when we manipulate we basically manipulate the value of the objective function right sometimes also we can have constrains on the optimization variable or a function of it these constraints can be equality constraints or inequality constraints right by the way during the presentation today feel free to stop me at any time if you have a question right ok so then what would be the mathematical formulation for an optimization problem it turns out that there are different mathematical formulation for optimization problems but the one that we are after today and it's actually a generic form of optimization problems called nonlinear programming problems or NLP and in a nonlinear programming problem this is how we write down our optimization problem we simply say that we would like to minimize is an objective by using our by manipulating the optimization variable w while we have some inequality constraints and equity and equality constraints that we would like to satisfy right so this is how the non-linear programming problem looks like in general and we also have the assumption that this function Phi D 1 and G 2 are usually assumed to be differentiable functions actually NLP is a general class of optimization problems where in some cases can be reduced to other special cases for example if the functions Phi G 1 and G 2 are affine and affine means that there are linear combinations of the optimization variable the problem turns out to be a linear programming problem and in the case that the G 1 and G 2 are fine but the objective function is quadratic the problem turns out to be a quadratic programming problem so linear programming a quadratic prime programming our sub problems or are already covered if we really know how to solve a nonlinear programming problem or an NLP right there could be other variants as well right so knowing how to solve an LP that's good to know are actually good to practice and turns out that this is enough to implement MPC and mhe at least for its the examples in this lecture all right so we said that we sometimes want to minimize an objective or maximize an objective so in case of minimization this is how our optimization problem is formulated and in case of maximization it turns out that maximization is equivalent to minimizing only the negative of the function so if we know how to minimize then we know already how to maximize right so we have an objective and we want to maximize it we simply minimize it's negative right all right so that's one thing the second thing is sometimes we could have a global minimum for a certain objective function or a local minimum so for example here we have a sinusoidal function that is enveloped by an exponentially increasing function so that's why we have this increasing oscillations right and for this minimization problem we would like our transition variable to be between this range zero and four pi so in this case if we look at the optimization problem we will see that the absolute minimum or in other words global minimum is actually at this point down here but at the same time we have multiple of local minimum so in this case we have three local minimums and one of them is actually a global minimum right so today we're actually focusing on finding the local minimum or the local minimum solution for the optimization problem because solving for global optimization is in general not straightforward and time consuming and if we talk about control and state estimation real time applicability is is a question for us right and turns out for our purposes in this workshop having a local solution is enough or is actually sufficient all right so now we know that this is how the optimization problem looks like so what happens if after we solve it what is the solution so the solution we can refer to as W star so the value of W at which the objective function is minimum so mathematically we can write this as W star equals to arrack men or the minimum argument for the minimum for the objective function Phi so basically if we take this w star and plug it back to the optimization objective or the objective function we will get the minimum of that objective function right in general we are interested in W star rather than the value of the objective at this w star right so let's go ahead and start with a very simple example so say for example that we would like to find the minimum or the solution for the minimum for this objective function it's a quadratic function in W so we can write this problem as a non linear programming problem in which we would like to minimize the this objective using or by manipulating the optimization variable w right so so far we don't have any constraints so this problem is unconstrained optimization problem and actually if you look at W and the objective function so the objective function is quadratic in the optimization variable already right however we will solve it using the NLP programming in general right turns out that this function is actually or this objective is actually very simple to minimize by simply finding the gradient of the objective function equating that to zero from calculus right and finding the value of W that makes the gradient or the derivative equals to zero and this value is actually three in this case right so if we plot this objective function here is our function right and here's the minimum of the function and three is the value of W the optimization variable at which the objective function is minimum right again we are normally interested in the value of W rather than the value of Phi at the minimum at the minimum however in this problem the value of the function at the minimum is actually 4 so 4 so 3 is the value of W that minimizes the cost function at the and the minimum at this time is basically 4 just as a side note say for example that we would like to multiply the objective function as a whole by some constant let's say 10 2000 or some constant nonzero constant apparently right so what happens to this objective function so the objective function will be lifted up a little bit or more we don't care when we solve the same this optimization problem we will get the same minimizer but probably a different value of the of the function at that minimizer right so if you scale your objective function by just a constant this doesn't change the minimizer but it will change just the value of the cost function at the at the minimizer right so this is just as a side note right ok so now let's try to solve this problem using one of the software's that I personally like and I personally used in my PhD and I actually recommend it as a tool for those who would like to work on optimization problems in general so the software is called Cassidy and I left up there the website which takes you directly to where to download it from and get things started right and Kesari they have a very nice slogan which says build efficient optimal control software with minimal effort and that's what it actually does right and we will see how in this lecture right so Cassidy has a general scope of numerical optimization and we can use Cassidy to facilitate the solution of nonlinear programming problems the NLP is we just talked about actually it facilitates the solution but not solving the solution so it depends on external solvers and it turns out that if you just go ahead go to this website download the package the Cassidy package it comes with some pre-installed installers sorry pre-installed solvers for you and it turns out that these solvers are basically enough for our purposes in this workshop right it's free and open source that's that's a plus and it's that's a project that started about nine years ago and it's most recent version was released August last year Cassidy has four problems to solve which are quadratic programming nonlinear programming route finding problems and initial value problems for all the ease or ordinary differential equations and DS or differential algebraic equations right we are using it today basically for nonlinear programming problems so let's go back to our problem that we would like to formulate in Cassidy so the installation of Cassidy is very simply go to the Cassidy website you download the installation for you change the MATLAB path to that folder and bam you're there right so let's see how to solve this optimization problem in Cassidy so I'll start by writing directly my code in MATLAB so as you can see up there in the first line I'm just adding the path to where the Cassidy package is on my computer and then I'm importing the workspace Cassidy right then I'm starting to define a variable calling it X in this case and X is a symbolic variable with this play name equals to or that that is a W writes OS X this function here as X as X is that is a basic data type in Cassidy that represents matrices with symbolic entries right so whenever I wanted to declare a symbolic entry I simply use the sx dot same data type right okay then we define our objective the variable option is our objective an objective equals to our basically the objective which we saw in the previous slide X square which is W square plus 6 minus 6x plus 13 right so if we go to the command window in MATLAB and write X so this will print for us the value that is inside X which is our symbol W right and if we go ahead and type objective in the command window of MATLAB we will get this symbolic expression of the objective so far we have the objective expressed as as a symbolic expression so this is how Kesari sees it so it sees W square as sq of W which is squared of W minus 6w + 13 right this is the symbolic representation that Kesari is based on right then we define couple of variables we're not going to use them for this problem but we'll use them later today like to empty variables GNP G's is actually our constraints vector it's empty for now and P is the optimization problem parameter which is again empty today so not today I mean for for this particular problem okay here I'm just three defining or I'm defining a new variable optimization variable which equals to basically X like my optimization we're just giving it a different variable name right and then I'm defining the structure this is how to you define a structure in MATLAB with four fields these fields are F X G and P and this structure is called NLP props like stands for nonlinear programming problem right and it has four fields that these fields are my objective F right and my optimization variable X right and the constraint G and the parameters P which are empty for now so if you go to the command window and right NLP probe we will see it as a structure with the objective as a Casati data type the same for X and 2 empty variable GNP right so if you look at F here so it says 1x1 kesariya because it's a scalar right so it's a 1 by 1 matrix right and it's of casada type or sx data type okay so far so good so now we we are keep like we will keep formulating our optimization problem so we're defining a new structure in MATLAB will call it up stands for options right and ODS has some optimization tolerances for our optimization problem and in this case we're using an assault for cold IP opt or interior point solver this is a standard optimization technique and this solver is actually again open source and we would like to just use these you know optimization options like the number of iterations we would like to have the acceptable tolerance for optimization and the acceptable change and the objective function right so you can find more details about the interior point optimizer for nonlinear programming problems in the IP optima Newell it turns out that these functions are actually good for our purposes again today right so now we will see also something new in Casati which is the NLP Soul class so here we're creating an object called solver and and the solver is basically an object of the class and LP solve that's an a class and Casati right and and when we define this object we define the solver that we would like to use its IP opt in this case we pass the nonlinear programming problem that we just developed in the previous slide which has the four entries the objective function the optimization variables G and P right and the options for optimization we would like to use in this problem okay right right finally we apply the optimization or we start the solver so in Cassidy this is how the optimization problem is defined so we have an objective function that is function of the optimization variable X and some optional parameters P not used so far right we have some lower bound and upper bounds on the optimization variable that's for the cases when we have a constraint optimization would like for example of optimization variable to be within a certain limit so in controls optimization variable could be the control action and in many cases we would like this control action to be between Satori some saturation limits right so these are the lower bounds and our bounds on the optimization variable X and optionally we could also have some constraints lower bounds and upper bounds on functions of this optimization variables right and optionally parameters right so we are going to define some arguments for the optimization problem again in the form of structure so so far we have three structures right an LP probe structure and opt structure and now the arcs or arguments structure right so in the argument structure and the arcs structure we define lower bounds and upper bounds on the optimization variable so since this optimization variable is unconstrained in this very first problem so the lower K is the lower bound is minus infinity and the upper bound is infinity unconstrained right the same for the for the constraints vector G so we don't have constraints so minus infinity to infinity and we don't have any parameters so I'm going to define a parameter field inside the Earth's structure and I leave it empty and now I'm going to define an initialization for this optimization problem so we said that it's a numerical like IP opt is a numerical technique for solving the optimization problem so we always have to initialize with some initial guess for the optimization variable so in this case I'm going going to initialize my optimization variable with an initial guess of minus 1/2 okay right so now we're going to call the object we created solver right and we're going to pass all these parameters we're going to pass the initialization of the optimization variable lower bounds on the optimization variable upper bounds on the optimization variable ok oh thanks for him it's ok right and then lower bounds on upper bounds on the constraint G in this case we don't have any and D parameters which is an empty again right right so once we call this function we get and this variable Sol stands for solution to fields the field X and the field F the field X is actually the optimal solution sorry yeah the optimal solution for the optimization variable like the minimizer value right so in this variable X all this is going to be my solution basically right and and we get an another field so that F which is the value of the objective function at the minimum at this solution right so if we print out these values we will get the X the X which is the minimizer as three and the value of the cost function here will be four that's pretty much or exactly what we had in the previous slide when we solved it analytically right so do you have any questions so far what is what actually yeah for this for this that's actually a good question like the difference between Cassidy and the built-in optimizers in MATLAB like for example f men UNK like minimization for unconstrained problems and F men con the optimization routine for constrained problem it turns out that with help of Casati and the external solvers we can much we can get much faster solutions like it depends on the size of the problem but for like some problems that I tried both F men Khan and Cassidy for example I would say that I can get the solution 10 to 20 times faster using Cassidy yeah yeah then then then F Mancala in MATLAB yeah like the number of control variables can be arbitrary large because we're using the interior point optimization here and this one is designed actually for large-scale nonlinear optimization problems and I'll show you today an example on how large we can scale our optimization variables right okay so far so good then few remarks for this particular problem it's again single very a single optimization variable that was cleared and it was also unconstrained and in our case we had only one solution for the optimization problem so this means that local minimum is the same as the global minima had we had basically one minimum right so let's go ahead and try a second motivation example for using Casati so we're looking again at this objective function sinusoidal times exponential function with these constraints if we plot these functions we see that we have three minimums within this range from 0 to 4 pi right and if you would like to solve this using Casati so here's the code right ahead it's pretty much similar to what we did so far the only differences would be the form of the objective function here we have this of exponential function right we have the same options we had in the previous problem the same class NLP solve the same class in Kesari the same options the same even optimizer but here the only difference are the constraints on X so here we would like our optimization variable to be between these two values like to be lower bounded by 0 and upper bounded by 4 pi and this is what we put here for lower bound X and our bound X right again G we don't have any use for G's like we don't have a function of X that is bounded I mean a function of the optimization variable that is bounded so these two fields are will have the lower bound of minus infinity and upper bound of infinity which means unbounded basically or not used right okay we will initialize here by this value 1 for example we will call our solver and we will get the solution in the field X and the value of the minimum in the field there but this particular problem has actually multiple solutions multiple minimums so if we minimize by one or we initialize sorry by one we would get the solution 0 like the value of W that minimizes the cost function is zero so starting from one here the solver took me to here so if we change the initialization to fall for example it will take me to this solution and will change it to another value 10 it will take me to that solution so at all these solutions you will see that the gradient is 0 right at the solution the gradient is 0 then this solution the gradient is also C but at this solution the gradient is not 0 right but this but we took this solution because that was the minimum that respects the constraints because it's a constraint optimization problem right ok so any question for the second motivation example ok sounds good so the last motivation example we have basically some data points that we would like to fit a straight line to it right so this problem is known as linear regression and it's very widely used in machine learning for supervised learning for example so say for example like by machine learning language we have this set of training data and we would like to fit a straight line in it so the straight line will have the form y equals to MX plus C right so in order to find the variables M which is the slope of the straight line and the y-intercept which is C in this case right we would like to minimize the square of the sum of these errors here and this method is called least squared least squares method right so we would like to find the optimization variables or the values for M and C so that we have we are we are minimizing this objective here which is the this objective here we have the difference between our output Y and the predicted output from the input X right okay so if we would like to write this as a nonlinear programming problem so we would like to minimize this objective over the two optimization variables this time we have two optimization variables and the objective is actually as some as you know just a function like in the previous two examples it's a sum of functions right okay so let's go ahead and and try implementing that and in Kesari so at the first part of the code I'm just visualizing the data set that I have so what I want you to remember from this slide are these two vectors x and y which have the training data the values for X and the values for y right so now we go to the second step where we implement kesaji so we start by the regular stuff for Kesari and we define here two optimization variables to store symbolic variables which are basically my optimization variables right and now we start to create our objective so in the previous two examples objective was a simple expression but here objective will be calculated over a sum we will make the sum here by a for loop for like simplicity okay so our objective here we have a loop over the length of the vector X the number of data points we have right and for it for each step we increment that objective by the newsome sorry by the new difference between the output and the predicted output right so now you can imagine how objective looks like if we want to print it on the command window right because it's expressed as a symbolic variable so it looks like pretty involved stuff right so here is the sum of the old all the terms we have in the sum here right again it's unconstrained problem and we don't have any parameters that we want to include right so here's the optimization variable and here's my structure the first structure that has the objective function optimization variables GMP options for optimization and our object from the NLP salt class and Casati right then the constraints again unconstrained so lower bounds and upper bounds on X and G are basically unbounded right and here when we initialize we have to initialize by two values not only one value right and that's all people obvious we call the solver and we get the solution an X and X will have two fields the first term and it is going to be our M because that was the order we defined the optimization variables we defined m and and the second one was C right and the second term is going to be my see the solution for C right so in this case this will is going to be my slope and this is going to be my y-intercept right and this min value this is this is actually the value of the objective function at the minimum right okay so this is just a visualization of the result here's our straight line fitting these data point right and it turns out you would get exactly the same straight line from Excel right because it's actually this this least square method for this linear regression has a closed form solution so we could have even solved it analytically so and you get exact basically the same result in Excel right ok one more thing that I wanna would like to let you know about Casali that will be helpful through the rest of the presentation today and it's still related to the problem we just finished let's see how we define the objective for that problem we define the to optimizations variables M and C and then we define the objective inside this for loop or we created the objective inside this for loop right and you remember how the objective looked like in the command window right so what if we have two random values of M and C and we would like to find the object this objective we have two options right the first option is we take m and C and we go through the same for loop again with and we get a value of the objective in this case it will be directly a scalar value right and the second option is to use something built-in in Casati which is called the objective sorry the function object in Kasauli and this function object has many useful uses we can actually use this object function to define a function we give it a name and we specify which input it takes and which output it gives right so this is how how we define a function object in Cassidy so later when we call this objective function and we pass these two parameters two random parameters it will give us a me directly the value of the objective for these two parameters right was that clear okay all right so what I'm doing what I'm going to do here I'm going to use this function to visualize the objective of our optimization problem so think that in this optimization problem for the linear regression we had two optimization variables the slope and the y-intercept right so if we have to of transition variables this means that we still can visualize the objective and it's going to look like a surface right okay so let's try a range of values for M and the range of values for C and try all the possible combinations of these values by by by calculating the objective function at all these combinations right so when we do that we will get the surface right so what I'm doing at this for loop basically is that I'm getting all the possible combination for M and C right and I'm calling this objective function AB fun here and I'm passing these two values and it returns to me the object the scalar value of the objective at these two variables right so here I used the function object in Cassidy right so I basically calculated all these objectives like at all the combinations of M and C we calculated the objective and that's how the objective look like at convicts that's why again here by the way the local minimum is the same as global we have one minimizer alright okay if we look at this surface and we'd like to get the minimum we will get this minimum right so we're getting the minimum from x and y so we'll get this minimum and this is very close to the minimum we got from solving that problem using Cassidy of course this one is approximate because we have a course of course discretization for the values of M and C right the objective of this this slide is basically to introduce you to this function object in Cassidy right and how we we used it later here right so any question no question then okay then that's that's it for the first part of the presentation so now we go ahead and shift to the second part we will talk about the optimization based technique for control called model predictive control or MPC so what is MPC and by the way MPC is also called receiving horizon control and sometimes moving horizon control so let's look at what MPC is like by looking at this very simple single input single output system right that is in general a nonlinear system it doesn't need to be actually a linear system okay so if we look at the this system through these two graphs we have here a graph for the control action U and here a graph for the state evolution X so so far these are the control actions applied so far and this is how the V state acts evolved over the past right you will see X here continuous and you discrete because we have the assumption that through this time step the control action is piecewise constant right alright so now we are at this time K and we would like to take a new decision or a new action so that our state X eventually reaches some reference X reference or X R right so what what do we do in an MPC or model predictive control is that we find the we look in the future for n time steps right and we try to find the best or optimal sequence of control actions that we can apply or we can predict that makes the prediction for the state approaches or actually goes to the state reference right so at this decision variable we solve an optimization problem to get this hopefully optimal sequence of control actions to have the prediction of the state going to the reference right okay and then what and then we apply only the first portion of that sequence of control actions to the system so when we apply this first control action and our current state is here so we predicted that that state goes to this state right from here to there but what happens in reality shouldn't shouldn't be exactly as we predicted right so that's why in the second after after applying the first portion of the control action we do another step of MPC and we get a new control sequence and we apply the first control action in that sequence so in MPC it we have three things that we do we do production because we would like how we would like to know how this state acts evolves we do online optimization by minimizing the difference between this prediction and the reference by manipulating the control actions right and receiving rise on implementation we do this every sampling time or every time step right all right so the mathematical formulation of MPC is very similar to lqr which most of you are already familiar with so we define what we call a running costs or a stage cost where we penalize the difference between the predicted state and the reference and the difference between the control action and its reference value for this case the reference is zero that's why I'm showing this difference as this distance right so this is called the running costs right and we we are penalizing the second norm of these two differences right so that we are the error is always positive right and then we define what we call a cost function which is a summation of this running costs over the production horizon over all these systems right so we started with this different these two differences so now we're summing them up over all the production steps all the production steps right so now the MPC problem can be formulated as an optimal control problem where we would like to minimize that objective function by manipulating the optimization variables in this case you write and this minimization problem is actually subject to very important constraint which is the relation between the production of X and the control action U and it turns out to be our system model right we have another constraint on the initial value of the state and the production which is the initial value of my state right the real estate right or the actual state and I have some constraints on the control action that I should respect over the old projection horizon all right and constraints on the state itself which I also should respect for all the production steps right the last thing I would like to define here is the value function and the value function is basically the minimum of the cost function add the solution for the minimizer u right we will not use we're not going to use this value function today but it's there for your reference right so do you have any question regarding the formulation of MPC ok so let's keep going MPC is actually one of the most I would say popular controllers now can be generally applied to multiple input multiple output systems it could be for linear systems or non linear systems right it has a constant natural consideration of constraints on the state X and the control you approximately optimal solution and it's been used in the industry since the 70s requires online optimization apparently and there are some central issues in MPC first of them is the stability issue so here's our system will apply MPC what guarantee is that the closed-loop system will be stable so that's one of the NPC issues right another issue is how good its performance so you say that this is an optimal controller then how do you measure the optimality of the solution right or the optimality of the controller how long does the prediction horizon should be right and finally how to implement it numerically right and this is the main our our main scope for the talk today right all right so let's go ahead and see how we will implement MPC for a mobile robot okay so we're talking actually about a none holonomic mobile robot in this case a differential mobile robot so this robot in general has three degrees of freedom it's two like two degrees position degrees of freedom x and y and angular orientation theta right the differential drive robot or this are non holonomic robot has this kinematics model so in this kinematics model the inputs are the speeds for the right wheels and the speeds of the left wheels right this is a very simple non autonomic kinematics model R is actually the wheel radius and D is the base width like the distance between the two wheels the wheels on the left and the wheels on the right okay and if we define our control variables as V and Omega or the linear speed of the robot and the angular speed of the robot and the relation between them and the right and left speeds of the wheels is basically this equation we will end up with this kinematics model right that's for holonomic by sorry uh non holonomic mobile robots for a holonomic mobile robot we would have two V's because our robot now can move laterally so we'd have VX and V Y and Omega but since this one is a non holonomic so the robot can move linearly in one direction forward or backward right okay so here is our system we have two control inputs and three states this and this is how we look we can look at our system from input-output perspective and if we look at the control objectives we can possibly have a point stabilization control objective our robot is as at some initial positions and we would like to drive it all over to a gold position and position means pose position and orientation right and we can also have the trajectory tracking problem to the extract problem we would like to our robot to move along a time-varying trajectory right there is a third class of control problems called path following with this timing or the timing law of the trajectory is not given a priority but but it's it's it's left as a degree of freedom for the controller to decide when to be where on the on the path or on the trajectory right so here's our control loop that's the control system controller we will assume that our sensors and state estimators work fine and they feed us back with the full state X Y and theta so now we are looking at the controller which is MPC in this case right all right so now let's formulate MPC for our particular problem the mobile robot right so first of all I'll look at the system kinematics model and then I'll discretize it because we're going to do predictions right so we we should we should use the model to do prediction for the state right in this case I'm going to do predictions using the sample form of the continuous time equation of motion right I'm I use the simple Euler discretization technique so here the new state equals to previous state plus delta T or the sampling time times the previous like my right hand side of this equation of the previous state and the control actions V and Omega so you just want you to remember that this is my equation or discrete time equation the new state equals to previous state plus delta T times the right hand this right hand side okay just keep this in mind right okay and then our optimization problem or the MPC optimal control problem we will get the running costs here then the summation of it gives us the objective function right that we would like to minimize for some admissible values for u and we have these constraints the first of them being the the system model right okay sounds good so this is how the optimal control problem looked like for MPC and this how OCP looks like right and this is how a nonlinear programming problem looks like this is the standard form of an LP that we saw how to solve it using Cassidy in MATLAB right right so our objective now is to see how to transfer or how to convert or discretize more precisely my optimal control problem to a nonlinear programming problem right we actually can use several techniques single shooting discretization technique multiple shooting and collocation method so in today's lecture I'll talk about basically single shooting and multiple shooting right these are two very important concepts single shooting might be very intuitive to you multiple shooting may not but it turns out that multiple shooting is much more efficient than single shooting and you will see this by example right ok so again we're going to use the Casati right so now now we understand the the structure of the problem right we have we have basically at this optimal control problem and we would like to solve it by transforming it to a nonlinear programming problem and we know how to solve a nonlinear programming problem in Casati already right all right so let's go ahead and solve that on matlab okay so as usual we will add the Cassidy path namespace we will start defining these variables the sampling time so assembly time here equals to 0.2 and the prediction horizon length and the prediction rise in here I set it to 3 that's a fairly short prediction horizon so 3 times 0.2 this means point 6 seconds so we predict only four point six seconds right a robo diameter not used here just for visualization later and here I'm defining the maximum and the minimum margins for the control actions on V and Omega okay so the maximum we should be 0.6 at Max and minus point 6 at minimum and for Omega maximum is PI over 2 and minimum is the negative is that right all right so now we will define three symbols for our three States X Y and theta right and we will define our state's vector we will call it staples and our and the number of states is three and I'm just calculating that by MATLAB length for the states so this gives me three apparently right the same for the control actions defining two symbols for the control actions a controlled actions vector and the number of controlled actions we will use this later in our code so I'm setting up my my code first and here I'm defining the right-hand side of that equation here if you remember that equation this is my kinematics model so I'm D here I'm defining this right-hand side RHS so RHS is basically a vector the first entry is V cosine theta the second entry is V sine theta and the third one is omega right okay nice so let's define these variables as well so the first one being a function you remember the function object in Cassidy so this function we defined two inputs for it and one output which is the right-hand side so what does this function do for me when I pass the value of the state and the values of the controls to this function it gives me this right-hand side right so if we go to the MATLAB command window and we typed F dot print dimensions it will give me these outputs that this function has two inputs the first one of dimensions 3 by 1 which is my state and the second input is 2 by 1 which is my controls right and the output is 3 by 1 which is my right-hand side of the kinematics model right clear okay then we will define that vector or this matrix actually big U so u has all the optimization variables for this problem u has the control actions for all the production steps we said that here we have N equals to three right so we have two control actions here and here here and there here and there right for these three steps right so this is how the control actions matrix U looks like in MATLAB and this is how it looks like and in writing okay all right and then we will define a parameters vector I'll call it P or uppercase P and P is of dimension six has six entries the number of states times two and this parameters vector will be later used to define my initial state because every 10 PC is initialized every time step so if every time I initialize NPC I need to provide the initial state of the robot or where the robot is at and these last three parameters are the reference state for my robot right and finally I'm defining the matrix the big matrix X of s X type of course and it's a matrix that has three rows and n plus 1 columns that that's the prediction of the state right for each control action how the prediction would look like so the prediction would be stored in this matrix that's clear we will see how we will fill up these matrices now okay if you have any question feel free to ask me at any time okay right so let's go to the next slide so you remember what was in P P has three parameters right and this big X is my matrix of the state production the first column in it should be my initial value when I started predicting the trajectory of the robot I should start this prediction from some initial state right so the initial State apparently will be in the first column of X capital X right and this this state will be taken from the parameters that I will pass later to the optimizer and these parameters are stored in the vector P right so what do you expect to see now in the first column of X P 1 sorry p 0 P 1 and P 2 right okay so now let's fill up the rest the rest of the entries for X so now we filled up this vector this column right so now let's fill up the rest of the vectors okay all right what would be the next vector here the next vector or the next state equals to the previous state plus delta T times the right-hand side of the kinematics equation right and that's what we are doing here in this for loop so in this for loop we extract the previous state from X right and we extract the control from the control matrix right we pass them through our function f this function has that gives us the right-hand side right so we say that the next state equals to the previous state plus delta T or T here right times F value that's the right-hand side right so what I'm doing in this for loop is that I'm basically integrating starting through from here going to there starting from here going to there and so on until I finish up this matrix right so the initialization was from my parameters and the prediction was from my control actions and the previous state we did this recurse yeah by forward recursion to get to fill up this matrix right okay here I'm just defining a function I'm calling it FF and this function takes as an input input the control matrix and the parameters matrix and gives me the production capital X so once I know some solution for you over the whole prediction horizon and I blog it in this matrix this immediately calculates for me the production so after this for loop here if you go in and if you go in MATLAB command window on type X you will see some fancy stuff here it won't be is just simple these no it will be a symbolic combination of please use and apparently T and the right-hand side of the equation this is gonna be a pretty fancy pretty involved matrix here and it's all samples right so in order not to do this every time I want to get the prediction I'm basically defining this function here so once I know that predicted control I pass the predicted control to this function and this returns to me this the prediction for the state okay and this is how yeah this is how the prediction of the state is like so initial x0 comes here then f x0 and use u goes here and we keep doing forward recursion until we reach the end of it so you can imagine how nonlinear Li the last column in this prediction is and even for like longer prediction horizon you can like this is a function of a function of a function of a function and this is all nonlinear propagation of these control actions through my prediction okay sounds good so far so good any question okay so now let's let's go ahead and calculate our objective we said that in in calculating our objective we should define some weighting matrices Q on some weighting meter matrix matrix R for penalizing the deviation state and deviation in control so I'm here I'm just defining Q on our diagonal matrix with with these diagonal entries these matrices can be used later to tune your controller so these are like the gains in a proportional controller for example like right okay and then we will now calculate or compute our objective so we start with a zero objective and we add up because objective is a summation now right so my objective equals to the difference between the state which comes from the big matrix X that has the prediction of my states right minus the reference and we said we will store the reference and the second three elements of P so we take here P from four to six from the fourth element to the sixth element right transpose Q X or the different this difference right plus the control action we take it from our control matrix capital u right transpose our control right straightforward right okay sounds good so that's our objective so now we are now we have some constraints so the constrains vector here we will fill up this vector with some constraints so imagine that your robot is in a map and we would like our robot to stay within the margins of this map so this means that we would like to add constraints on what elements in the state vector x and y right so we define we started this g vector as an empty vector and then we will fill up this vector by entries from the prediction matrix X capital X right so the this is the capital matrix X so what do we fill up this guy with in the end we fill up it with these values here right so I always take the first entry on the second like from the first row and the second row for each column which means for each step in the production right sounds good okay so so far we defined P G and the objective right so now we can define our nonlinear programming problem structure which has the objective calculated already optimization variables G constraints and P actually the optimization variables I'm passing here has to be one vector but we defined it as a matrix as a 2 by n matrix 2 by n matrix right so we have to pass this as a vector right so what this three shape function does that's a matlab function what it does it basically takes my new matrix and converts it to one vector right I just want you to note that u 0 + u 1 u 0 is actually the linear velocity V and u 1 is the angular velocity W or Omega right then here V and that's megha that's V and that's Omega and when we put that as a vector so this be the optimization variables became V Omega V Omega V Omega and so on right so the first two are the V and Omega for the first time step and the prediction second time step and so on right we're defining here the object for the NLP soul object in Casati if we take an LP prop in the command window we will get this stuff here so it has an objective that is 1 by 1 scalar it has the optimization variables X X is 6 by 1 these are my optimization variables don't confuse X which is the optimization variable with X which is the state vector with X which is the x position of the robot so lots of X's here right ok so X here is actually the optimization variables this X right and it's six by one right and G the constraint vector we said that we have n the plus one state production right so we have eight constraints each production we have x and y so x and y 2 x 4 that's the 8 by 1 in the g here right and finally the P vector is 6 by 1 and we know the first three is my knee is my initial state and the last three are my reference state right ok so now we define the arguments it's again a structure we have lower bounds and upper bounds on G and what was in G what was indeed the x and y position of the robot over the whole prediction horizon so as we said we would like our robot to be in a map that is that has a maximum X of 2 and my and minimum X of minus 2 and the same values for y so I'm just setting the maximum and the minimum or the upper bound and lower bound for the states in x and y right and these parameters here on these two arguments here right now we're setting the maximum and the minimum for the optimization variable itself or variables right ok so we said that our optimization variables is arranged as V Omega V Omega V Omega right so the way we define the constraints here I know that the initial optimization variable is V and every second one or every odd index and that vector is my V right so I'm going to the lower bound and every odd index and this vector is basically V minimum right and every odd index and the upper bound or for the upper bound is V maximum right the same for Omega but it's every even index and here every even index I set my Omega a minimum or the upper bound and lower bound on the second optimization variable or the second control action okay that's my that's my problem now ready to be solved right okay so we will go ahead and start our simulation loop right so we're done with the formulation of the MPC problem and we did not see any number so far right it's all formulated as simples so you can imagine how how fancy for example this objective look like looks it would look like really fancy if you want to print it on the command window right so let's let's start our simulation loop I'm defining here the initial position for the robot XS some variables the reference I would like my robot to go to this state so the robot is starting from the origin and I want to drive it diagonally to 1.5 meters in X 1.5 meters in Y with the same initial orientation right so sometimes this is called parallel parking right or well forward parallel parking not exactly by reporting but yeah right I'm defining here as a matrix that will store the value of X for each time step like how the the state X evolved is worth time in my simulation loop initial time and here an initialization for the optimization problem so here in this problem how many optimization variables we have to answer this question we need to know how long the prediction horizon is and how many control actions we have so apparently we have three optimization horizon of three or prediction horizon of three and two optimization variables this makes up six optimization variables right because we have sorry to up to my control actions okay so this makes up six optimization variables so here we have we are initializing this by a matrix of n rows three in this case and two columns so this is a matrix with six inches and it's all zeros this initial just keep this initialization in mind now okay we set the Mac a maximum simulation time of twenty in this case and a counter for the loop right and couple of variables to store the predicted state and to store the control actions we will take so our simulation will be pretty much in a while loop and this while loop breaks after the simulation time ends or after reaching a certain error like a certain closeness to the reference state if my robot is close enough then I'm break and how how how close is close enough is basically this for now that's how I measure how close is the robot s to the reference right okay now remember when we define the ergs structure we always define a p field in it or a peep variable in it and P here has six terms right the first one is my initial state and the second one is my what the reference right okay and now we're defining the initialization for the optimization variable and don't confuse X again with all different X's we talked about today so X here is my optimization variable X you this X 0 don't confuse it with this X 0 so this X 0 is the initialization for the optimization variables right and we initialize the optimization variables by this new 0 here and it was a matrix and here we'll just reshaping it to be a vector as what we what we did earlier right so our MPC problem is now solved just in calling this solver call the solver with these arguments by the way what arguments need to be changed every time step in this loop apparently these two arguments every time step the initial or the current position of the robot changes were actually updated XS remains the same so I could have just updated the X 0 that's fine and I can initialize the optimization variables by a different initialization every time step and we will see how but for the very first time step we initialized the control variables or director of optimization variables by just zeros okay all right now we called our so far we called our solver so what would Sol have Sol have two fields the X field which has my optimal or my minimizer like the optimal values for the control actions to minimize this objective function I'm reshaping it to get again U as a matrix because here salted X gives me u as a vector so I'm just free shipping it to getting again as a matrix right and here F f value you if you remember this F function gives me the prediction now I know some values for you I want to calculate the production for X right you remember this function f F that we defined earlier like in the two previous slides or something right so I'm just getting the prediction caching or storing it in this three dimensional matrix so for every time step I have a prediction and a prediction is a two by two matrix so this makes up a third dimension matrix right so think of a three dimensional matrix as a book each page in a book is a two by two matrix and each page is the third dimension of this 3x3 matrix right so so think of the pages as the whole prediction every time step so every time I turn a page that's a new prediction every time I turn the page that's a new prediction right all right all right and my control I'm just storing here my control actions I'm taking just the first term and you we said that my control action would be always the first term in you so far we haven't applied this control action to my system so and we will do this in this shift function so let's see what shift function does for us turns out that shift this should function does two things the first thing is shift function we propagate the control action in our function f and calculate the new state x and this will be my new initial state for the next time step that's why we call it x0 here so initially x0 was this one so once we once we are done with one loop here x0 gets updated by propagating the control action through my system model right actually you do a second thing here which is you remember how we initialize the optimization variables here by what just zeros right but now we have an optimal solution form from a previous time step why don't we recycle it and use it as a initialization for the optimization variable for the next time step this is what we do here but we simply trim the first entry of it because we already used it so we trim the first entry and you zero and we just repeat the last entry this is our best guess about that entry right right okay so that's the shift function so after this step we apply the control action and we updated the initialization for you zero that will be used again here right we update the counter and that's it right I'm just here calling some visualization functions to see what's working and let's do some demos on the on MATLAB so here I I have the my implementation here on MATLAB so this code will be given to you I'll send an email with the link on how to get it right and just wanna bring up MATLAB on the monitor or the projector and yes so here you go yes so that's my single shooting implementation so again you can see how large is my optimization horizon so it's just three so the optimization horizon is just three steps so let's run this so these are my iterations that count down here and this is how the robot approaches the reference and this is how the prediction looks like the prediction is actually very short right so the robot could not manage to go exactly to where I would like it to be so one of the things that I can think of now is just increasing the prediction horizon length right so I have a good prediction of the of where the robot is going to be okay so the simulation here will terminate after the 20 seconds we set for the simulation time all right and these are the control actions so you see that the maximum control action in NV was 0.6 and that's something we are we we are not going beyond right and the same for Omega that's one thing down here and the matlab implementation i'm calculating something called average MPC time per step so that's basically the total simulation time of the loop divided by the number of iterations is the slope this roughly gives me the average computation time per MPC step and this turns out to be about 17 milliseconds in this case the average is about 17 milliseconds in this case so if I have a sampling rate of 10 Hertz which is 100 milliseconds so I'm well below that number right from a wild robot 1010 Hertz is is good enough right for low speeds at least right okay so let's now increase the prediction horizon lengths and set it up to 10 so the simulation will take a little bit longer time so he's now we see a longer prediction right and the robot goes nicely to the reference position maybe not exactly but that was good enough for the error tolerance we set right okay so let's now change the orientation of the reference in terms of stability this controller with this prediction horizon was enough to ensure that my system is stable like the robot goes exactly to where I would like it to be right okay so now let's just change the reference so I'll go down here to the simulation loop here's my simulation loop and I'm just going to change the reference like to be the same position but opposite orientation of the robot so I set here the reference theta to be PI okay and I'm going to run again my code it turns out that a prediction horizon of 10 was not enough for doing that so the robot goes ahead of my reference a little bit and then it backs up as if it's parking right if when you see this actually done on a robot this looks really fancy actually on an actual robot right okay so in order to fix this problem we can again increase the prediction horizon lengths so the simulation would keep moving until we're done with the 20 milliseconds of simulation so this notch here and the angular speed that's when it turns and goes backward right so let's just increase the prediction horizon then length I think 15 would work here I don't know of course longer protection horizon is reflected on longer simulation time or longer computation time right so here it backs up and meets my probably my my error tolerance or no maybe not but it seems like not because if it met it could have stopped yeah I would have stopped actually but this means that we increase the prediction life okay let me go back to the slide because it has the values oh yeah so in this case 25 would do the job right like that's a pretty long prediction horizon but that's fine okay so do you have any questions so far that's actually a very good question and it like how to how to put a constraint or how to penalize the rate of change of the speed that's basically the acceleration so if you go to the cost function we penalize the difference between the speed and the reference is speed and in this case the reference is speed is zero right you can you can also add an extra term to that cost function which is the change in the speed like that speed minus the previous speed the current speed minus the previous speed and if you put a good weight on that this will make the acceleration and deceleration like slower than usual or then without penalizing it so instead of penalizing the difference between the control action and the reference you penalize the change in the control action I can show this to you on the slide so I'm going to bring up the slides again I'll go back oh that's really bad yeah here so that's my running costs so instead of penalizing this difference or in addition to penalizing this difference both can work right so you penalize the difference between you and the previous you because think of it as a that's that's a summation right so here I would put U of K minus U of K minus 1 see what I mean and in the code that would be that would be right here oops yes here here we're penalizing just the control action so we get the control Khan here right so you could have we could get Khan right and Khan -1 for example or contrast one right so we would have U of K and u of k plus 1 because n decks here doesn't start from 0 so if I am starting from K equals to 1 I can't say K minus 1 as your index doesn't work in MATLAB right so here I can define Khan and Khan plus 1 and column plus 1 would be you of all columns sorry all rows and K plus 1 ones column and here when I penalize it won't be just con it will be the current control or the next control minus the current control action I'm penalizing the change in the control not the difference between the control and the reference control and in this case yeah we are penalizing the acceleration we're giving it a weight like we would like to minimize this acceleration anything you put in this objective function remember that we are minimizing this objective function in the end right so anything you put in this objective you are minimizing and and and how important this term in Europe the objective function to minimize depends on how much weight you put for that term in the minimization problem or in the objective function since acceleration here is not a direct control action so like it's not an optimize it so it turns out that it will not be an optimization variable so I can't put direct limits on the acceleration in this case but I can penalize it I can involve I can like calculating the the difference between the two speeds this is somehow calculating the acceleration right so when I penalize this difference turns out that I'm paralyzing basically the acceleration right or similar to penalizing the acceleration because the acceleration is a difference in speed divided by delta T just penalizing the different speed is some sort of penalizing the acceleration right okay any any more question oh I think yeah so we so far we stopped here right I think we have some good time to do multiple shooting quickly yeah so unless you have any question I'll go forward so what we did actually was a single shooting implementation it was very intuitive right so we had this big prediction matrix X that we wanted to fill using the system model so we we filled it by the system model and the control actions right so what we did call the multiple choice re single shooting technique so analytically we said that hey our optimization variables are W which has all the control actions from u 0 to n minus 1 if we start from an index 0 then we stop at n minus 1 right and then we got the prediction for X by propagating this U and the initial value for X and we got this big matrix X if you remember right that we got this matrix F where the initial vector in this matrix X I'm calling here F again so sorry for changing the variables names the first entry in this matrix here is basically my initial condition right so this big matrix X is mainly function of what W my optimization variable and some other constants or parameters right so my NLP problem or the nonlinear programming problem was that I wanted to minimize this objective the objective function that is function of the prediction of X and apparently the optimization variables this is my cost function right and this is another way of looking at it subject to some constraints right so we read the objective function Phi is function of the prediction which is already a function of what optimization variables and the optimization variables themselves right and the constraints that I used here I put the constraints on X&Y right but x and y were already expressed in terms of what the optimization variables right so we had n equality constraints we had constraints on the map margins and we had constraints on the control limits themselves but did we have any quality constraints so far no we didn't have any quality constraints right so what what's the main drawback of single shooting technique it's the nollie non-linearity propagation so you remember how we filled up this prediction matrix and you can imagine how not linearly the last entry could get if the prediction horizon is really long like 10 20 hundreds so that that can get really really nonlinear so this nonlinear propagation is reflected on the computational time as reflected on the conversions in general like it may not converge for a really large prediction horizon and so on right so the single shooting technique means that the NLP problem was discretized only and the control actions where we wanted to have this equality between the state and the predicted state only and the initial condition and the rest of the prediction was a forward recursion of this initial state right that's single shooting we have this equality only at a single point that turns out to be a shooting point if you want to like go and check more theoretical details right right so then single shooting workit fine for us right but we can actually do better than just that using the multiple shooting so multiple shooting is another way of expressing the or of converting or discretizing the OCP or the optimal control problem to a non linear program problem so what we do in an in a multiple shooting method or a multiple shooting technique we consider both U and X as optimization variables so the optimization variables are not just you they are X as well of course this will be reflected on how we formulate the NLP later but this is the first thing to note so we have X as an optimization variable and you as a optimization variable this means that we have more optimization variables right just keep that in mind right and then we add a path constraint we will have it like an equality constraint which constrains the difference between the predicted state sorry the actual state and the predicted state and the difference between the two that's basically my model so the difference between X and f of X should be zero and we apply this at every prediction step so that's why it's so applying this constraint or equality constraint at every prediction step or every shooting step that's why it's called multiple shooting okay when we see how this is coded it will sync and immediately and it won't be any problem like there will not be any problem then right so now my optimization variables again are not only used but also XS so when the optimization variables was only U and we had a prediction horizon of three and we ended up with how many optimization variables six right so now if we additionally have X's just keep in mind that XS we have one more prediction in Ex than in you because when you tells me what the next X is right but the current X doesn't necessarily tell me what the next two is right so that's why the length of X is normally one step longer than the length of you right so now we have more optimization variables right okay and then we will minimize our of the objective function which is directly function of U of W instead of a function instead of being a function of a function of W so now our objective is directly function of W because W has both has their predicted state and has the control actions right and we will have these two types of constraints we will have this these constraints in equality constraints like map margins and control limits and we'll have now equality constraints which is that satisfies the system dynamics right so let's see how this is coded but before that let's look at some advantages of multiple shooting multiple shooting is actually called a lifted single shooting and lifting means that refill reformulating the problem in more variables in order to get us like a convergence faster it's a simple way of simplification looks like making making things complex but it actually simplifies things it's it's performance is much superior than the single shooting because of lifting and now we can instead of initializing only you when we solve MPC recursively we can also initialize what X right and this is an advantage by the way so think of single shooting again what is the only thing we can initialize you right but normally we can know how the robot will move like but we are not necessarily familiar with how the control action will look like like if I would like to initialize the control actions with some known trajectories I have to solve the dynamics over that trajectories inversely like I do embrace dynamics get the control action that satisfies this motion profile right and initialize my controller with this motion profile but now with the luxury of having the state as an optimization variable itself so I can take the proposed initialization for X and plug it directly to my initialization a little bit unclear or laughs maybe it needs more clarification but this turns out to be an advantage of having a multiple shooting discretization for the optimal control problem right the drawback might seem to be having a larger nonlinear programming problem but it turns out that it's more sparse and this means that it can be solved with faster and the conversions can be achieved as well right so let's go ahead and do multiple shooting so what we have so far is very common to what we have or what we had earlier right defining the sampling time prediction horizon length margins symbols for X Y and theta symbols for V and Omega the right-hand side the same function f the same the matrix U for the controls the same vector P for the parameters the same matrix X for the predicted state or the state production the same q and the same R right so now let's compute our objective and the Equality constraint which represent the dynamics all right previously what we used to do or in the single shooting what we did was we filled up this matrix X by propagating U and the parameter is P and the system model right this is what we did earlier right now we're not going to do that right so what are we going to do now we're going to in parallel calculate my objective function and these constraints entries right so again the first entry of X is my initial state right so my first constraint is going to be the difference between this initial state and the first parameter and the first three terms in the parameters vector right and then we go through this for loop and this for and in this for loop we calculate the objective so so you can see here that the objective is calculated directly from X and X like we did not change anything in X that's the the variable X as defined like we did not fill this X with anything we're just putting X here as defined right and we're calculating this state s T from X we're finding the difference between it and the reference P from four to six penalize it add the penalisation for the control right there is a step that we didn't do here and we didn't a single shooting which was filling up this matrix this big matrix X we did not do that here but instead we filled up this vector G with the difference between the state like the next state from the vector X and the predicted next state which has which by evolving the current state and the current control and the right-hand side of this equation multiplying it by by delta T and adding this up to the previous state if you look at this like for 10 seconds you will you'll make sense of everything pretty much in this for loop right so what were what we're doing here is we are building up my objective and we're filling up my constraint vector here right so we are we would like to ensure this equality constraint at all the shooting steps that's why this is called a multiple shooting technique okay so again the the the the main difference encoding from X here and previously previously before calculating the objective first we filled up the big matrix X by forward recursion of the system model and the initial state right but here were not doing that here we're using X as an optimization variable itself and instead we're creating a constraint an equality constraint between X the optimization variable and X the predicted value of it which comes from the models right okay so now the optimization variable is longer so instead of having only ratio and instead of free shaping on the you now we're shaping X as well so now this optimization variable vector will have like many values of course depends on the production of Rison length we're using the same like the same structure for defining the NLP problem the same fields the same options for optimization and now the constraints if we look at the constraints remember in g and g what do we have in g these differences right what is the upper bound and the lower bound for all these terms upper bound is 0 and lower bound is 0 this is an as an equality constraint so upper and lower bounds are 0 is basically right okay so when we define our arguments so the upper bound and lower bound for this vector are zeros right and look at the length of this vector it's from 1 to 3 times n plus 1 and a plus 1 because that's my prediction horizon and three because three states like every month every shooting step has three states right ok ok let's go backwards one step so what do we have in the optimization variables the first part is what the state so so we should put upper bounds and lower bounds on the states right and we will use the map margins we have 2 1 minus 2 for x and y so we did that for x and y and for theta the upper bounds and lower bounds are basically infinity theta is free to be chosen right and the remaining terms and the upper bound and lower bounds of X which are my optimization where because now my optimization variables have everything right they have the X's and D and the use right so and then we put the V min and Max and Omega min and Max right and then the simulation loop right so the simulation loop is like before except for the initialization so the initialization here we have initialization for you and initialization for X as well so the initialization for the control actions over the prediction horizon is zeros and the initialization of the X or the state trajectory over the prediction horizon is basically a repetition of the initial state for the old time steps well it turns out to be zeros as well because the initial state was already zeros okay all right so now my overall overall initialization of the optimization variables is this big vector here which has the initialization for the states and the initialization for control because they are all now optimization variables right calling my solver means that I have now the control actions the nice thing about multiple shooting as well is that it doesn't give you the control actions it also gives you the predicted trajectory right because it's part of the solution is the predicted trajectory right so that's why here I'm extracting from X which is the solution the control action and I'm also extracting the predicted trajectory that's that's the controls only and that's the predicted trajectory or the solution trajectory right and then applying the shift function as previously and everything else in this slide is as previously some now let's look at a MATLAB demo for multiple shooting okay all right so now I'm going back yeah I'll bring the MATLAB again I'm just going to do single shooting just one step before doing multiple shooting you just want to show you something before going ahead so I'm setting back my reference orientation to be zero again right so let's run this and see the average computation time bursts there they're like their time step of the NPC right so here NPC work it fine for an equal to ten yeah and the average NPC time was 24 milliseconds or 25 milliseconds right that's not bad so let's look at multiple shooting so I'm gonna set this to 10 again just want to make sure that the reference is zero as well okay okay so now that's multiple shooting in action for N equals two ten works fine but let's look at the differences especially in time or the computation time between the two single shooting and multiple shooting here this multiple shooting finish that and again around 24 milliseconds right it's pretty close right so let's look what happens if the prediction horizon is really long so let's go back to single shooting and set my prediction rising to a hundred so 100 means how many optimization variables 100 times to write 100 times to 200 right so it's gonna be really slow at the beginning right so let's look at the average computation time we're sampling step so it seems like it's actually really slow and the computation time turns out to be 230 milliseconds or 0.2 seconds 0.23 seconds that's that's quite time-consuming right so let's do a hundred on multiple shooting and see what happens that's ten that's multiple shooting oh that's not even 100 milliseconds or even a 50 milliseconds so that's so that's let's say that's this is a 50 milliseconds and the other one is 250 milliseconds so this multiple shooting is actually five times faster for this pretty long prediction horizon length right and that's not the only advantage the other advantage could be in single shooting for even larger prediction horizon the solver may not be able to find a solution all all over or like at all okay so multiple shooting so now when when this was set to a hundred how many optimization variables we have in this problem now 500 yeah we have five optimization variables times n equals to 100 we have actually not only five hundred five hundred and three right because of that right so we have five hundred optimization variables so compared to two hundred and five hundred was quite fast so it's not only that that you need a good solver to solve an f4 and MAPC problem you also need a good formulation for for your problem and it turns out that multiple shooting and I personally use multiple shooting then more than single shooting actually afternoon multiple shooting I did not go back to single shooting again and the reasons are so obvious right I think stopping here is a is a good point to stop at and tomorrow we will talk about the rest of MPC stuff which is basically how to include obstacle avoidance in MPC and this turns out to be just adding more state constraints or constraints on the state and then we will see how to implement mhe right so

Channel: Mohamed W. Mehrez

Views: 29,942

Keywords: model predictive control, moving horizon estimation, casadi, matlab code, single shooting, multiple shooting, nonlinear programming, differential drive robots, non-holonomic robots, mobile robots, mpc, mhe, nlp, programming

Id: RrnkPrcpyEA

Length: 103min 39sec (6219 seconds)

Published: Tue Jan 29 2019

Reddit Comments

[u/meelon_usk]

Probably one of the best video series I've seen explaining how to implement MPC in MATLAB. If anyone else has some good videos, please share.