The Cubli: Modeling and Nonlinear Attitude Control Utilizing Quaternions

This paper covers the modeling and nonlinear attitude control of the Cubli, a cube with three reaction wheels mounted on orthogonal faces that becomes a reaction wheel based 3D inverted pendulum when positioned in one of its vertices. The proposed approach utilizes quaternions instead of Euler angles as feedback control states. A nice advantage of quaternions, besides the usual arguments to avoid singularities and trigonometric functions, is that it allows working out quite complex dynamic equations completely by hand utilizing vector notation. Modeling is performed utilizing Lagrange equations and it is validated through computer simulations and Poinsot trajectories analysis. The derived nonlinear control law is based on feedback linearization technique, thus being time-invariant and equivalent to a linear one dynamically linearized at the given reference. Moreover, it is characterized by only three straightforward tuning parameters. Experimental results are presented.


I. INTRODUCTION
Inverted pendulum systems have been a popular demonstration of using feedback control to stabilize open-loop unstable systems. Introduced back in 1908 by Stephenson [1], the first solution to this problem was presented only in 1960 with Roberge [2] and it is still widely used to test, demonstrate and benchmark new control concepts and theories [3]- [9].
Differently from cart-pole inverted pendulums, that have a controlled cart with linear motion (Fig. 1a), reaction wheel pendulums have a controlled rotating wheel that exchanges angular momentum with the pendulum (Fig. 1b). First introduced in 2001 by Spong et al. [10], it was soon adapted to a 3D version by Lee and Goswami in 2007 [11].
Perhaps, even most notable is the Cubli (Fig. 2). Originally developed and baptizedin 2012 by Gajamohan et al. [12], [13] from the Institute for Dynamic Systems and Control of Zurich Federal Institute of Technology (ETH Zurich), the Cubli is a device that consists of a cube with three reaction wheels mounted on orthogonal faces. By positioning it in one of its vertices, it becomes a reaction wheel based The associate editor coordinating the review of this manuscript and approving it for publication was Zhong Wu . 3D inverted pendulum. This method of utilizing reaction wheels is similar to the one used for decades to stabilize satellites and spacecraft in space [14]- [16], but due to gravity and surface friction, the dynamics of these systems are somewhat different.
The purpose of this paper is first to model the system, and then design and implement a nonlinear attitude controller for it. Although the ETH team has already done this [17], [18], the novel of this work is the use of quaternions as feedback control states. In addition, ETH's attitude estimator utilizes just accelerometers [19], [20], which is only possible due to the fact that the Cubli has a non-accelerated pivot point and, thus, angular and centripetal acceleration terms can be dismembered. However, a disadvantage of this approach is that VOLUME 9, 2021 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ it requires six IMUs spread throughout the entire structure in previously known positions. The Cubli of this work utilizes an accelerometer together with a rate gyroscope in a quaternion based complementary filter, which requires only one IMU placed anywhere and yields equally satisfactory results. Quaternions were used for simulation of the rotational motion of rigid bodies as early as 1958 by Robinson [21], but it was only a decade later, in 1968, that some early results on the use of quaternions as feedback control states was shown by Mortensen [22]. In fact, it was Meyer that introduced the attitude control theory a few years earlier, in 1966, but he utilized rotational matrices instead [23]. In1985, Wie and Barba proposed a linear decoupled model-independent control law, also utilizing quaternions [24]. Although different, all of them used Lyapunov control theory. A disavantage of it is that the control law is based on intuition rather than fundamental principles. Moreover, important concepts such as damping and loop bandwidth are not well defined as in linear control theory.
Dwyer, in 1984 [25], and Slotine and Li, in 1991 [26], approached this problem utilizing a nonlinear transformation to realize an exact linear model of the rotational dynamics to which linear control can be applied, a method that is also called feedback linearization or dynamic inversion. The first utilized the Gibbs vector, whereas the second utilized Euler angles. The main problem here is that both parametrizations are singular at certain attitudes, even with no attitude error.
It was in 1993 that Paielli and Bach proposed an approach which incorporates features of these others while avoiding their main problems [27], [28]. They utilized the same dynamic inversion as Dwyer, Slotine and Li, but with quaternions in the rotation dynamics, which are globally nonsingular, just as Mortensen and Wie et al. However, the Gibbs vector was utilized in the error dynamics because they have no nonlinear mathematical constraints to prevent the realization of linear error dynamics.
The control law derived in this paper utilizes this same approach. Although it was proposed three decades ago, it continues to be considered state of the art control law in the field, with few practical applications even today. Most of nowadays aircraft and even some spacecraft are designed from a set of linear plant models and implemented with a gain-scheduled linear controller [29]. The first aircraft to utilize dynamic inversion control was the Lockheed Martin F-35, released in 2006 and currently the most advanced fighter jet in service. This scenario has started to change in the last years with the astonishing growth of nanosatellites and commercial UAVs [30]- [34].
Although this control technique is not novel, the implementation of it in the Cubli, as far as the authors know, has not been presented is the literature before. Moreover, a nice advantage of quaternions, besides the usual arguments to avoid singularities and trigonometric functions, is that it allows working out quite complex dynamic equations completely by hand utilizing vector notation [35]. This becomes evident in this paper, where the entire modeling and nonlinear control law is derived without the need for any mathematical symbolic software in a very didactic and self-contained way, being a contribution to control education as well.

