Baumgarte Stabilisation Over the SO(3) Rotation Group for Control

2015 IEEE 54th Annual Conference on Decision and Control (CDC) December 15-18, 2015. Osaka, Japan

Baumgarte Stabilisation over the SO(3) Rotation Group for Control

Sebastien Gros, Marion Zanon, Moritz Diehl

Abstract— Representations of the SO(3) rotation group are quaternion and the Direction Cosine Matrix (DCM). For a crucial for airborne and aerospace applications. Euler angles complete survey, we refer to [2]. is a popular representation in many applications, but yield Quaternions are an extension of complex numbers to models having singular dynamics. This issue is addressed via non-singular representations, operating in dimensions higher a four-dimensional space, which allows for describing the than 3. Unit quaternions and the Direction Cosine Matrix are SO(3) rotation group. They can be construed as four- the best known non-singular representations, and favoured in dimensional vectors q R4. The SO(3) rotation group is challenging aeronautic and aerospace applications. All non- ∈ 4 then covered by q Q := q R q>q = 1 with: singular representations yield invariants in the model dynamics, ∈ ∈ | i.e. a set of nonlinear algebraic conditions that must be fulfilled R(q) = E(q)G(q)> SO(3), by the model initial conditions, and that remain fulfilled over  ∈  q1 q0 q3 q2 time. However, due to numerical integration errors, these condi- − − tions tend to become violated when using standard integrators, G(q) =  q2 q3 q0 q1 , − − making the model inconsistent with the physical reality. This q3 q2 q1 q0 issue poses some challenges when non-singular representations  − −  q1 q0 q3 q2 are deployed in optimal control. In this paper, we propose a − − simple technique to address the issue for classical integration E(q) =  q2 q3 q0 q1 . − − schemes, establish formally its properties, and illustrate it on q3 q2 q1 q0 the optimal control of a satellite. − − Keywords SO(3) rotation group, optimal control, Direction A significant advantage of quaternions over Euler angles Cosine Matrix, quaternions. is that the aforementioned singularity does not occur, such that aircraft or satellite models based on the quaternion I.INTRODUCTION representation do not suffer from the gimbal lock. Systems having a 3 degrees-of-freedoms (DOF) orienta- A sometimes prefered higher-dimensional representation tion are common in the aerospace and aeronautic industries, of the SO(3) rotation group is using the three orthonormal e.g. aircraft and satellite control requires the description of vectors describing the three axis of the object reference frame their 3DOF orientation. The orientation of a body in space given in the fixed reference frame. The 9 numbers providing is described via the group of all rotations about the origin these three orthonormal vectors, if properly organised, yield in the three-dimensional Euclidian space, labeled the SO(3) directly the rotation matrix R SO(3) between the object rotation group [1]. and the fixed reference frame. The∈ latter is then labeled the Because they are simple to manipulate in the context Direction Cosine Matrix (DCM). Similarly to the quaternion of basic control theory, Euler angles are often preferred to representation where q is normed to 1, a valid DCM must carry out that description. However, Euler angles suffer from satisfy the orthonormality condition a major defect due to the algebra underlying the tangent R>R I = 0. (1) space to the SO(3) rotation group. Namely, for any choice − ( ) of rotation sequence, there are always elements in SO 3 The DCM representation is easy to handle, and the non- for which the associated tangent space is not spanned via linearity of the model dynamics associated to rotations are variations of the corresponding Euler angles. typically milder than when using Euler-based or quaternion- This difficulty is often referred to as the gimbal lock and based representations. This makes the DCM approach partic- implies that aircraft and satellite models based on Euler ularly attractive in the context of direct optimal control [3], angles are singular for some orientations. In order to avoid [4]. The DCM representation has been used for airborne that problem, higher-dimensional representations need to be applications in e.g. [5], [6], [7], [8], [9], [10]. used. The two most commonly used higher-dimensional While providing non-singular models, the quaternion and ( ) representations of the SO 3 rotation group are the unit DCM-based representation of the SO(3) rotation group S. Gros is with the Department of Signals and Systems, Chalmers present some challenges in control. Long simulations of the University of Technology, Horsalsv¨ agen¨ 11, SE-412 96 Goteborg,¨ Swe- model based on quaternions or on the DCM over a long den, [email protected] . M. Zanon and M. Diehl are with time horizon do not preserve the norm or orthonormality the Systems Control and Optimization Laboratory, Department of Mi- conditions q q = 1 and R R I = 0 respectively, because crosystems Engineering (IMTEK), University of Freiburg, Georges- > > − Koehler-Allee 102, 79110 Freiburg, Germany. M.Diehl is additionally of accumulated numerical error. That is, over time these with the Department of Mathematics of the University of Freiburg. representations of the SO(3) rotation group become invalid. mario.zanon,moritz.diehl @imtek.uni-freiburg.de { * This research was supported by} the EU via ERC-HIGHWIND (259 In the context of direct optimal control, when a long control 166), FP7-ITN-TEMPO (607 957), and H2020-ITN-AWESCO (642 682). horizon is used, this can yield inconsistent or inaccurate

