Control system modelling & simulation from dynamic equations (quadrotor)

I have a control system that i have been desperately trying to simulate in matlab.
It for the a quadrotor, i have the dynamic equations (attached) which have been confirmed from my own theoretical understanding and from several journels and books.

I want to design an PID controller for the control of the angular position however all the inputs to the system are in terms of Ui. (a term which is interns of thrust). How can I model this system and graph the overshoot, undershoot rise and settling times for a given controller from what i have here, how do i change the form of the inputs into something that i can quantify (using MATLAB).

I only reviewed your equations briefly because I'm at work. But, my first glance indicates that this is a nonlinear system. Hence, using Matlab and designing a PID controller is probably best done by linearizing the equations and making a linear state space system model. The model will be dependent on steady state operating point. The procedure for doing this is shown in most text books on modern control theory. If you need any more information just let me know and I can provide when I'm at home with more time.

Your first step would be to rewrite your equations as a nonlinear state space system, which means write out all the first order differential equations that equivalently represent your second order differential equations.

That system looks fairly easy to linearize, so I could do it for you with about 20 minutes of work, but I wont do it unless you ask. My gut feeling is that you are good at math and wouldn't have much trouble doing it. These are good tricks to learn, so I encourage you to figure this out and do it if you can. The first time might seem a little difficult, but after that you will find this easy in the future.

Thanks for your reply, I undertook this project precisely because i wanted to wrap my head around control system design and modelling after finding the mass- spring damper and RLC circuit rather easy. However having done them, im now struggling to make the transition to something of this scale.

I'll break out a text book and do a bit of googling to help me linearise and give it a go.


I would love you to go through it too when you get the chance so that I have something to check against and possibly as a guideline if my efforts go 'belly-up'. Especially since from my (rather limited) perspective I still cant fathom how a transient response could be formed from such equations.

But such is the fun of learning from those in the know..

thanks so much.

OK, no problem. I'll go through it and sketch something out for you in the next few days. Let me know if you don't have a good text on the subject. I can go through my text books and make a recommendation. My first thought is that since you are probably thinking about analog control (tell me if you are interested in digital control), then"Modern Control Engineering" by Ogata, may be a good choice.

Well I'm going to be using a micro-controller to implement the controller. Thanks for the book recommendation. I googled it and the first link was a PDF of the complete book. Im interested in both types of control. If you have any more books to recommend i'm all ears.

Thanks again

As you know, for multiple-input multiple output (MIMO) systems, you need to use the state-space formalism that is the central tool of modern control theory. There are many books that cover this. Most introductory books provide all the theory but restrict the discussion to single-input single-output SISO systems, but it is a small step to extend the ideas to MIMO once you know state-space methods for SISO systems.

In the end, a device such as the quadrotator probably will need a linear quadratic regulator (LQR) approach which is the getting towards the most sophisticated end of the available control schemes, but you can try to do it with a PID controller of some type. After a project like this, you will be a good control engineer, so this is all time well spent, if that is your interest.

One of the best books on digital control is "Digital Control: A State Space Approach", by Vaccaro. This is my favorite because Vaccaro is my old professor and because the book is so well written and concise.

This book and many other digital control books may appear too abstract, and you may want to focus on the continuous time control theory at first. However, you will need to convert your system model to a digital (discretized) form and use a discrete time controller when you do your Matlab simulations. So just go one step at a time, take your time, and ask any questions if you get to a sticking point

Some other good books are as follows.

"Modern Control System Theory", by Gopal

"Control System Design: An Introduction to State-Space Methods", by Friedland

"Digital Control System Design", by Santina, Stubberud and Hostetter

"Digital Control Engineering: Analysis and Design", by Fadali

"Control Systems with Input and Output Contraints", by Glattfelder and Schaufelberger

"Feedback Control of Dynamic Systems", by Franklin, Powell and Emami-Naeini

There are many others too, but I know these particular ones to be good, each in their own way.

If you don't mind, I will try to post small steps in the process, so that you can verify that you are proceeding on track. Feel free to point out if you are way ahead of me, or not following me, or insert your own steps as needed.