II. MODELING
The Cubli is composed of four rigid bodies: a structure and three reaction wheels (Fig. 3). The structure rotates freely around pivot point O (articulation vertex), whereas each reaction wheel, besides rotating together with the structure, also rotates around its axial axis. There are other bodies, such as motors, batteries, microcontrollers, etc., that can be interpreted as being part of one or other of them. The exceptions are the motors, whereby their stators are considered part of the structure whereas their rotors are considered part of the reaction wheels.

A. KINEMATICS
Unlike Euler angles, based on the property that any orientation of a rigid body can be described with a sequence of three consecutive rotations around a predefined axis, quaternion rotation is based on the property that any orientation of a rigid body can be described with a single rotation around the real eigenvector of the transformation matrix between body and inertial axes. For this, quaternions require four parameters: three to describe the eigenaxisê coordinates plus one to describe the rotation angle φ.
Quaternions possess a not very common and somewhat complex algebra, that is going to be briefly described in this section. Before diving into quaternion algebra, the rotation around a specific axis (the property which quaternions are based on) will be derived.

1) SPACIAL ROTATION
Let r be an arbitrary vector to be rotated around a unit vectorê by an angle φ generating a rotated vector r (Fig. 4). Projection vectors v 1 , v 2 , v 3 and v 4 can be written in terms of vector r, unit vectorê and angle φ as The rotated vector r can be written in terms of projection vectors such that Substituting (1) to (4) in (5) results in Equation (6) is the Rodrigues' rotation formula that describes the rotation of a vector r by an angle φ along a unit vectorê.

2) QUATERNION FUNDAMENTALS
Quaternion algebra can be generated from the following properties By left and right multiplying (7), together with associativity and distributivity, the following multiplication rules arise as it can be seen, the product is non-commutative.

3) QUATERNION NOTATION
A quaternion q is a set of four parameters, a real value q 0 and three imaginary values q 1 , q 2 and q 3 , such that A quaternion can also be represented as a four-dimensional column vector composed of a real value q 0 and a vectorial imaginary value q = q 1 q 2 q 3 T , such that The conjugate of a quaternion is defined as and its norm (a non-negative real value) as

4) QUATERNION PRODUCT
From the rules given in (7) and (8), the product of two quaternions q and r (represented by the • operator) can be derived Since (13) is linear in r, it can also be written in matrix-vector product form whereq is the rotation quaternion vector represented as a skew-symmetric matrix corresponding to its cross product Note that the matrix corresponding to its quaternion product can also be written as where From (13), it can be seen that and Moreover, if the quaternion has unit norm, i.e. |q| = 1, two other properties are valid and VOLUME 9, 2021

5) ROTATION QUATERNION
Let r be an arbitrary fixed vector described in an inertial coordinate frame O : {x, y, z} (Fig. 5a) and r be this same vector but described in a body fixed coordinate frame O : {x , y , z } (Fig. 5b). To transform r into r , consider the following quaternion multiplication where r and r are quaternions with no real part and with vectors r and r in their imaginary part and q is the rotation quaternion whose components are defined in terms of the eigenaxisê and rotation angle φ, such that Note that, because the eigenaxis has unit norm, i.e. |ê| = 1, the rotation quaternion also has unit norm, i.e. |q| = 1.
Substituting (23) and (24) into (22) results in which is the Rodrigues' rotation formula derived in (6). For the inverse transformation, one just needs to swap the rotation quaternion with its conjugate Moreover, since vector r is fixed in the inertial coordinate frame, a rotation quaternion q can be used to represent the rotation of the body fixed coordinate frame with respect to the inertial coordinate frame where R(q) is the rotation matrix in terms of the rotation quaternion, given by