978-1-4799-7886-1/15/$31.00 ©2015 IEEE 620 solutions. Some classes of integrators offer solutions to that such that the quaternion norm is globally driven to 1. For vec- 4 issue [11], [12], [13]. However, these solutions require the tors q R such that q>q = 1, the quaternion dynamics (3) implementation of specific codes which are typically not reduce∈ to (2). We additionally observe that the proposed available in optimal control toolboxes. Additionally, to the correction is orthogonal to the quaternion natural dynamics, 1 1 1 best author’s knowledge, the proposed methods all involve i.e.q ˙natural = G(q) ω andq ˙correction = ρq q q − 1 2 > 2 > − additional computations when compared to standard integra- are orthogonal. Indeed, tion methods. Finally, the computational burden of perform- 1 ρ − ing the Algorithmic Differentiation [14] of these integrators q˙natural,q˙correction = ω>G(q)q q>q 1 = 0 (4) is not established yet, and may be more demanding than for h i 4 − classical integrators. We next provide a similar development for the more intricate In contrast, we propose a trivial (Baumgarte-like [15]) case of the DCM representation. modification of the dynamics associated to both the quaternions and the DCM in order to address the issue of numerical B. DCM representation drift while using standard integration schemes, such that the In this Section, we present the modification of the dynam- formulation of optimal control problems is straightforward ics associated to the DCM so as to tackle the issues raised and no special care needs to be taken. In this paper, we in the introduction. The dynamics of the DCM are given by: extend the framework developed in [3], propose a less R˙ = RΩ, (5) computationally expensive DCM stabilisation, a quaternion 3 3 stabilisation, and analyse formally the effect of these modi- where Ω R × is a skew symmetric matrix stemming from fications. We illustrate their effect on the optimal control of the angular∈ velocity ω. In principle, dynamics (5) preserve a satellite. the orthonormality condition (1). Indeed, for R>R = I the II.S TABILISATION OFNON -SINGULAR SO(3) following holds: REPRESENTATIONS d R>R I = Ω>R>R + R>RΩ = Ω> + Ω = 0 (6) In this Section, we present the correction that allows dt − for stabilise the orthonormality conditions arising in the However, because of numerical integration error, a long quaternion and the DCM representation. We start with the simulation of (5) can yield large violations of (1). Let us case of the quaternions, which is straightforward. first provide a Proposition that establishes the behaviour of the orthonormality condition (1) under the dynamics (5) for A. Quaternion representation 3 3 any arbitrary matrix R R × . The quaternion dynamics are given by ∈ 3 3 Proposition 1: For any matrix R R × , subject to the 1 dynamics (5), the following holds: ∈ q˙ = G(q)>ω, (2) 2 d 1 2 3 where ω R is the angular velocity in the reference frame R>R I = 0. ∈ dt 2 − F attached to the rotating body. For any vector q R4, it can be verified that G(q)q = 0 holds, such that the∈ quaternion Here the notation . F is used for the Froebinuis norm. norm is an invariant of the dynamics (2), in the sense that Proof: Propositionk k 1 follows from the properties of the it is preserved over time. In particular, if a unit quaternion trace operator, and can be established as follows: vector is provided as initial condition, the unit norm is d 1 2 conserved over the whole system trajectory. Due to numerical R>R I = dt 2 − F integration error, however, when processed numerically the d 1 > dynamics (2) yield quaternion vectors that are not unitary. = Tr R>R I R>R I To tackle that issue, one can introduce a simple correction dt 2 − − in (2), so as to ensure that the quaternion norm is not actually > = Tr R˙>R + R>R˙ R>R I preserved over time, but such that it actually exponentially − converges to 1. In the following, we will refer to this > = Tr R>RΩ ΩR>R R>R I correction as a stabilisation of the invariant. This stabilisation − − ensures that non-unitary norm cannot develop over time, = Tr R>RΩ ΩR>R R>R I since they naturally decay to unitary quaternions. − − We propose the following stabilisation of the invariant: = Tr R>RΩR>R Tr R>RΩ 1 − 1 1 − q˙ = G(q)>ω + ρq q>q 1 . (3) Tr ΩR>RR>R + Tr ΩR>R 2 2 − − We observe that = Tr ΩR>RR>R Tr ΩR>R d − q>q 1 = 2q>q˙ = q>G(q)>ω ρ q>q 1 dt − − − Tr ΩR>RR>R + Tr ΩR>R − = ρ q>q 1 . − − = 0.