The first step you probably already did, but let's make sure. You need to write the nonlinear state space equations before you can linearize them. This is easy in your case because your equations are all second derivatives of postion, and you only need to define velocity states (3 linear velocities and 3 angular velocities), and the 12 state space equations will fall right out as follows. Note that I'm using the Greek letter beta instead of phi because the phi is not provided by the editor and because phi and theta are easy to mix up. Also, I use T instead of tau for the torque moment inputs. Note that 9 of your 12 state equations are already linear. So, linearization will be easy and needs to be done to 3 equations only.

dx/dt = vx

dy/dt = vy

dz/dt = vz

dψ/dt = ωψ

dθ/dt = ωθ

dβ/dt = ωβ

dvx/dt = u (sinβ sinψ + cosβ cosψ sinθ) / m

dvy/dt = u (cosβ sinθ sinψ - cosψ sinβ) / m

dvz/dt = u (cosθ cosβ) / m - g

dωψ/dt = Tψ

dωθ/dt = Tθ

dωβ/dt = Tβ

Some key question and comments are needed here. First, is "u" a constant, and what does it represent? Also, are the torque moments true inputs of the system that do not depend on any of the system states? These are critical questions that must be answered before you or I can linearize the system.

Hi, posting it in stages is absolutely fine. in fact preferred. Ive read the book you suggested up upto and including linearising non linear functions and have been looking at the examples which follow.

In honesty I only managed to get as far as you did in this post as I was quite sure (as confirmed by your last post) that most of the equations were already linear. Glad I'm not losing my mind.

U is the thrust generated by the quad, expressed in terms of the sum of the torques of the motors at a given point in time. This thrust will vary depending on the command in put to the micro.

For the purposes of simplicity the torques of the motor are assumed to be equal to the square of the angular velocity. The angular velocity of each motor, for a given PWM duty cycle will be verified tomorrow.

(I decided to build a tachometer to measure motor RPM, however there are pretty much zero electronic component stockists that exist as traditional stores anymore so i was unable to pick up an infrared transmitter/receiver today. However it has been ordered and will arrive tomorrow and be assembled/tested tomorrow.)

Finally the torque moments are true inputs of the system.

Incase I am babbling and being incomprehensible these few pages describe everything that I may have failed in portraying:

https://tinyurl.com/pf3d882

I m very grateful. Thanks again.

OK, so my interpretation of this is that there are 4 independent inputs that you can control. There will be various conversions you will implement in software to convert various parameters and system controls into the equivalent inputs of the mathematical model. The inputs to the mathematical model are u, Tψ, Tθ and Tβ.

So what becomes apparent from the equations is that when you linearize, you need to specify an "operating point" which is the DC steady state conditions that affect the linear model. You will need DC values of u, ψ, θ and β.

What I usually do is write all variable with DC values with an overbar and AC values with an over-tilde in the form x=\bar{x}+\tilde{x} in latex notation. For some reason I cant get the latex to work. However, this gets awkward, so going forward lets use u', ψ', θ' and β' to represent the DC steady state values, and let the ordinary variable represent the AC part of the signal from now on.

The linearized equations work out as follows, if I didn't make a mistake (note that a small mistake is likely in this since I didn't double check, but you will be double checking me). The DC values u', ψ', θ' and β' act as parameters (similar to m) in the AC model.

dx/dt = vx

dy/dt = vy

dz/dt = vz

dψ/dt = ωψ

dθ/dt = ωθ

dβ/dt = ωβ