6) KINEMATIC EQUATION
Let us suppose now that the body fixed coordinate frame is in rotational motion around the origin O (Fig. 6). Its angular velocity vector ω is given by Note that this is the angular velocity with respect to the inertial coordinate frame but described in the body fixed coordinate frame axes.
Let ω be a quaternion with no real part and with the vector ω in its imaginary part Since vector r is fixed in the inertial coordinate frame, its time derivative, as seen by the inertial coordinate frame, is zeroṙ In turn, its time derivative, as seen by the body fixed coordinate frame, depends on the body-fixed coordinate frame angular velocity vectoṙ The minus sign appears because, if the body coordinate frame rotates in one direction, the vector will be seen by the body coordinate frame as rotating in the opposite direction.
Since quaternions r and ω have no real part, (32) is equivalent toṙ Differentiating (22) and using (26) and (31), yieldṡ Comparing (34) with (33), it is possible to obtain the angular velocity quaternion in terms of the rotation quaternion and its time derivative which can also be rewritten by making use of (18) as By left-multiplying both sides of (35) with q and using (20), the rotation quaternion time derivativeq can be isolatedq Equation (37) is the rotational kinematic equation of a rigid body utilizing quaternions.
Because quaternion ω has no real part, (35), (36) and (37) can also be written in vector notation utilizing (16) as and also from (16), it is possible to demonstrate that

B. KINETICS
Is this section, the kinetic equations of the Cubli will be derived utilizing the Lagrange equations.

1) KINETIC ENERGY
The Cubli total kinetic energy is the sum of the kinetic energy of each moving body where T s is the kinetic energy of the structure and T wi is the kinetic energy of the i-th reaction wheel.
Let ω s be the structure angular velocity vector described along the body fixed coordinate frame but with respect to the inertial coordinate frame (Fig. 7), given by Let ω w1 , ω w2 and ω w3 be the reaction wheels relative angular velocity vectors described in and with respect to the body fixed coordinate frame (Fig. 7), given by Note that these angular velocity vectors are relative; hence, to obtain the reaction wheel angular velocity vector with respect to the inertial coordinate frame, the structure angular velocity vector needs to be added (since the reaction wheels are rotating together with the structure).
The structure can be approximated to a cube of side length l, mass m s and moment of inertia around its principal axes I s xx = I s yy = I s zz , whereas each reaction wheel can be approximated to a disc of mass m w , moment of inertia around its axial principal axis I w xx and moment of inertia around its perpendicular principal axes I w yy = I w zz . These parameters were obtained from the CAD version of the Cubli and are given in Table 1. Let I s G be the structure inertia tensor on its center of mass G s with respect to the x y z axes and r s be the vector from pivot point O to the structure center of mass G s (Fig. 8a), given by (45) Let I w1 G be reaction wheel 1 inertia tensor on its center of mass G w1 with respect to the x 1 y 1 z 1 axes and r w1 be the vector from pivot point O to reaction wheel 1 center of mass G w1 (Fig. 8b) given by Since all three reaction wheels are identical and differ only in their position, orientation and axis around which they rotate, it can be inferred that With all these values, it is possible to calculate I s O and I wi O , the structure and i-th reaction wheel inertia tensor on pivot point O, respectively, with respect to the x y z axes, by applying the Huygens-Steiner theorem Thus, the total kinetic energy of the Cubli is Because each reaction wheel rotates around an axis orthogonal to each other, (51) can be simplified to where ω c is Cubli angular velocity vector, which is the same as the structure ω w is the composition of all three relative angular velocities vectors of the reaction wheels I w is the net inertia tensor of the three reaction wheels around each of their individual rotational axis andĪ c is the Cubli total inertia tensor I c O on pivot point O, subtracting the net inertia tensor I w previously defined The Cubli total potential energy is given by where V s is the potential energy of the structure and V wi is the potential energy of the i-th reaction wheel. Let g be the acceleration of gravity vector described in the inertial coordinate frame (60) In the body fixed coordinate frame, it is simply the rotation of the previous vector Thus, the total potential energy of the Cubli is which can be simplified to where m c is the Cubli total mass and r c is the vector from pivot point O to the Cubli center of mass G c

3) LAGRANGE EQUATIONS
The Lagrangian, L = T − V , is given from (52) and (63) The kinetic equations of the Cubli can then be obtained applying the Lagrange equations d dt where Q i is the generalized coordinates of the system, and F Q i the generalized forces in the Q i direction.
There are two generalized coordinates of interest in the Cubli: the rotation quaternion q and the reaction wheels relative angular displacement vector θ w .
As for the generalized forces, there is only the vector of torques τ from the motors applied on each of the three reaction wheels, given by These torques occur in the same direction as the reaction wheels relative angular displacement, which means that For each one of these generalized coordinates, there will be one kinetic equation to be calculated separately.