621 We observe that: We propose next a simple modification of (5) that preserve Tr A>R>R + R>RA R>R I = the rotational dynamics but makes the orthonormality condi- − tion (1) globally stable. We will consider the modification: = Tr A> + A R>RR>R Tr A> + A R>R . − R˙ = R(A + Ω), (7) and use: where A 3 3 is the symmetric matrix: 1 R × − ∈ A> + A = ρ R>R I , 1 − ρ − A = R>R I (8) 2 − such that: We will establish next that the correction (8) in the dynamics d 1 2 R>R I = ρ 2Tr R>R Tr R>RR>R n (7) globally and asymptotically stabilise the orthonormality dt 2 − F − − condition (1). To establish this stability, let us first provide 2 = ρ R>R I , a supporting Proposition. − − F n n Proposition 2: For any matrices symmetric M R × the where the last equality results from Proposition 2. It then ∈ following holds: follows that: 2Tr(M) TrM2 n = M I 2. d 2 2 F R>R I = 2ρ R>R I , Proof: We observe− that − −k − k dt − F − − F which yields 2Tr(M) TrM2 n = TrM2 2M + I = − − − − 2 2 2 2ρ(t t0) Tr (M I) . R>R I = e− − R0>R0 I , − − − F − F Then if M is symmetric, we have: and hence (9). One can observe that (8) provides corrections of the DCM 2 2 Tr (M I) = M I , matrix that are orthogonal to the DCM natural dynamics, i.e. − k − kF the dynamics R˙ = RΩ are orthogonal to the correction which concludes the proof. natural R˙correction = RA, in the sense that R˙natural,R˙correction = 0. We can now proceed with the proof of stability. h 3 3 i 3 3 Indeed, it can be verified that for any R R × : Lemma 1: For any matrix R R × , if matrix A given by ∈ ∈ ρ (8) exists, the condition (1) is exponentially stable under the R˙natural,R˙correction = Tr ΩR>RA = Tr Ω ΩR>R = 0 Froebinius norm, i.e. h i 2 − holds since R R is symmetric. ρ(t t0) > R>R I = e− − R0>R0 I , (9) − F − F The correction (8) involves the inverse of the 3 3 matrix R R, which is yields a significant increase of the× symbolic where R0 is the DCM matrix at time t0 and R is the DCM > complexity of the corrected dynamics (7). In the context matrix at time t t0. Proof: First,≥ we observe that of optimal control, this increased complexity is undesirable, making the proposed correction rather unattractive in prac- d 1 2 R>R I = tice. We address that issue in the next section. dt 2 − F C. Approximate stabilisation of the DCM d 1 > = Tr R>R I R>R I dt 2 − − In this section, we propose a local approximation of (8) > which avoids the need of inverting matrix R>R, and hence = Tr R˙>R + R>R˙ R>R I − yields significantly less symbolic complexity than (8). We will show that the approximate correction preserves the > = Tr A> Ω R>R + R>R(A + Ω) R>R I global stabilisation of the orthonormality condition. − − Using a first-order Neumann expansion of the correction = Tr A>R>R ΩR>R + R>RA + R>RΩ R>R I (8) it can be verified that the following approximation holds: − − 1 = Tr R>RΩ ΩR>R R>R I − − − R>R I I R>R, − ≈ − + Tr A>R>R + R>RA R>R I − for R>R I, which provides the approximate correction: ≈ = Tr A>R>R + R>RA R>R I , ˜ ρ − A A = I R>R (10) ≈ 4 − where the latest equality results from Proposition 1. We then This correction is valid for small violations of the or- turn to proving the negativity of thonormality condition, yet it nonetheless provides a global d 1 2 stabilisation of the orthonormality condition. We establish R>R I = Tr A>R>R + R>RA R>R I . dt 2 − F − this statement in the following Corollary.

