An Insight into the Dynamics and State Space Modelling of a 3-D Quadrotor

An Insight into the Dynamics and State Space Modelling of a 3-D Quadr otor Rahul V igneswaran K ∗ † Department of Mechanical Engineering Amrita V ishwa V idyapeetham Amritapuri, India rahulvigneswaran@gmail.com Soman KP Center for Computational Engineering and Networking (CEN) Amrita School of Engineering, Amrita V ishwa V idyapeetham Coimbatore, India kp_soman@amrita.edu Abstract Drones have gained popularity in a wide range of field ranging from aerial pho- tography , aerial mapping, and in vestigation of electric po wer lines. Every drone that we know today is carrying out some kind of control algorithm at the low lev el in order to manoeuvre itself around. For the quadrotor to either control itself autonomously or to dev elop a high-level user interf ace for us to control it, we need to understand the basic mathematics behind ho w it functions. This paper aims to explain the mathematical modelling of the dynamics of a 3 Dimensional quadrotor . As it may seem like a tri vial task, it plays a vital role in how we control the drone. Also, additional effort has been taken to e xplain the transformations of the drone’ s frame of reference to the inertial frame of reference. 1 Introduction The popularization of the drone technology has rendered the need for understanding its dynamics and answering the question of how to contr ol accor ding it to our desirability? as vital. Drones are formally kno wn as Unmanned Aerial V ehicles (U A Vs). It can either be controlled wirelessly by pilots on base stations or can autonomously execute a pre-programmed task. They are very in size and configurations. The drone studied in this paper belongs to the category of micro-drones with 4 symmetrical rigid rotors. Even though the quadrotors hav e become common, the underlying mathematics is still complicated. This is so, because, quadrotors are highly coupled , that is, the fluid dynamics of propellers influence each other . Since its dynamics inv olve multiple inputs and outputs, it is considered a multiv ariate problem and call for the requirement of designing a non-linear mathematical model. It must be noted that the system is actually under-actuated. The term under - actuated means, when the number of actuators is less than the degree of freedom. Here too, the drone can mo ve along x,y ,z axes and can rotate about the x,y ,z ax es which makes up for a total of 6 DOF b ut the quadrotor , as the name suggests is equipped with only 4 actuators, hence effecti vely making it as an underactuated system. Further on how to o vercome this and manoeuvre through all 6 DOF will be discussed later on in this paper . Section 2 gi ves a deeper understanding of rigid body transformations ∗ https://rahulvigneswaran.github.io † W ork done while interning at CEN, Amrita V ishwa V idyapeetham, India. Preprint. W ork in progress. Figure 1: Arbitary points p, q and their corresponding vectors V ,W are being mapped on a rigid body . (both translational and rotational). Section 3 introduces Euler angles from basics and mo ve onto deriv e a method to find the deriv atives of them. Section 4 explains the assumptions in volved in the modelling and further proceeds to e xplaining the kinematics model. Then it sho ws the design of a dynamics model from scratch by discussing the various forces and aerodynamic moments that affects the model and deriv es a state-space model for it. 2 Rigid body T ransformations Due to the reason that the drone is always in constant motion, it becomes necessary to understand its current position with respect to its previous position or how a set of mutually orthogonal vectors can be mapped to another set of mutually orthogonal vectors. This can be achieved by using Rigid body transformations [ 1 ]. For the purpose of generalization, an arbitrary body is taken into account instead of the quadrotor itself. For example, let us take the case that is sho wn in Figure 1. When a point in p is displaced, it is denoted by g ( p ) . 3 W e must note that displacement is essentially a transformation of points. Similarly , q is displaced to g ( q ) . Now consider the vector V from p to q . As the points p , q mov es to g ( p ) , g ( q ) , the V is mapped to g ∗ ( V ) . 4 A transformation is valid only when it satisfies the follo wing conditions. • A set of mutually orthogonal vectors after getting mapped must remain mutually orthogonal. • The transformation g must preserve lengths. k g ( p ) − g ( q ) k = k p − q k • The transformation g* must preserv e cross products. g ∗ ( V ) × g ∗ ( W ) = g ∗ ( V × W ) Now let us take two rigid bodies { A } and { B } in a configuration as sho wn in Figure 2 and try mapping them a point P on B to A . The body A has x − y − z frame of reference with origin O attached to it while the body { B } has x 0 − y 0 − z 0 frame of reference with origin O 0 attached to it. This is a two-stepped process. As we may understand from Figure 2, the mapping in volves a rotation mapping and translation mapping. In order to map the x − y − z frame to x 0 − y 0 − z 0 frame, it must be rotated once and then translated. The operations can be carried out the other way too. 3 When a point is transformed, it is denoted by g 4 When a vector is transformed, it is denoted by g ∗ 2 Figure 2: Mapping of two rigid bodies { A } and { B } with different frame of references. A r P = A R B B r P + A r O 0 (1) The equation (1) gi ves the transformation of point P from frame of reference of { B } to { A } . A r P represents the transformation of position vector of P in { B } to position vector of P in { A } while B r P represents the relati ve position of { B } in reference to { A } . A r B represents the rotation of { B } in reference to { A } and A r O 0 represents the translation of origin of { B } in reference to { A } . Therefore, the equation (1) first indicates the rotation transformation of { B } in reference to { A } and then the translation transformation of the resulting body . 2.1 Rotational T ransf ormation It is essential to understand the basic nature of the rotational transformation to obtain intuiti veness for the rest of the paper . For the sake of simplicity let’ s take a 2-D configuration as sho wn in Figure 3 and later extrapolate it to 3-D configuration. From it, the following components of the V and V 0 can be obtained. V x = r ∗ cos α (2) V y = r ∗ sin α (3) V 0 x = r ∗ cos( α + β ) (4) V 0 y = r ∗ sin( α + β ) (5) No w trigonometric identities are used to break do wn the equations (4) and (5) to obtain the following, 3 Figure 3: A x − y configuration in which a point from position V is rotated to position V 0 . V 0 x = r ∗ cos α cos β − r ∗ sin α sin β (6) V 0 y = r ∗ sin α cos β − r ∗ cos α sin β (7) Then by substituting equations (2) and (3) in equations (6) and (7), we get, V 0 x = V x ∗ cos β − V y ∗ sin β (8) V 0 y = V y ∗ cos β − V x ∗ sin β (9) This when we con vert into matrix notation, it giv es the rotation of V about an arbitrary point O . [ V 0 x V 0 y ] = [ V x V y ]  cos β sin β − sin β cos β  Therefore, a 2-D rotation is defined by an angle and an origin. In 3-D, unlike 2-D, it is defined by an angle and a rotational axis. T o obtain the rotational transformation in 3-D, as the axis of rotation is restricted to one of the three major axis, on component always remains constant. Then, by following the same procedure as in 2-D, the rotational transformation about x-axis, y-axis and z-axis are obtained. The rotation around x-axis, [ V 0 x V 0 y V 0 z ] = [ V x V y V z ] " 1 0 0 0 cos β sin β 0 − sin β cos β # The rotation around y-axis, [ V 0 x V 0 y V 0 z ] = [ V x V y V z ] " cos β 0 − sin β 0 1 0 sin β 0 cos β # 4 Figure 4: Roll( φ ), pitch( θ ), yaw( ψ ) in reference to a glider . The rotation around z-axis, [ V 0 x V 0 y V 0 z ] = [ V x V y V z ] " cos β sin β 0 − sin β cos β 0 0 0 1 # 3 Euler angles Leonhard Euler showed that an y rotation can be described by three successiv e rotations [ 2 ] about linearly independent axes and those three angles of rotations are called as roll ( φ ), pitch ( θ ) and yaw ( ψ ). Figure 4 illustrates these angles in reference to a glider . The Euler angles φ , θ , ψ can be determined from the rotational transformation matrix. For the purpose of e xplanation let us take a Z − Y − X rotational transformation. This means that the body is first rotated angle about Z-axis, then angle about Y -axis and then angle about X-axis. Equation (10) is the resulting transformation matrix after Z − Y − X rotation. R = R z ( ψ ) R y ( θ ) R x ( φ ) = " cos ψ sin ψ 0 − sin ψ cos ψ 0 0 0 1 # " cos θ 0 − sin θ 0 1 0 sin θ 0 cos θ # " 1 0 0 0 cos φ sin φ 0 − sin φ cos φ # = " cos θ cos ψ cos ψ sin θ sin φ − cos φ sin ψ sin φ sin ψ + cos φ cos ψ sin θ cos θ sin ψ cos φ cos ψ + sin θ sin φ sin ψ cos φ sin θ sin ψ − cos ψ sin φ − sin θ cos θ sin φ cos θ cos φ # = " R 11 R 12 R 13 R 21 R 22 R 23 R 31 R 32 R 33 # (10) Every element in the matrix (10) is a combination of two or more angles except for R 31 . By using it as starting equation, we get the following. R 31 = − sin θ θ = − sin − 1 ( R 31 ) (11) As we may understand from the equation (11), there are 2 possibilities of θ , 5 θ 1 = − sin − 1 ( R 31 ) (12) θ 2 = π − θ 1 = π + sin − 1 ( R 31 ) (13) From this step on, there are sev eral cases that must be taken into account. Case 1.1 : ( − 1 < | R 31 | < 1) and (cos θ 6 = 0) T o find the v alue of φ , we must observe that, R 32 R 33 = tan φ Now we use this equation to solv e for φ by using the atan2 5 function, as φ = atan2( R 32 , R 33 ) (14) If cos θ > 0 , then the equation (14) holds. Howe ver , when cos θ < 0 , then φ = atan2( − R 32 , − R 33 ) . This can be bypassed by using the equation as, φ = atan2  R 32 cos θ , R 33 cos θ  (15) So, by substituting the equations (12) and (13) in (15), we get, φ 1 = atan2  R 32 cos θ 1 , R 33 cos θ 1  (16) φ 2 = atan2  R 32 cos θ 2 , R 33 cos θ 2  (17) T o find the corresponding v alues of ψ , similar analysis holds good. W e observ e that, R 21 R 11 = tan ψ W e solv e for ψ using an equation similar to (14), ψ = atan2  R 21 cos θ , R 11 cos θ  (18) Now , by substituting the equations (12) and (13) in (18), we get, ψ 1 = atan2  R 21 cos θ 1 , R 11 cos θ 1  (19) ψ 2 = atan2  R 21 cos θ 2 , R 11 cos θ 2  (20) Therefore, for the case where ( − 1 < | R 31 | < 1 and cos θ 6 = 0) , we get two sets of triplet Euler angles, ( φ 1 , θ 1 , ψ 1 ) ( φ 2 , θ 2 , ψ 2 ) 5 An atan2(y , x) is an implementation of atan( y , x ) function that solves the sign ambiguity caused, by taking the ratios and signs of y and x into consideration. Also, atan( y , x ) = atan  y x  = tan − 1  y x  . 6 Both of these solutions are absolutely valid. Case 1.2 : ( − 1 < | R 31 | < 1) and (cos θ = 0) In this case the elements R 11 , R 21 , R 32 and R 33 will become zero causing problems 6 when solving the method used in case 1 and doesn’ t constrain the v alues φ , ψ . As a result,equations (14) and (18) becomes, φ = atan 2  0 0 , 0 0  ψ = atan 2  0 0 , 0 0  Therefore, we must use different elements of the rotational transformation matrix (10) to find the solution. Case 2.1 : ( R 31 = +1) Since we know that, ( R 31 = +1) . From it, we can get θ =  π 2  . Therefore, the rotational transformation matrix (10) becomes, R = " 0 cos ψ sin φ − cos φ sin ψ sin φ sin ψ + cos φ cos ψ 0 cos φ cos ψ + sin φ sin ψ cos φ sin ψ − cos ψ sin φ − 1 0 0 # As a result, ψ and φ have an infinite number of v alid solutions. Case 2.2 : ( R 31 = − 1) Similar to the case 2.1, since we know that, ( R 31 = − 1) . From it, we can get θ = −  π 2  . Therefore, the rotational transformation matrix (10) becomes, R = " 0 − cos ψ sin φ − cos φ sin ψ sin φ sin ψ − cos φ cos ψ 0 cos φ cos ψ − sin φ sin ψ − cos φ sin ψ − cos ψ sin φ 1 0 0 # As a result, ψ and φ hav e an infinite number of valid solutions. In both the case 2.1 and 2.2, we ha ve found that ψ and φ are linked. This phenomenon is called Gimbal lock . Although in this case, there is an infinite number of solutions to the problem, in practice, one is often interested in finding one solution. For this task, it is con venient to set ψ = 0 and compute φ . It is interesting to note that there is always more than one sequence of rotations about the three principle axes that result in the same orientation of an object. As an example, consider a book laying on a table face up in front of you. Define the x-axis as to the right, the y-axis as away from you, and the z-axis up. A rotation of π radians about the y-axis will turn the book so that the back cover is now facing up. Another way to achie ve the same orientation would be to rotate the book π radians about the x-axis, and then π radians about the z-axis. Thus, there is more than one way to achiev e the desired rotation. 3.1 Derivati ve of Euler angles In order to form the non-linear state space equation later on in this paper, it is necessary that we deriv e the deriv ativ es of Euler angles. This subsection is dedicated to deriving an equation to obtain deriv ates of Euler angles ( ˙ φ, ˙ θ , ˙ ψ ) from angular velocities ( p, q , r ) while undergoing a Z − Y − X rotational transformation. Angular velocities in body frame can be express ed in terms of rate of change of the Euler angles, as follows, 6  0 0  is indeteminate as it can hold any v alue. Therefore the equation cannot be solved to obtain the v alues of φ and ψ . 7 ω = p ˆ i + q ˆ j + r ˆ k = ˙ φ ˙ φ ˙ φ + ˙ θ ˙ θ ˙ θ + ˙ ψ ˙ ψ ˙ ψ (21) The first rotational transformation is X 1 , Y 1 , Z 1 → X 2 , Y 2 , Z 2 along Z-axis. In this Z 1 = Z 2 which implies that ˆ k 1 = ˆ k 2 . Therefore, ˙ ψ ˙ ψ ˙ ψ = ˙ ψ ˆ k 1 = ˙ ψ ˆ k 2 (22) The second rotational transformation is X 2 , Y 2 , Z 2 → X 3 , Y 3 , Z 3 along Y -axis. In this Y 2 = Y 3 which implies that ˆ j 2 = ˆ j 3 . Therefore, ˙ θ ˙ θ ˙ θ = ˙ θ ˆ j 2 = ˙ θ ˆ j 3 (23) The third rotational transformation is X 3 , Y 3 , Z 3 → X, Y , Z 7 along Z-axis. In this X 3 = X which implies that ˆ i 3 = ˆ i . Therefore, ˙ φ ˙ φ ˙ φ = ˙ φ ˆ i 3 = ˙ φ ˆ i (24) Now by combining equations (21), (22), (23) and (24), we get, ω = p ˆ i + q ˆ j + r ˆ k = ˙ φ ˙ φ ˙ φ + ˙ θ ˙ θ ˙ θ + ˙ ψ ˙ ψ ˙ ψ = ˙ φ ˆ i + ˙ θ ˆ j 3 + ˙ ψ ˆ k 2 (25) In the transformation X 2 , Y 2 , Z 2 → X 3 , Y 3 , Z 3 , " U 2 V 2 W 2 # = " cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ # " U 3 V 3 W 3 # (26)   ˆ i 2 ˆ j 2 ˆ k 2   = " cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ #   ˆ i 3 ˆ j 3 ˆ k 3   (27) In the transformation X 3 , Y 3 , Z 3 → X, Y , Z , " U 3 V 3 W 3 # = " 1 0 0 0 cos φ − sin φ 0 sin φ cos φ # " U V W # (28)   ˆ i 3 ˆ j 3 ˆ k 3   = " 1 0 0 0 cos φ − sin φ 0 sin φ cos φ #   ˆ i ˆ j ˆ k   (29) Since, we already know that (26) and (28) are true, if we replace the velocity v ector with unit vectors like in (27) and (29), they still hold true. From (27) we can find the expression for ˆ k 2 in ˆ i , ˆ j , ˆ k and it is as follo ws, ˆ k 2 = − sin θ ˆ i 3 + cos θ ˆ k 3 But from (29) we know that, ˆ k 3 = sin φ ˆ j + cos φ ˆ k and ˆ i 3 = ˆ i , so, ˆ k 2 = − sin θ ˆ i + cos θ sin φ ˆ j + cos θ cos φ ˆ k (30) 7 X, Y , Z refers to the inertial/ground frame of reference. 8 Similarly , the expression for ˆ j 3 in ˆ i , ˆ j , ˆ k can be deri ved from (29) and it is as follo ws, ˆ j 3 = cos φ ˆ j − sin φ ˆ k (31) Now by substituiting (30) and (31) in (25), we get, p ˆ i + q ˆ j + r ˆ k = ˙ φ ˆ i + ˙ θ (cos φ ˆ j − sin φ ˆ k ) + ˙ ψ ( − sin θ ˆ i + cos θ sin φ ˆ j + cos θ cos φ ˆ k ) (32) From (32), we can deriv e the following v alues, p = ˙ φ − (sin θ ) ˙ ψ q = (cos φ ) ˙ θ + (cos θ sin φ ) ˙ ψ r = (cos θ cos φ ) ˙ ψ − (sin φ ) ˙ θ By rearranging the abov e in matrix form, we get, " p q r # = " 1 0 − sin θ 0 cos φ cos θ sin φ 0 − sin φ cos θ cos φ #   ˙ φ ˙ θ ˙ ψ   Or ,   ˙ φ ˙ θ ˙ ψ   = " 1 sin φ tan θ cos φ tan θ 0 cos φ − sin φ 0 sin φ sec θ cos φsecθ # " p q r # (33) The equation (33) can be used to obtain the deri vati ve of Euler angles when we kno w the angular velocities. 4 System Modelling of Quadrotor Let’ s us take the configuration shown in Figure 5 for modelling the system. In this, the propellers 1 and 3 rotate in the same direction while propellers 3 and 4 rotate in opposite direction. This is done so, to overcome the imbalance caused moments of the rotating propellers. A quadrotor is an underactuated system. It has only 4 rotors facing in the same direction in order to manoeuvre through all 6 DOFs. This underactuation is tackled by e xecuting tw o or more operations (roll, pitch or yaw) at a time to mov e in the desired direction. Let us tak e the task of a quadrotor mo ving forward. When a quadrotor is hovering and is stationary , it has to produce enough ov erall thrust force (say F ) to equalize the gravitational force (say G ) acting on it, that is F = G . So, in order to mov e forward, it has to make a pitch θ . This creates a forward horizontal component F sinθ as sho wn in Figure 6 which enables the quadrotor to mo ve forward. But doing so reduces the vertical component to F cosθ . As we know from earlier , F cosθ 6 = G . So, the quadrotor will lose altitude and ov ercome it, we must increase the thrust force F accordingly to both move forward and not lose altitude. Similarly , all the other operations are executed with a combination of roll, pitch and yaw rotations to overcome the hurdle of underactuation. There are a fe w assumptions taken into account while modelling the configuration giv en in Figure 5 to make the modelling easier and they are : • It is rigid, that is there are no other mo ving parts other than the rotor . • It has a symmetrical design. • The propellers are equally spaced from the centre of the body . • V arious masses present in quadrotor (battery , circuit boards, etc.) are distributed equally with a centre of mass coinciding with the frame’ s centre. 9 Figure 5: Force diagram of a con ventional quadrotor . Figure 6: Thrust force diagram of a quadrotor moving in forward direction. 10 4.1 Kinematic Model The first step to model a system is to define the coordinate frames and do the necessary transformations. As shown in Figure 5, the earth inertial frame axes are N, E, D and the body frame axes are X, Y , Z. The inertial frame is fix ed to a specific place and the notations N-E-D represent North, East and Downw ards respecti vely . For the body frame of reference, the origin is located at the centre of the quadrotor’ s body with X, Y and Z axes pointing tow ards propellers 1, 2 and ground respectively . The orientation of the quadcopter is defined by the three Euler angles ( φ, θ , ψ ). The thrust force is defined from the body from of reference and we need a transformation matrix ( W R B - W orld frame of reference to Body frame of reference) to obtain that. First the rotation transformation is done about Z-axis followed by Y -axis and X-axis. By doing so, we get , W R B = R z ( ψ ) R y ( θ ) R x ( φ ) = " cos ψ sin ψ 0 − sin ψ cos ψ 0 0 0 1 # " cos θ 0 − sin θ 0 1 0 sin θ 0 cos θ # " 1 0 0 0 cos φ sin φ 0 − sin φ cos φ # = " cos θ cos ψ cos ψ sin θ sin φ − cos φ sin ψ sin φ sin ψ + cos φ cos ψ sin θ cos θ sin ψ cos φ cos ψ + sin θ sin φ sin ψ cos φ sin θ sin ψ − cos ψ sin φ − sin θ cos θ sin φ cos θ cos φ # (34) But we need a transformation from the body frame of reference to w orld frame of reference which can be obtained by in verting W R B . By doing so, we get, B R W = " cos θ cos ψ cos θ sin ψ − sin θ cos ψ sin θ sin φ − cos φ sin ψ cos φ cos ψ + sin θ sin φ sin ψ cos θ sin φ sin φ sin ψ + cos φ cos ψ sin θ cos φ sin θ sin ψ − cos ψ sin φ cos θ cos φ # (35) 4.2 Dynamic Model The quadrotor’ s motion can be divided into tw o subsystems - translational motion (X, Y and Z) and rotational motion (roll, pitch and yaw). The forces and moments acting on the quadrotor will be in vestigated using Ne wton-Euler formalism. In Figure 5, F 1 , F 2 , F 3 and F 4 are the thrust forces acting on the quadrotor while M 1 , M 2 , M 3 and M 4 are the moments caused by the rotation of the propellers. Also, τ x , τ y and τ z are the torques caused by the thrust forces about X, Y and Z axes respectiv ely . In some places, for the sake of alignment, " A B C # is written as [ A B C ] T . 4.2.1 T ranslational Equations of Motion According to Newton’ s la w , ~ F ω = d dt ( m. ~ v ) = m. ˙ v (36) where, ~ F ω = ~ G − ~ T . G is the gravity and T is the thrust generated by the propellers. As ~ T is described in body frame of reference, equation (35) is used to transform it to world frame of reference and the expression (36) can be e xpressed as, ˙ v = 1 m "" 0 0 mg # − B R W " 0 0 T ## (37) Now substitute (35) in (37), to get, 11 ˙ v = 1 m " − T (cos φ sin θ cos ψ + sin φ sin ψ ) − T (cos φ sin θ sin ψ − sin φ cos ψ ) mg − T (cos φ cos θ ) # (38) This vector equation can be written in terms of its components as, ¨ x = − T m (cos φ sin θ cos ψ + sin φ sin ψ ) ¨ y = − T m (cos φ sin θ sin ψ − sin φ cos ψ ) ¨ z = g − T m (cos φ cos θ ) (39) 4.2.2 Rotational Equations of Motion In the inertial frame, the moment can be defined as the time deriv ativ e of the angular momentum and can be expressed as follo ws, ~ M ω = d ~ L dt = d dx ( ~ I • ~ ω ) (40) where, ~ I is the body’ s inertia tensor . ~ I = " I xx I xy I xz I y x I y y I y z I z x I z y I z z # = " I xx 0 0 0 I y y 0 0 0 I z z # ] (41) In equation (41), the off-diagonal terms are zero due to the symmetric mass distrib ution assumption as mentioned in Section 4. According to Euler’ s rotational equation, ~ M b =  ~ I • ˙ ~ ω  + ~ ω ×  ~ I • ~ ω  = " τ x τ y τ z # (42) where, ˙ ~ ω = " ˙ p ˙ q ˙ r # and the right hand side is a vector of applied torques. Also, by simplifying ~ ω ×  ~ I • ~ ω  , we get, ~ ω ×  ~ I • ~ ω  = " p q r # × " I xx p I y y q I z z r # = " ( I z z − I y y ) q r ( I y y − I z z ) r p ( I y y − I xx ) pq # (43) Now , by substituting (43) in (42), we get, τ x = I xx ˙ p + ( I z z − I y y ) q r τ y = I y y ˙ q + ( I y y − I z z ) r p τ z = I z z ˙ r + ( I y y − I xx ) pq (44) Here p, q , r are actually equal to ˙ φ, ˙ θ , ˙ ψ since by definition, angular velocity of the body are same as time deriv ativ es of Euler angles of it .When the eqution (44) is rearranged, we get, 12 ˙ p = ¨ φ = ( τ x + I y y q r − I z z q r ) /I xx ˙ q = ¨ θ = ( τ y − I xx pr + I xx pr ) /I y y ˙ r = ¨ ψ = ( τ z + I xx pq − I y y pq ) /I z z (45) Using equation (33), we can get the deriv ativ es of the Euler angles as follows, ˙ φ = p + ( r cos( φ ) sin( θ )) / cos( θ ) + ( q sin( θ ) sin( φ )) / cos( θ ) ˙ θ = q cos( φ ) − r sin( φ ) ˙ ψ = ( r cos( φ )) / cos( θ ) + ( q sin( φ )) / cos( θ ) (46) 4.2.3 Aerodynamic Moment and thrust F orce As the propeller rotates, it generates a thrust force. This thrust force is directly proportional [ 4 ] to the square of the rotor speed and can be expressed as, F i = − K a ω 2 i (47) Apart from thrust force, the rotating propellers generate aerodynamic moment which is also directly proportional to the square of the rotor speed and can be expressed, M i = − K m ω 2 i (48) For the case of lo w altitude flight, the values of K a and K m in expression (47) and (48) can be considered as constants. The directions of the thrust force and aerodynamic moments generated by the propellers is illustrated in the Figure 5. Now , we can take the moment caused by the thrust force generated into account. Let’ s say that the length of the arms reaching out to the propellers from the centre of the body is l . Now , the total moment caused by thrust force about the X-axis is, τ x = − F 2 l + F 4 l = − ( K a ω 2 2 ) d + ( K a ω 2 4 ) l = K a l ( ω 2 4 − ω 2 2 ) (49) T otal moment caused by the generated thrust about Y -axis is, τ y = F 1 l − F 3 l = ( K a ω 2 1 ) l − ( K a ω 2 3 ) l = K a l ( ω 2 1 − ω 2 3 ) (50) The total moment about Z-axis is influenced only by the aerodynamic moment caused by the propellers (48) but not af fected by the thrust force and it can be expressed as, τ z = M 1 − M 2 + M 3 − M 4 = ( K m ω 2 1 ) − ( K m ω 2 2 ) + ( K m ω 2 3 ) − ( K m ω 2 4 ) = K m ( ω 2 1 − ω 2 2 + ω 2 3 − ω 2 4 ) (51) The total thrust force can be expressed as, T = − K a ( ω 2 1 + ω 2 2 + ω 2 3 + ω 2 4 ) (52) The negati ve sign in expression (52) indicates that the total thrust force is acting in the negati ve direction of Z-axis. Also, in this quadrotor configuration, thrust force exists only in the direction of Z-axis. 13 4.3 State Space Equation In general, the e volution of a dynamical system’ s states are gov erned by set of ordinary dif ferential equations. In control problems, these ordinary differential equations are often rearranged into state-space form, ˙ X = f ( X, U ) (53) where, X is a matrix of states and U is a matrix of inputs. The formation of state space equation in volves 3 steps : • Identify the go verning equations and and find the order ( n ) of it. • Define the state and input v ector . • Define the system of first-order dif ferential equations. 4.3.1 Gover ning Equations Equations (39) and (45) are the gov erning equations and are as follows, ¨ x = − T m (cos φ sin θ cos ψ + sin φ sin ψ ) ¨ y = − T m (cos φ sin θ sin ψ − sin φ cos ψ ) ¨ z = g − T m (cos φ cos θ ) (39) ˙ p = ¨ φ = ( τ x + I y y q r − I z z q r ) /I xx ˙ q = ¨ θ = ( τ y − I xx pr + I xx pr ) /I y y ˙ r = ¨ ψ = ( τ z + I xx pq − I y y pq ) /I z z (45) The order ( n ) of the all the gov erning equation is 2. 4.3.2 State and Input V ector The order of each gov erning equation is 2. Therefore, each gov erning equation needs ( n − 1) states, that is 2 states each. Therefore, in order to define the motion of a quadrotor , practically , we will need (2 × 6) states, that is 12 states - x, y , z , ˙ x, ˙ y, ˙ z , φ, θ, ψ , ˙ φ, ˙ θ , ˙ ψ . Therefore the matrix of the states of the system is, X = [ x 1 x 2 x 3 x 4 x 5 x 6 x 7 x 8 x 9 x 10 x 11 x 12 ] T =  x y z φ θ ψ ˙ x ˙ y ˙ z ˙ φ ˙ θ ˙ ψ  T (54) The matrix of the inputs to the system is, U = [ U 1 U 2 U 3 U 4 ] =  ω 2 1 ω 2 2 ω 2 3 ω 2 4  (55) 4.3.3 System of first-order equations The first 6 equations simply related states to each other by finding their deriv ativ es like, 14 dx 1 dt = dx dt = ˙ x = x 7 dx 2 dt = dy dt = ˙ y = x 8 dx 3 dt = dz dt = ˙ z = x 9 dx 4 dt = dφ dt = ˙ φ = x 10 dx 5 dt = dθ dt = ˙ θ = x 11 dx 6 dt = dψ dt = ˙ ψ = x 12 The next 6 equations can be derived by dif ferentiating and rearranging the above 6 first-order differential equations lik e, dx 7 dt = d ˙ x dt = ¨ x dx 8 dt = d ˙ y dt = ¨ y dx 9 dt = d ˙ z dt = ¨ z dx 10 dt = d ˙ φ dt = ¨ φ dx 11 dt = d ˙ θ dt = ¨ θ dx 12 dt = d ˙ ψ dt = ¨ ψ Now , by substituting the v alues of values for ¨ x, ¨ y , ¨ z , ˙ φ, ˙ θ , ˙ ψ , ¨ φ, ¨ θ , ¨ ψ , from the the equations (39), (45) and (46) onto the above 12 first-order dif ferential equationa=s, we get our state space equation in the form { ˙ X = f ( X, U ) } as,                      ˙ X 1 ˙ X 2 ˙ X 3 ˙ X 4 ˙ X 5 ˙ X 6 ˙ X 7 ˙ X 8 ˙ X 9 ˙ X 10 ˙ X 11 ˙ X 12                      =                   f 1 f 2 f 3 f 4 f 5 f 6 f 7 f 8 f 9 f 10 f 11 f 12                   =                    ˙ x ˙ y ˙ z ˙ φ ˙ θ ˙ ψ ¨ x ¨ y ¨ z ¨ φ ¨ θ ¨ ψ                    =                   x 7 x 8 x 9 p + ( r cos( φ ) sin( θ )) / cos( θ ) + ( q sin( θ ) sin( φ )) / cos( θ ) q cos( φ ) − r sin( φ ) ( r cos( φ )) / cos( θ ) + ( q sin( φ )) / cos( θ ) − T m (cos φ sin θ cos ψ + sin φ sin ψ ) − T m (cos φ sin θ sin ψ − sin φ cos ψ ) g − T m (cos φ cos θ ) ( τ x + I y y q r − I z z q r ) /I xx ( τ y − I xx pr + I xx pr ) /I y y ( τ z + I xx pq − I y y pq ) /I z z                   Therefore, the non-linear state-space equation for a 3-D quadrotor has been deriv ed. It is non-linear due to the presence sine and cosine terms in it. 5 Conclusion Dynamics of a 3-D quadrotor and its state-space equation has been deri ved from the grass root level with detailed explanation and t he logical reasoning behind e very step ha ve been stated clearly . As 15 a future work, the dynamical system that is modelled in this paper can be used in conjunction with concepts of control system theory to mak e filters and controllers that can mak e the quadrotor follo w a trajectory or tackle any kind of task with precision and robustness using MA TLAB. Apart from that, the concepts of Quaternions [ 5 ] can be used in the modelling of the system to make the computations easier . Acknowledgments This paper includes a collecti ve kno wledge majorly from the lectures of Dr .V ijay Kumar and Sarah T ang from Uni versity of Pennsylvania and the master thesis work [ 3 ] of Haifeng L yu from Uni versity of Rhode Island. Also, this paper would ha ve not been possible without the continued guidance of Dr . KP . Soman of CEN, Amrita V ishwa V idyapeetham, India. References [1] Roger W Brockett. Robotic manipulators and the product of exponentials formula. In Mathematical theory of networks and systems , pages 120–129. Springer , 1984. [2] Leonhard Euler . Formulae generales pro translatione quacunque corporum rigidorum. Novi Commentarii academiae scientiarum P etr opolitanae , pages 189–207, 1776. [3] Haifeng L yu. Multi variable control of a rolling spider drone. 2017. [4] Paul Pounds, Robert Mahon y , Joel Gresham, Peter Corke, and Jonathan M Roberts. T owards dynamically- fa vourable quad-rotor aerial robots. In Proceedings of the 2004 Austr alasian Conference on Robotics & Automation . Australian Robotics & Automation Association, 2004. [5] Stanley W ayne Shepperd. Quaternion from rotation matrix.[four-parameter representation of coordinate transformation matrix]. 1978. 16