4) GENERALIZED COORDINATE q
Applying the Lagrange equations for Q i = q, the Lagrangian can be written substituting the angular velocity vector ω c with (38), when differentiating with respect toq, and with (39), when differentiating with respect to q. This yields the following kinetic equation . (71) Equation (70) can be further simplified by left multiplying it with 1 2 G(q) and making use of (21) and (41) When applying the Lagrange equations for Q i = θ w , it is possible to rewrite the Lagrangian substituting the angular velocity vector ω w with˙ θ w , which yields the following kinetic equation Because ω w ω c , the approximation ( ω c + ω w ) ≈ ω w was applied. Moreover, since the reaction wheels angular velocities are relative (measured with respect to the structure), their gyroscopic torques have no influence on them, only on the Cubli. Hence, the reaction wheels gyroscopic torques in (74) were disregarded.
For the model to become even more realistic, it is necessary to include friction forces. There are two main frictions in this model: the friction between the Cubli and the surface at pivot point O, and the friction of the motors.
The surface friction can be modeled by a viscous friction coefficient b. However, since it occurs only in the direction orthogonal to the gravity vector, it depends on the orientation of the Cubli.
The motor friction is somewhat more complicated. Besides having viscous friction, it also has static friction (that generates a dead zone) and aerodynamic drag (since the reaction wheels are hollow). It can be modeled as where τ c is the Coulomb friction, b w is the viscous friction coefficient and c d is the aerodynamic drag coefficient. Those parameters were identified experimentally with a torque controller by varying the torque reference, registering the equivalent steady-state velocity (where the input torque equals the friction torque), and curve fitting of the data (Fig. 9). The identified parameters are given in Table 2. Including the kinematic equation (37) and friction forces in (75), the full equations of motion are obtained (77) VOLUME 9, 2021 where The system can also be represented in a block diagram (Fig. 10), where it is easier to interpret the gyroscopic terms, gravity torque, surface friction and motor friction. Note that the Cubli and the reaction wheel dynamics are coupled by the gyroscopic terms and motor friction. In steady-state conditions, angular velocities are close to zero and thus they are coupled only by the viscous friction of the motors.

D. VALIDATION
The model was validated by means of computer simulations, and they were divided into three main types. In all of them, friction forces are being despised.