dvx/dt = u (sinβ' sinψ' + cosβ' cosψ' sinθ') / m + ψ u' (sinβ' cosψ' - cosβ' sinψ' sinθ') / m + β u' (cosβ' sinψ' - sinβ' cosψ' sinθ') / m + θ u' cosβ' cosψ' cosθ' / m

dvy/dt = u (cosβ' sinθ' sinψ' - cosψ' sinβ') / m + ψ u' (cosβ' sinθ' cosψ' + sinψ' sinβ') / m - β u' (sinβ' sinθ' sinψ' + cosβ' cosψ') / m + θ u' cosβ' sinψ' cosθ' / m

dvz/dt = u (cosθ' cosβ') / m - β u' cosθ' sinβ' / m - θ u' sinθ' cosβ' / m

dωψ/dt = Tψ

dωθ/dt = Tθ

dωβ/dt = Tβ

Once you double check this, you can generate A, B, C and D matrices as needed to specify a state space system in Matlab using the "ss" command.

I should point out that in order to determine the C and D matrices, you have to specify what measurements you will take as your outputs of the system. I expect there are accelerometers and/or gyros that will give you information to estimate some or all of your state variables. You may want to discuss this now because the outputs you have will impact the design. It is also possible to create an uncontrollable system if you don't have enough measurements integrated into the design.

Forgive my absence, Just come back from work. My parts have arrived and ill be able to accurately determine the rotor RPM and hence the torque.

The Gyro and Acc will measure the angular velocity and position, they're both already calibrated and have been determined as accurate. this is what will be fed back into the system.

I believe that is sufficient for my design (correct me if I'm wrong.).

Additionally im just checking the calculations late tonight and tomorrow morning, and will complete the circuit and let you know of the results

mikeOJ said:

Forgive my absence, Just come back from work. My parts have arrived and ill be able to accurately determine the rotor RPM and hence the torque.

The Gyro and Acc will measure the angular velocity and position, they're both already calibrated and have been determined as accurate. this is what will be fed back into the system.

I believe that is sufficient for my design (correct me if I'm wrong.).

Click to expand...

I can't say you are wrong, as I don't have enough expertise with gyros and accelerometers to be sure if you are right or wrong. I believe you are correct about the gyros providing angular velocity accurately. My concern is determining position from accelerometers. Since position is the second integration of acceleration, it is not trivial to get position accurately, from what little experience I have with this. Still, I've heard that people are able to do this with certain techniques.

Either way, it makes sense to first study the control issues under the assumption that you can accurately measure all of the state variables. This makes sense because if you can't get it to work with perfect measurements, then imperfect measurements have no chance of working. If you do have measurement issues, then the usual solution is to develop a state observer to accurately reconstruct the state estimates from the measurements that are available.

So, again, it makes sense to go step by step and deal with problems as they arise. Most control problems are done based on the idea of "what is the minimum I need to get it to work?". You try the simple ways first, and as they fail you go to the next level, until you have sufficient complexity to get the job done. Fortunately, control theory provides many levels you can apply.

By the way, don't worry about how fast or slow you are able to respond to this thread. I'm certainly in no rush, and more time means more time to think. I've found that problems of this complexity level are best solved slowly, with sufficient time to think and double check.

The details of the control design have not been mentioned, but it occurs to me that you may not want to do x, y and z position control. You certainly could if you want, but you may choose to control vx, vy, vz, ψ, θ and β. If you make this choice, you will only need 9 of the state equations. This also has the advantage that the gyros give you ωψ, ωθ and ωβ without differentiation or integration, and only one integration gets you to ψ, θ and β. Likewise, the accelerometers give you 3 accelerations and only 1 integration is needed to get to vx, vy and vz.

Just food for thought.

OK, so my interpretation of this is that there are 4 independent inputs that you can control. There will be various conversions you will implement in software to convert various parameters and system controls into the equivalent inputs of the mathematical model. The inputs to the mathematical model are u, Tψ, Tθ and Tβ.

Click to expand...

Each motor is treated as a separate input which is what allows for the attitudes of ψ, θ and β (pitch, yaw and roll) to be achieved, These inputs are sent from a microcontroller via PWM. Therefore the average voltage seen in the motors (duty cycle) is what will determine how fast the motors spin, so I believe I may be right in saying that the input to the system is Velocity of each motor.

However the assumption being made is that Torque is the square of the angular velocity, which is why it is being used in these equations. This is also the reason that I am building the tachometer so that i can graph the angular velocity at different PWM inputs. However after tinkering for two days, and having tested in a known good circuit. I realise that i have been delivered faulty transistors .

The Gyro and Acc will measure the angular velocity and position, they're both already calibrated and have been determined as accurate. this is what will be fed back into the system.

I believe that is sufficient for my design (correct me if I'm wrong.).

Click to expand...

I have definitely been unclear, and you are correct in your assumption. My aim is not to control the position of the vehicle along the x,y and z axis. When i said angular velocity and position. I should have said angular velocity and angular position.

Initially I want to be able to achieve stable flight within particular angular limits and return to a steady hover/ assent/ decent on command. Accurate measurement of angular position would allow me to through an integration of the gyroscopic data. In this instance monitoring angular velocity itself is advantageous because it will allow the software to determine whether the craft is out of control i.e in the instance of a motor fault at altitude (or just poor pilot control) resulting in a wild spinning free fall.

I will also be using the accelerometer data to determine the speed of the craft, probably for user display or data logging at this stage. At a later stage I (once im more comfortable with the theory) I will attempt to use this data to aid control of the x,y,z positions.

Each motor is treated as a separate input which is what allows for the attitudes of ψ, θ and β (pitch, yaw and roll) to be achieved, These inputs are sent from a microcontroller via PWM. Therefore the average voltage seen in the motors (duty cycle) is what will determine how fast the motors spin, so I believe I may be right in saying that the input to the system is Velocity of each motor.

However the assumption being made is that Torque is the square of the angular velocity, which is why it is being used in these equations.

Click to expand...

For this reason i am designing the state space based on the input of each motor which will create a moment around each of the Tψ, Tθ and Tβ axis.
Tψ is: k((ω1^2)+(ω3^2)-(ω2^2)-(ω4^2))
Tθ is: Lk((ω2^2)-(ω4^2))
and Tβ is: Lk((ω3^2)-(ω1^2))

where ω1-4 respectively are the angular velocity of a given motor, and (ω^2) is the torque provided by said motor.
Thus a movement on a given axis is the based on the ratio of torques or motor velocities on each axis.

Thus:

dωψ/dt = k((ω1^2)+(ω3^2)-(ω2^2)-(ω4^2))

dωθ/dt = Lk((ω2^2)-(ω4^2))

dωβ/dt = Lk((ω3^2)-(ω1^2))

where L is the distance between each motor and the centre of gravity and K is a constant greater than 1.

mikeOJ said:

Each motor is treated as a separate input which is what allows for the attitudes of ψ, θ and β (pitch, yaw and roll) to be achieved, These inputs are sent from a microcontroller via PWM. Therefore the average voltage seen in the motors (duty cycle) is what will determine how fast the motors spin, so I believe I may be right in saying that the input to the system is Velocity of each motor.

However the assumption being made is that Torque is the square of the angular velocity, which is why it is being used in these equations. This is also the reason that I am building the tachometer so that i can graph the angular velocity at different PWM inputs. However after tinkering for two days, and having tested in a known good circuit. I realise that i have been delivered faulty transistors .

Click to expand...

This all sounds fine. The only comment I can make here is that the state space equations are "more linear" with those torques and thrust inputs, as specified. When you make your control model, you will but using those equations. Hence, it seems to me that you need to have conversion equations that will convert the torque/thrust input commands to the motor velocity commands needed to get those torques and thrust.

Then when you make your control model, the variables you are trying to control will each have a command input which the feedback controller will try to track. So, the open loop system has particular inputs, and then the closed loop system has different inputs.

This is not trivial to set up and model. I would not say it is hard, but this is not trivial and it can be a little tedious to work out all the equations and implement them in a controller. I have not sat down and written it all out, but I may do it at some point, just because I find it an interesting problem.

mikeOJ said:

I have definitely been unclear, and you are correct in your assumption. My aim is not to control the position of the vehicle along the x,y and z axis. When i said angular velocity and position. I should have said angular velocity and angular position.

Initially I want to be able to achieve stable flight within particular angular limits and return to a steady hover/ assent/ decent on command. Accurate measurement of angular position would allow me to through an integration of the gyroscopic data. In this instance monitoring angular velocity itself is advantageous because it will allow the software to determine whether the craft is out of control i.e in the instance of a motor fault at altitude (or just poor pilot control) resulting in a wild spinning free fall.

I will also be using the accelerometer data to determine the speed of the craft, probably for user display or data logging at this stage. At a later stage I (once im more comfortable with the theory) I will attempt to use this data to aid control of the x,y,z positions.

Click to expand...

This all sounds reasonable. I would probably proceed in a similar way. Sometimes it is difficult to visualize and foresee everything with a complex control problem like this. However, as you dive into it, everything will become clearer. The equations tell you what you need to know and as you become more familiar and comfortable with the equations you will begin to visualize better. Then, the Matlab modeling will teach you even more things that may not have been obvious to you from the equations.

These problems would be nearly impossible without Matlab tools and modeling to work out all the issues. This is why modern control theory and modern computer technology have been so critical to improvements in aircraft control. It is not only the technology to implement the solutions that is needed. The technology is also needed to design the control system. And, the technology would not even be enough without the tools of modern control theory which can handle the multivariable control problem.