622 0.4 104 0.3 1 3 − 0.2 10 q

⊤ 0.1 q 102 0 0 5 10 15 20 25 30 35 40 45 50 101 0.5 F

k 0.4 I 100 0.3 − 0.2 R -1

⊤ 10 0.1 R

k 0 0 5 10 15 20 25 30 35 40 45 50 10-2

0.5

F -3

k 0.4 10 I 0.3 − 0.2 10-4 R

⊤ 0.1 R k 0 10-5 0 5 10 15 20 25 30 35 40 45 50 10-5 10-4 10-3 10-2 10-1 100 101 t ρ Fig. 1. Invariant stabilisation: 50 randomly generated perturbations (red) Fig. 2. Max invariant violation as a function of ρ using a Runge-Kutta of a nominal (blue) initial DCM or quaternion. Top Graph: quaternion integrator of order 4, with a fixed stepsize Ts = 0.05, and a simulation time invariant stabilisation. Middle graph: Frobenius norm of the DCM invariant. Tf = 10000. The maximum violation is displayed in black for the quaternion Bottom graph: Frobenius norm of the DCM invariant using the approximated invariant, in blue for the Frobenius norm of the DCM invariant and in red stabilisation. for the Frobenius norm of the DCM invariant stabilised using the first order approximation. ˜ ρ Corollary 1: The correction term A = 4 I R>R deployed in the DCM dynamics, i.e.: − respectively, with Ω = ω and constant ω. First, we ran 50 simulations× for randomly generated R˙ = RA˜ + Ω, (11) perturbations of the initial quaternion or DCM invariant using a fixed-stepsize Runge Kutta 4 (RK4) integrator with yields a globally stable orthonormality condition. stepsize T = 0.05 s. The results are shown in Figure 1, Proof: We reuse the developments of Lemma 1, i.e. s where the quaternion invariant and the Frobenius norm of d 1 2 the DCM invariant are plotted over time. As it can be seen, R>R I = Tr A˜> + A˜ R>RR>R dt 2 − F the proposed approach successfully reduces the invariant Tr A˜> + A˜ R>R , violation over time. − While integration schemes exist which guarantee to pre- ˜ ˜ ρ serve the system invariants, in the context of optimal control where A> + A = 2 I R>R . This yields − one typically prefers to deploy simple integration schemes, d 1 2 ρ R>R I = Tr I R>R R>RR>R so as to minimise the computational burden of carrying out dt 2 − F 2 − ρ the integration with sensitivity generation [16]. On short Tr I R>R R>R simulation horizons a good integration accuracy is generally − 2 − ρ sufficient to keep the invariant violation small. However, if = ρTr R>RR>R Tr R>RR>RR>R − 2 the horizon is made long enough, the invariant violation can ρ grow prohibitively large. The proposed stabilisation keeps Tr R>R − 2 the invariant violation small over arbitrarily long horizons. ρ There is no upper bound to constant ρ, and in principle = Tr R>R R>RR>R 2R>R + I − 2 − a larger ρ would reduce the invariant violation to very ρ 2 small values. However, large values make the system stiff, = Tr R>R R>R I , − 2 − which impedes the accuracy of the integrators. This effect which entails is illustrated in Figure 2, where the maximum invariant violation over a long horizon simulation is plotted. It can be d 2 2 R>R I = ρ R R>R I . seen that as expected, when ρ is small, the invariant violation dt F F − − − is not significantly reduced. As ρ increases the violation decreases, however it can be seen that when ρ becomes too We additionally observe that similarly to the correction large, the invariant violation increases again. A, the correction A˜ is also orthogonal to the DCM natural 3 3 III.APPLICATIONTOOPTIMALCONTROL dynamics. Indeed, it can be verified that for any R R × : ∈ We illustrate next the the proposed correction of the DCM R˙natural,R˙correction = Tr ΩR>RA˜ = h i representation of the SO(3) rotation group on the optimal ρ control of a small satellite using magnetorquers interacting Tr ΩR>R ΩR>RR>R = 0, (12) 4 − with the earth magnetic field, inspired from [17]. Both the de-tumbling and reorientation problems are considered. since both R>R and R>RR>R are symmetric. A. Optimal re-orientation of a Satellite D. Illustration The satellite equations are given by: In this section we illustrate our theoretical results by simulating the stabilised DCM and quaternion dynamics: R˙ = R ω + A˜ , (13a) × 1 Jω˙ = T ω Jω, (13b) 1 1 − R˙ = R(A + Ω), q˙ = G(q)>ω + ρq q>q 1 − × 2 2 − T = u R>B(t). (13c) × 623 2.0 0.15 0.10 ]