1) INVARIANT ANALYSIS
Invariant analysis considers parameters that must remain unchanged with time when no forces are being applied. The Cubli has three invariants: • Total mechanical energy E • Angular momentum projection in the gravitational field direction H z • Angular momentum projection in the gyroscopic axis direction (diagonal axis of the Cubli) H z The total mechanical energy, E = T +V , is given from (52) and (63) Assuming the Cubli is initially aligned with the inertial coordinate frame, i.e. q(0) = 1 0 T , with no initial angular velocities, i.e. ω c (0) = ω w (0) = 0, it results in the motion presented in Fig. 11. Because quaternions do not have an intuitive physical meaning, it is just possible to infer that the Cubli presented some kind of periodic motion. However, since the objective is only to analyze its energy, this is not a problem. The mechanical energy, presented in Fig. 12, remained unchanged. As Cubli lost potential energy, it acquired the same amount of kinetic energy, and vice-versa. This not only confirms the hypothesis of periodic motion, but also ensures that the model is consistent. For the angular momentum projection invariants, the reaction wheels were assumed to be fixed. The angular momentum vector is so that its projections are simply given by Let us assume the same initial conditions, but now with an arbitrary initial angular velocity, i.e. ω c (0) = 1. As it can be seen in Fig. 13, the angular momentum projection on the gravitational field and gyroscopic axis directions remained unchanged. Singular motions consider pre-defined initial conditions in which the behavior of the system can be predicted. They will be divided into static equilibrium, whereby the system states must remain unchanged, and dynamic equilibrium, whereby the system states change as expected. The Cubli has two static equilibrium positions: stable and unstable, as it can be seen in Fig. 14. Note that the stable one is only being considered for simulation purposes, since the Cubli would never be under the xy plane. The rotation quaternions corresponding to these orientations are Since the Cubli can rotate around its diagonal axis and still be in an equilibrium position, there are infinite other equivalent rotation quaternions. In one simulation, the Cubli was considered initially on its stable equilibrium position, i.e. q(0) = q s , whereas in the other, it was considered in its unstable equilibrium position, i.e. q(0) = q u , both with no initial angular velocities, i.e. ω c (0) = ω w (0) = 0. In both cases, rotation quaternions remained unchanged, confirming that these are in fact static stable positions.
The Cubli has many dynamic equilibrium motions, the most well-known being those like the spinning top motion. Two of them will be analyzed: the single spin motion and the precession, nutation, and spin motion. The first occurs when the Cubli is in its static equilibrium position (either stable or unstable) but spinning around its diagonal axis (Fig. 15a), whereas the second occurs when the Cubli center of mass vector r c is not perfectly aligned with the z axis in the inertial coordinate frame, so it spins around its diagonal axis and also precesses around the z axis in the inertial coordinate frame (Fig. 15b). All these simulations were performed utilizing quaternions, but for the ease of representation, the results were converted to precession, nutation and spin angles. Moreover, the reaction wheels were assumed to be fixed.
For the single spin motion, the same initial conditions, i.e. q(0) = q u , were assumed, but now with an initial angular velocity, i.e. ω c (0) = 2π 1 √ 3 , meaning it is spinning.
Results are presented in Fig. 16. The spin angle kept increasing whereas the precession and nutation angles remained unchanged, meaning that the Cubli only rotated around its diagonal axis. Moreover, Cubli rotated at exactly 2π rad/s (1Hz), which agrees with the initial angular velocities. To simulate the precession, nutation and spin motion, a non-equilibrium rotation quaternion q ne was calculated considering a somewhat small nutation angle (10 • ). Considering this new rotation quaternion as an initial condition, i.e. q(0) = q ne , and with initial angular velocities 10 times faster (to guarantee it precesses), i.e. ω c (0) = 20π 1  whereas the precession and spin angle kept increasing. Moreover, the spin velocity is clearly higher than the precession velocity, which is, in fact, expected in the spinning top motion. Although the initial spin velocity is 10 times that of the previous simulation, the frequency is not 10 times higher, meaning that the spin is now somewhat slower. This is because the Cubli is now also performing a gyroscopic precession.
Another interesting graph of the same simulation is a three-dimensional position of the Cubli center of mass, which can be seen in Fig. 18. Although not in scale, it gives a clear perspective of the spinning top motion. To completely validate this motion, they will be compared to the well-known [36] where I o is the Cubli maximum principal moment of inertia, around axis 1 and 2, whereas I is the minimum principal moment of inertia, around axis 3 (Fig. 19). These moments of inertia are the eigenvalues of the Cubli inertia tensorĪ c and are given by Considering the same previous initial conditions, but now in terms of Euler angles, i.e. ψ(0) = φ(0) =ψ(0) = θ(0) = 0, θ(0) = 10 • andφ(0) = 20π, and simulating (83), the result is the same of Fig. 17, confirming that the dynamic equations are consistent.
Next, the steady precession case is considered. For this motion to happen, the Cubli should have constant spin and precession velocities, and also a constant nutation angle, i.e. ψ =φ =θ = 0. This simplifies (83) to a single equation From (85), it is possible to calculate the precession velocity in terms of the spin velocity and nutation anglė Note that, for this equation to be valid, the square root term must be real, meaning that there is a minimum spin velocity needed for steady precessioṅ The spin velocity and nutation angle of the previous simulation satisfy (87), but for the Cubli to present steady precession, the precession velocity from (86) should be ψ 1 = 4.40 rad/s or ψ 2 = 22.19 rad/s. Assuming the same initial conditions but now with ψ(0) = ψ 1 , which means that ω c (0) = 38.47 38.47 39.40 T , and simulating (77) instead of (83), yield the results presented in Fig. 20. As it can be seen, the Cubli is now clearly in steady precession, showing once again the consistency of the model.

3) POINSOT TRAJECTORIES
Poinsot trajectories are a geometrical method for visualizing the torque-free motion of a rotating rigid body [37]. Since the system needs to be in torque-free motion, gravity will be neglected. The conservation of angular momentum implies that in the absence of applied torques, H is conserved in an inertial coordinate frame ( d H dt = 0). The conservation of energy implies that in the absence of input torques and energy dissipation, T is also conserved ( dT dt = 0). Considering the principal axes, it is possible to write H = I o ω 1 I o ω 2 I ω 3 T , so that the total angular momentum is simply the magnitude of this vector The angular kinetic energy, also considering the principal axes, is given by Writing (88) and (89) in terms of the angular momentum vector components along the principal axes yields which are equivalent to two constraints for the 3D angular momentum vector H . The angular momentum constrains H to lie on a sphere, whereas the kinetic energy constrains H to lie on an ellipsoid. These two surfaces intersections define the possible solutions for H . Simulations in Fig. 21a considered various initial angular velocities, all with the same total H . Each line or dot is a different simulation and represents an intersection with the kinetic energy ellipsoid. The surface created is clearly a sphere, which is expected for a constant angular momentum. Moreover, because each simulation has constant H 3 , the body is axisymmetric along this axis, which is in fact the case for the Cubli. Considering now the same kinetic energy T , yields Fig. 21b. In this case, the surface is an ellipsoid, which is also expected for constant kinetic energy. Now each line or dot represents an intersection with the angular momentum sphere. Moreover, because two moments of inertia are the same and the third one is smaller than the other two, the shape is in fact a prolate spheroid, which is a particular case of an ellipsoid.

III. ANALYSIS
Before diving into the control of the Cubli, its stability and controllability properties will be analyzed.

A. LINEARIZED DYNAMICS WITHOUT REACTION WHEELS
When the Cubli is at rest, i.e. ω c = 0, perfectly balanced on its unstable equilibrium position, i.e. q = q u , the linearized dynamics, despising the reaction wheels dynamics, are where ω n 0 is the natural frequency of the roll and pitch dynamics, whereas ω n 1 is the natural frequency of the yaw dynamics, given by Note thatĪ c xx −Ī c xy andĪ c xx + 2Ī c xy are the Cubli principal moments of inertia derived in (84), which are, in fact, the moments of inertia around the roll and pitch motion and around the yaw motion.
Although quaternions have been utilized the whole time, the characteristic equation of the linearized dynamics is clearly described in terms of Euler angles. Roll and pitch dynamics are unstable due to its poles being located at ±ω n 0 , whereas yaw dynamics are marginally stable due to its poles being located at 0 and −ω n 1 . Moreover, there is also an extra pole at 0, which is inherited from the rotation quaternion kinematic equation since a rotation quaternion is a redundant way to describe an orientation.
The controllability matrix has rank(C) = 6, whereas the system has dimension n = 7. However, even that rank(C) = n, the system is fully controllable since one of the system states is redundant due to quaternion representation. In other words, although quaternions are being utilized (which includes an extra redundant state), the system still has 3 d.o.f. and thus its ''physical'' dimension remains n = 6.

B. LINEARIZED DYNAMICS WITH REACTION WHEELS
Let us now consider the full linearized dynamics, that is, without despising the reaction wheels where The characteristic equation is almost the same but with an extra term from the reaction wheels dynamics where ω n 2 is the natural frequency of the reaction wheel dynamics, given by The reaction wheel dynamics are marginally stable due to its poles being located at 0 and ω n 2 . Although the viscous friction of the motors couples the Cubli and the reaction wheel dynamics, it does not interfere in the linearized roll, pitch and yaw dynamics since its poles remained unchanged.
The controllability matrix now has rank(C) = 11, whereas the system now have dimension n = 13. Despising the quaternion redundancy, its ''physical'' dimension is n = 12, which means that now the system is not fully controllable.

IV. CONTROL
Although it is possible to control the yaw motion or reaction wheel motion, it is impossible to control both simultaneously. As will be shown further, this is due to the Cubli symmetry around the yaw axis. One way to deal with this problem is to decouple the yaw dynamics and do not try to control it, thus, viscous friction with the surface will make the open-loop yaw dynamics marginally stable, as shown in [18]. An alternative way, considered here, is to implement a trajectory control for the yaw axis to track a sinusoidal like signal with zero-mean.

A. ATTITUDE CONTROLLER
Initially, one will focus only on the Cubli dynamics, without care about controlling the wheels.

1) FEEDBACK LINEARIZATION
Adopting a new input vector u and making the input torque τ equal to a feedback linearization law that cancels out all the gyroscopic terms, gravity torque, surface friction and motor friction is obtained (Fig. 22).
By substituting (99) into (77), it reduces the system to Although the angular velocity differential equation is now linear, the rotation quaternion differential equation is still nonlinear.

2) STATE REGULATOR
Let q r be an orientation quaternion reference and q e be an orientation quaternion error, such that The orientation error represents the rotation needed from current orientation to match the orientation reference. In quaternion notation, consecutive rotations can be represented as multiplications between respective orientation quaternions, which means that By left-multiplying both sides of (102) withq and using (20), it is possible to isolate orientation quaternion error q e =q • q r .
Let ω r be an angular velocity vector reference and ω e be an angular velocity vector error, which can be represented as quaternions with no real parts, such that The angular velocity error represents the difference between angular velocity reference and current angular velocity. However, because there is also a difference between orientations, the angular velocity reference must be rotated from orientation reference to the current orientation which, in vector format, is the same as When current orientation matches the orientation reference, no additional rotation is needed and thus the orientation quaternion error is q e = 1 0 T . Because q e is not zero (and will never be since a orientation quaternion always have unit norm), (107) could not be used to guarantee asymptotically stable error dynamics However, the vector part of the orientation quaternion error will be zero, which means that (108) could be used instead q e + k d˙ q e + k p q e = 0, The first time derivative of q e is obtained by differentiating (103)q where its vector part is given bẏ On the other hand, the second time derivative of q e can be calculated differentiating (109) where its vector part is given bÿ Substituting (110) and (112) into (108) yieldṡ The term q e q e 0 is the Gibbs vector error σ e , given by This vector is singular for φ e = ±180 • , which appears to be a disadvantage of utilizing rotation quaternions as feedback control states. However, the biggest possible attitude error between two orientations is 180 • , and if it is necessary to go to a reference in the longest path, a trajectory control may be utilized.
The time derivative of ω e is obtained differentiating (105) where its vector part is given bẏ Isolating˙ ω c in (116) and substituting (113) in it yields the following control law This control law was derived at NASA Ames Research Center back in 1993 by Paielli and Bach [27], [28] for spacecraft attitude control.
For small rotations, the term ω T e ω e is close to zero, q e 0 is close to one, ω e × ω c is close to zero and R(q e ) T is close to identity, which further simplifies the control law u ≈ 2k p q e + k d ω e +˙ ω r . (118) Moreover, the orientation quaternion error and angular velocity error vectors are approximate to the Euler angles errors Substituting (119) into (118) yields a state regulator that is equal to the one commonly utilized with Euler angles when dealing with small rotations This means that, for small rotations, the derived nonlinear control law of (117) is equivalent to a linear one dynamically linearized at the reference position.