s] 1.5 2 / 0.05 1.0 0.00 [Am [deg 0.5

x 0.05 x − u

ω 0.0 0.10 − 0.5 0.15 − 0 200 400 600 800 1000 1200 − 0 200 400 600 800 1000 1200 0.2 0.15 0.0 0.10 ] s] 2

/ 0.2 − 0.05 0.4 − 0.00 [Am

[deg 0.6 − y 0.05 y 0.8 − u

ω − 1.0 0.10 − − 1.2 0.15 − 0 200 400 600 800 1000 1200 − 0 200 400 600 800 1000 1200 0.6 0.15 0.4 0.10 ] s] 2 / 0.2 0.05 0.0 0.00 [Am

[deg 0.2 − z 0.05 z 0.4 − u

ω − 0.6 0.10 − − 0.8 0.15 − 0 200 400 600 800 1000 1200 − 0 200 400 600 800 1000 1200 t [s] t [s]

Fig. 3. Rotational speed for the satellite without stabilisation (red) and Fig. 4. Controls for the satellite without stabilisation (red) and stabilised stabilised using the approximate DCM stabilisation with ρ = 10. using the approximate DCM stabilisation with ρ = 10.

3 3 3 100 where R × is the DCM, ω the angular speed and R R 1 3∈3 the resulting skew symmetric∈ matrix. Vector T 10− ω R × 2 × ∈ 3 3 10− is the torque generated by the magnetorquers, J R × the 10 3 ∈ − F 4 satellite inertia in its own referential, and k 10− I

− 5 10− 5 R B(t) = 2 10 0 cos(t/5400 s) sin(t/5400 s) > > 6 − R 10− · k 7 10− is the earth magnetic field, which is rotating due to the 8 3 10− satellite non-equatorial orbit. The control input u R is 10 9 ∈ − the dipole moment generated by the magnetorquers. In the 10 10 − 0 200 400 600 800 1000 1200 following we will lump the states R,ω together in vector x t [s] and collect the system dynamics (13) inx ˙ = f (x,u). Fig. 5. Collocation: Frobenius norm of the invariant for the satellite without In order to re-orient the satellite we formulate the follow- stabilisation (red) and stabilised using the approximate DCM stabilisation ing optimal control problem (OCP) with ρ = 10. Z t f 2 1 2 1 2 B. Optimal de-tumbling of a Satellite min 5 R(t) I F + ω(t) J + u(t) dt (14a) x( ),u( ) 0 k − k 2k k 2k k We now turn to the de-tumbling of the same satellite, · · s.t. x(0) = x0, (14b) which can be formulated an OCP similar to (14), but with different weights, i.e.: x˙ = f (x,u), t [0,T] (14c) ∈ Z t 2 2 f 4 2 2 2 0.1 Am u 0.1 Am , (14d) min 10− R(t) I F + 0.1 ω(t) J + u(t) dt − ≤ ≤ x( ),u( ) 0 k − k k k k k · · with the end time tf = 18 min and an initial state given by (15a) R0 = diag 1 1 1 , and ω0 = 0 rad/s. s.t. (14b) (14d) (15b) − − − We solve OCP (14) via direct collocation with Radau poly- with the end time tf = 36 min and an initial state given nomials of order 1. The control inputs are discretised as by R0 = I, and ω0 = 0.05 0 0 > rad/s. We turn to piecewise constants, with a sampling time of 3.6s. The solving the OCP (15) via direct multiple shooting using problem is transcribed using the software tool CasADi [18], N = 200 control intervals. The control inputs are discretised and the resulting NLP solved using the solver Ipopt [19]. as piecewise constant with a sampling time of 3.6s and the The trajectories resulting from solving (14) for both ρ = 0 system dynamics are discretised using a single step of a RK4 and ρ = 10 are reported in red and black, respectively, in scheme using fixed step size. For the stabilised dynamics, the figures 4 to 5. The Frobenius norm of the DCM invariant value ρ = 1e 2 has been chosen. As opposed to collocation, is displayed in Figure 5, where it can be seen that the RK4 is an explicit− integrator and therefore not suited for stiff OCP formulated using the non-stabilised dynamics yields systems. Higher values of ρ would deteriorate integration a solution with a large invariant violation. By violating the accuracy, as described in Sec. II-D. The same software DCM invariant, the OCP gains the ability to steer the system approach as in Sec. III-A is used. in an unphysical way, as can be seen in Equation (13c). This The controls and rotational speed are displayed in Figure 8 results in different control and rotational speed trajectories, and 6 respectively, where it can be seen that the non as it can be seen in Figures 4 and 3 respectively. stabilised dynamics yield a different solution. In particular, In this example, the violation of the orthonormality con- the kinetic energy is reduced more when the dynamics are dition has a limited impact on the OCP solution. This not stabilised, suggesting that the optimiser exploits the observation, however, cannot be generalised and if matrix numerical inaccuracy in order to alter the system dynamics R has a strong impact on the dynamics, e.g. when they enter (in particular (13c)) and further decrease the cost. the construction of the aerodynamic forces [10], it can be The invariant violation is displayed in Figure 7. It can be crucial to maintain a good orthonormality. seen that the stabilisation of the system dynamics effectively

624 3.0 8

s] 2.5 ] 6 /