3) CONTROLLER GAINS
Substituting (117) into (100) and rewriting the first differential equation in terms of the Gibbs vector error and no longer in terms of quaternions yields When the Cubli is in its unstable equilibrium position, i.e. σ e = ω e =˙ ω r = 0, the closed loop linearized dynamics are Its characteristic equation is Comparing (123) with the characteristic equation of a generic 2 nd order system with two complex poles

B. ATTITUDE AND WHEEL CONTROLLER
Since the Cubli is influenced by the acceleration of the reaction wheels, it may happen that one reaction wheel velocity saturates for a while. Moreover, the Cubli construction may be imperfect, or attitude sensors may be misaligned, so what seems to be an equilibrium position may not be, and the reaction wheels will keep accelerating trying to maintain that erroneous equilibrium. It is thus desirable to achieve the dual goals of stabilizing the Cubli and keep the reaction wheels velocities small.

1) STATE REGULATOR
To achieve this, the control law of (117) must be slightly modified by also having feedback from the reaction wheels angular displacements and velocities, such that The full nonlinear control law (Fig. 23) is composed of the feedback linearization from (99) and the state regulator from (126).
Note that, if the objective is just to stabilize the Cubli, i.e. ω r =˙ ω r = 0, the control law reduces to 2) CONTROLLER GAINS Substituting (126) into (100) yields When the Cubli is in its unstable equilibrium position, i.e. σ e = θ w = ω e = ω w =˙ ω r = 0, the closed loop linearized dynamics are where and Its characteristic equation is Note that, by varying just the controller gains k p , k d , k p w and k d w , it is possible to freely allocate yaw or roll and pitch closed-loop poles, but never both simultaneously. What could be done is to allocate the roll and pitch poles in the desired locations and just ensure that the yaw dynamics remains stable. However, even this is not possible. Due to the negative sign in term k p w , reaction wheel gain k p w would need to be negative to make yaw dynamics stable, but then roll and pitch dynamics would become unstable. There is no way out. Physically speaking, gravity acts as a non-restorative force in the roll and pitch dynamics, whereas surface friction acts as a restorative force in the yaw dynamics. This inversion of signs is exactly what makes the system not controllable.
However, there is a way to at least mitigate this problem. If the parameter or controller gain k p w are equal to zero, the yaw dynamics would be marginally stable. So, two things can be done: first is to position the Cubli on a smooth surface with the least possible friction to make b, and consequently , small; second is to reduce k p w the maximum, but without compromising the roll and pitch dynamics.
Comparing the roll and pitch terms of (132) with the characteristic equation of a generic 4 th order system with two complex poles and two repeated real poles, given by s 2 + 2ζ ω n s + ω 2 n (s + αζ ω n ) 2 = 0, which is equivalent to s 4 + 2ζ ω n (1 + α) s 3 + ω 2 n 1 + αζ 2 (4 + α) s 2 + 2αζ ω 3 n 1 + αζ 2 s + α 2 ζ 2 ω 4 n = 0, (133) yields the following values for the controller gains in terms of the desired closed-loop dynamics ζ , ω n and α and parameters β, γ and δ Note that, if α = 0, the controller gains k p and k d are equal to those ones derived in (125), whereas the controller gains k p w and k d w are equal to zero. By choosing a small enough value of α, one guarantees that the reaction wheel dynamics would be slow enough to not interfere in the Cubli dynamics. In other words, the Cubli roll and pitch closed-loop poles will be sufficient faster than the reaction wheel closed-loop poles.

V. EXPERIMENTAL RESULTS
To validate the derived control law, experiments were carried out with the Cubli prototype (Fig. 2). Its electronics is composed of one STM32 NUCLEO-L432KC development board (80MHz ARM 32-bit Cortex M4), one SparkFun 9DoF Sensor Stick inertial measurement unit (LSM9DS1), three Maxon EC 45 Flat brushless motors with a Maxon ESCON Module 50/5 dedicated motor controller each and one Turnigy Graphene Panther 1000mAh 6S LiPo battery. The microcontroller runs ARM Mbed OS open-source operating system, communicates with the IMU with I2C serial communication protocol and with the motor controllers with PWM, for current set-point, and analog signals, for hall sensor readings.
A dedicated PCB was built to interface all these components. The mechanical parts were made in laser cut aluminum and 3D printed ABS.
To estimate the Cubli orientation and angular velocity, a quaternion based complementary filter was developed that fuses the accelerometer readings, which has high-frequency noise due to centripetal and tangential accelerations, with the gyroscope readings, which has low-frequency noise due to a constant bias being integrated over time. Each wheel angular displacement and velocity was obtained from a dedicated 2 nd order state observer, which takes into account not only the applied torques as well as the motors hall sensors readings.
Results were obtained adopting ζ = √ 2 2 , ω n = ω n 0 and α = 0.2 for the controller gains and setting the rotation quaternion reference to the Cubli unstable equilibrium position, i.e. q r = q u .
In the first experiment, presented in Fig. 24, the Cubli was released around 10 • from its equilibrium position and it was stabilized in less than 1 second, as it can be seen from its angular velocities quickly decaying to zero. The reaction wheels angular velocities also decayed to zero, but at a much slower rate of around 5 seconds. This makes sense since α = 0.2, which means the Cubli dynamics should be 5 times faster than the reaction wheel dynamics.
Two external disturbances were applied, one around 4.7 seconds in the inclination angle and another one around 7.3 seconds in the yaw angle. In both cases, the control system quickly rejected the disturbance without oscillating too much or saturating the actuators, which would happen for torques above 0.5N.m.
Moreover, the Cubli did not stabilize at 0 • inclination angle but around 4 • . This probably happened due to construction imperfections or sensor misalignment. However, because the reaction wheels angular displacements and velocities are also being feed-backed, the controller was able to find the real equilibrium position.
The experimental data is practically noise free due to the implementation and tuning of the state estimators previously described. Fig. 25 represents the same experiment but plotting data for a much longer period. It clearly shows the controllability problem. As it can be seen, although the Cubli has been stable for almost 1 minute, the yaw angle and reaction wheels angular displacements are increasing in modulus. In addition, one can see that the reaction wheels velocities are also increasing in modulus, which means that at some point, they will saturate and the Cubli will lose its inclination stability.
Second experiment is presented in Fig. 26. By making the reference follows a sinusoidal motion in the yaw axis, the yaw angle and reaction wheels angular displacement still increase in modulus, but now the reaction wheels angular velocities stay bounded. This does not mean that the yaw angle will be controllable, but at least it will remain marginally stable, not leading to the inclination instability mentioned earlier.
Another experiment is a comparison between the derived nonlinear controller, utilizing quaternions, with a linear controller, utilizing Euler angles. To generate the same disturbance in both cases, a forced input was applied, i.e. a constant torque of 0.2N.m for 0.2 seconds on the x and y motors. Fig. 27 shows the result for the linear controller, while Fig. 28 shows the result for the nonlinear controller. As it can be seen, the forced input applied at 1 second caused an almost 10 • disturbance in the inclination angle. While the nonlinear controller managed to recover, the linear controller did not. Note that this only occurs for large disturbances, where the Cubli leaves its equilibrium position. For small disturbances, not shown in the figure, the performance of both controllers would be virtually identical.
A video of the real system, including the experiments presented here and some other cases, is available at https://youtu.be/AWEWNBDW6CM, while the estimator and controller source code is available at https://github.com/fbobrow/cubli-firmware.

VI. CONCLUSION
By utilizing quaternions instead of Euler angles, modeling and control design could be performed utilizing vector notation. Although somewhat complex at a first view, in the end, the control law was quite compact and obtained completely by hand, without the need for any mathematical symbolic software. Moreover, its implementation is computationally inexpensive, which makes it effective even with low-performance micro-controllers.
Computer simulations showed that the model is consistent, whereas Poinsot trajectories presented a geometrical approach that also validated the model.
The Cubli presented a controllability problem inherent to its nature, which, although not solved, was mitigated with a sinusoidal reference strategy. Experimental results showed that the designed nonlinear control law is consistent and much more robust than a linear one.