2.0 2 1.5 4 1.0 2 [deg 0.5 Am k

x 0 0.0 [ ω

x 2 0.5 − − 0 500 1000 1500 2000 2500 u 4 3.0 − s] 2.5 6 / 2.0 − 0 500 1000 1500 2000 2500 1.5 2.0 1.0 ] [deg 1.5

0.5 2 y 0.0 1.0 ω 0.5 0.5 − 0 500 1000 1500 2000 2500 Am 3 k 0.0 [ s] 2 y 0.5 / − 1 u 1.0 − 0 1.5 [deg 1 − 0 500 1000 1500 2000 2500 z −2 3 ω −3 11 ] 2

− 0 10− 500 1000 1500 2000 2500 2 6.4 × 1 J] 6.2 t [s]

µ 0 Am [ 6.0

k 1 5.8 [ −

z 2 Jω 5.6 − u

> 5.4 3 − ω 5.2 4 0 500 1000 1500 2000 2500 − 0 500 1000 1500 2000 2500 t [s] t [s] Fig. 6. Multiple Shooting: Rotational speed for the satellite without Fig. 8. Multiple Shooting: Controls for the satellite without stabilisation stabilisation (red) and stabilised using the approximate DCM stabilisation (red) and stabilised using the approximate DCM stabilisation with ρ = 0.01. with ρ = 0.01. REFERENCES 100 [1] A. Joshi, Elements of Group Theory for Physicists. New Age International Publishers, 1997. [2] M. D. Shuster, “A Survey of Attitude Representations,” The Journal 1 10−

F of the Astronautical Science, vol. 41, no. 4, pp. 439–517, 1993. k I [3] S. Gros, M. Zanon, M. Vukov, and M. Diehl, “Nonlinear MPC and − R

> MHE for Mechanical Multi-Body Systems with Application to Fast R k Tethered Airplanes,” in Proceedings of the 4th IFAC Nonlinear Model 10 2 − Predictive Control Conference, Noordwijkerhout, The Netherlands, pp. 86–93, 2012. [4] S. Gros and M. Diehl, Airborne Wind Energy, ch. Modeling of 10 3 Airborne Wind Energy Systems in Natural Coordinates. Springer, − 0 500 1000 1500 2000 2500 t [s] 2013. Fig. 7. Multiple Shooting: Frobenius norm of the invariant for the [5] S. Gros, R. Quirynen, and M. Diehl, “Aircraft Control Based on Fast satellite without stabilisation (red) and stabilised using the approximate Nonlinear MPC & Multiple-shooting,” in Conference on Decision and DCM stabilisation with ρ = 0.01. Control, 2012. [6] S. Gros and M. Diehl, “Attitude estimation based on inertial and position measurements,” in Conference on Decision and Control, reduces the invariant violation by keeping it to small values, pp. 1758–1763, 2012. while for the non stabilised dynamics the invariant violation [7] S. Gros, M. Zanon, and M. Diehl, “A Relaxation Strategy for the increases over time. Note that it is hard in general to draw Optimization of Airborne Wind Energy Systems,” in European Control Conference, pp. 1011–1016, 2013. conclusions about the best value to choose for parameter ρ. [8] S. Gros, M. Zanon, and M. Diehl, “Control of Airborne Wind Energy However, as we used a smaller value for ρ in comparison Systems Based on Nonlinear Model Predictive Control & Moving to collocation, the invariant violation is larger. Note that for Horizon Estimation,” in ECC 2013, pp. 1017–1022, 2013. [9] M. Zanon, S. Gros, and M. Diehl, “Rotational Start-up of Tethered the unstabilised dynamics, the invariant violation increases Airplanes Based on Nonlinear MPC and MHE,” in Proceedings of the over time while in the collocation collocation example they European Control Conference, pp. 1023–1028, 2013. saturate quickly. This last fact stems for the way direct col- [10] M. Zanon, G. Horn, S. Gros, and M. Diehl, “Control of Dual-Airfoil Airborne Wind Energy Systems Based on Nonlinear MPC and MHE,” location handles dynamics in the OCP, which distributes the in European Control Conference, pp. 1801–1806, 2014. integration error over the time horizon rather than cumulating [11] E. Hairer, C. Lubich, and G. Wanner, Geometric Numerical Inte- it sequentially. gration: Structure-Preserving Algorithms for Ordinary Differential Equations. Springer, 2006. [12] P. Crouch and R. Grossman, “Numerical Integration of Ordinary IV. CONCLUSION Differential Equations on Manifolds,” Journal of Nonlinear Science, In this paper, we have presented trivial modifications of the vol. 3, pp. 1–33, 1993. [13] T. Lee, N. Harris McClamroch, and L. M., “A Lie group variational dynamics associated to the quaternion and DCM representa- integrator for the attitude dynamics of a rigid body with application tions of the SO(3) rotation group, which allow for avoiding to the 3d pendulum,” in Proceedings of the 2005 IEEE Conference on a drift of the associated orthonormality conditions. Avoiding Control Applications, 2005. [14] A. Griewank and A. Walther, Evaluating Derivatives. SIAM, 2 ed., this drift is important when optimal control is deployed 2008. on systems relying on the SO(3) rotation group such as [15] J. Baumgarte, “Stabilization of Constraints and Integrals of Motion in airborne and aerospace applications, in particular when the Dynamical Systems,” Computer Methods in Applied Mechanics and Engineering, vol. 1, pp. 1–16, 1972. optimisation is carried out over a long horizon. While the [16] R. Quirynen, M. Vukov, M. Zanon, and M. Diehl, “Autogenerating drift can be avoided by deploying ad-hoc integration tech- Microsecond Solvers for Nonlinear MPC: a Tutorial Using ACADO niques, in the context of optimal control classical numerical Integrators,” Optimal Control Applications and Methods, vol. 36, pp. 685–704, 2014. integration techniques are computationally less expensive [17] M. Lovera and A. Astolfi, “Spacecraft attitude control using magnetic and therefore favoured. Correcting the drift via modifying actuators,” Automatica, pp. 1405–1414, 2004. the dynamics allows for tackling the drift problem when [18] J. Andersson, A General-Purpose Software Framework for Dynamic Optimization. PhD thesis, Arenberg Doctoral School, KU Leuven, classical numerical integration techniques are used. This October 2013. paper establishes the properties of the proposed corrections, [19]A.W achter¨ and L. Biegler, “IPOPT - an Interior Point OPTimizer.” and assess their efficiency in the optimal control of a satellite. https://projects.coin-or.org/Ipopt, 2009.

625