Attitude Calculation

Preface

The simulation of the aircraft dynamic is the first step and probably one of the most important in order to have the right fidelity in a full mission simulator. In this post we face the problem to determine the attitude of the aircraft once the angular velocities and accelerations are know

Aircraft attitude calculation

As stated in other posts, we define attitude of an aircraft its angular position respect to some reference system. In the post ”Euler Angles” we define a proper reference system and the three attitude angles (psi, theta and phi) defining the angular position of the aircraft at some time. In the same post we find a relationship between the Euler angular velocities psidot, thetadot and fidot and the instantaneous components P, Q, R of the overall angular velocity. Let’s recall that result (16a, 16b and 16c in the mentioned post):

$\dot{\varphi }=P+\tan (\theta )\sin (\varphi )Q+\tan (\theta )\cos (\varphi )R$

$\dot{\theta }=\cos (\varphi )Q-\sin (\varphi )R$

$\dot{\psi }=\frac{\sin (\varphi )}{\cos (\theta )}Q+\frac{\cos (\varphi )}{\cos (\theta )}R$

We posed also the question: is it possible with a proper integration process (an algorithm) applied to the previous relations, to determine $\varphi ,\theta ,\psi$ ? Theoretically the answer is affirmative, but looking at the expressions we discover that the condition for $\theta =\frac{\pi }{2}$ makes two of the three expressions undefined. This condition (true also for $\theta =-\frac{\pi }{2}$ ) is often called “gimbal lock” condition, from the analogous difficulties to drive a gimbal when approaching that condition. So we arrived to the conclusion to look for another approach: to do that we introduced the Euler Parameters.

In the ”Euler Angles” post we also defined a standard reference system to use in the definition of $\varphi ,\theta ,\psi$ and we calculate the transformation matrix to pass from the reference system to the actual angular position of the aircraft. We recall that result:

$M_{\psi \theta \varphi }=\left(\begin{array}{ccc}\cos (\theta )\cos (\psi )& \cos (\theta )\sin (\psi )& -\sin (\theta )\\ -\sin (\psi )\cos (\varphi )+\sin (\varphi )\sin (\theta )\cos (\psi )& \cos (\psi )\cos (\varphi )+\sin (\varphi )\sin (\theta )\sin (\psi )& \sin (\varphi )\cos (\theta )\\ \sin (\psi )\sin (\varphi )+\cos (\varphi )\sin (\theta )\cos (\psi )& -\cos (\psi )\sin (\varphi )+\cos (\varphi )\sin (\theta )\sin (\psi )& \cos (\varphi )\cos (\theta )\end{array}\right)$  1)

We added the consideration that, if we are able to know 1) in some other way, the calculation of $\varphi ,\theta ,\psi$ becomes very easy applying the following relations:

$\theta =-\arcsin (m_{13})\varphi =\arctan (\frac{m_{23}}{m_{33}})\psi =\arctan (\frac{m_{12}}{m_{11}})$  2)

being m_ij the generic element of 1).

Euler Parameters

In this Euler Parameters post we find another way to express the rotation matrix 1). We defined in a proper reference system, an axis (described by a vector forming with the reference system axis the three angles α β and γ) where to apply a rotation of the angle μ. According to Euler's rotation theorem , choosing the right α β γ and μ, we are able to have the same transformation described by 1), as a function of α β γ and μ instead of $\varphi ,\theta ,\psi$ .

We recall here the related transformation matrix:

T = $\left(\begin{array}{ccc}{a}_1^2+a_2^2-a_3^2-a_4^2& 2(a_1{a}_4+a_2{a}_3)& 2(a_2{a}_4-a_1{a}_3)\\ 2(a_2{a}_3-a_1{a}_4)& a_1^2-a_2^2+a_3^2-a_4^2& 2(a_3{a}_4+a_1{a}_2)\\ 2(a_2{a}_4+a_1{a}_3)& 2(a_3{a}_4-a_1{a}_2)& a_1^2-a_2^2-a_3^2+a_4^2\end{array}\right)$  3)

where

$a_1=\cos (\frac{\mu }{2})$   $a_2=\sin (\frac{\mu }{2})\cos \alpha$   $a_3=\sin (\frac{\mu }{2})\cos \beta$   $a_4=\sin (\frac{\mu }{2})\cos \gamma$

are named Euler Parameters. It is quite obvious that if matrix 3) and matrix 1) represents the same transformation, each corresponding member of each matrix must be equal, and this, applying relation 2) to matrix 3), allows to find easily the attitude angles, provide that there is a viable way to calculate a₁, a₂, a₃ and a₄. We will shortly demonstrate that it is the case.

Quaternion

In the post Quaternion we introduced this amazing mathematical entity. This is a three dimensional extension of a complex number. It is formed by a real part and an imaginary 3D vector part. The general form of a quaternion is:

$q=q_0+i{q}_1+j{q}_2+k{q}_3$

where i, j, k are three unitary pure imaginary number (in analogy with i, j, k versors of a real three dimensional reference system). In the same post we discovered that in case the quaternion was a versor, of the form

$q=\cos \frac{\mu }{2}+i\cos \alpha \sin \frac{\mu }{2}+j\cos \beta \sin \frac{\mu }{2}+k\cos \gamma \sin \frac{\mu }{2}$

the following relation:

$V\text{'}=\tilde{q}Vq$   4)

allows to calculate V’ in a reference system derived by the one where V is defined, rotating it by an angle μ around an axis (vector) which forms with the X, Y and Z axis the three angles α β γ. In fact relation 4) can be written as (see related post Quaternion)

$\left(\begin{array}{c}V{\text{'}}_x\\ V{\text{'}}_y\\ V{\text{'}}_z\end{array}\right)=\left(\begin{array}{ccc}(q_0^2+q_1^2-q_2^2-q_3^2)& (2q_0{q}_3+2q_1{q}_2)& (2q_1{q}_3-2q_0{q}_2)\\ (2q_1{q}_2-2q_0{q}_3)& (q_0^2-q_1^2+q_2^2-q_3^2)& (2q_0{q}_1+2q_2{q}_3)\\ (2q_0{q}_2+2q_1{q}_3)& (2q_2{q}_3-q_0{q}_1)& (q_0^2-q_1^2-q_2^2+q_3^2)\end{array}\right)\left(\begin{array}{c}{V}_x\\ V_y\\ V_z\end{array}\right)$  5)

It is evident that the matrix transformation in 5) is identical of the matrix transformation 3) when we pose

$q_0=a_1=\cos \frac{\mu }{2}$   $q_1=a_2=\cos \alpha \sin \frac{\mu }{2}$

$q_2=a_3=\cos \beta \sin \frac{\mu }{2}$   $q_3=a_4=\cos \gamma \sin \frac{\mu }{2}$

We have here established the equivalence between the Euler parameters and the quaternion components.

So, summarizing, if we know the quaternion components, we know the Euler parameters, and if we know the Euler parameters we know matrix 1), and if we know matrix 1), using relations 2), we know the attitude angles. Let’s orient now our effort in knowing the quaternion components in a viable way.

In the post Quaternion we face the problem of quaternion differentiation, and we found out that the derivative of a quaternion is:

$\dot{q}=\frac{1}{2}q(t)\overline{\omega }$  6)

where

$\overline{\omega }=ì{\omega }_x+j{\omega }_y+k{\omega }_z=ìP+jQ+kR$

is the pure imaginary quaternion representing the overall angular velocity.

In the same post we developed relation 6) arriving to the result:

$\left(\begin{array}{c}\dot{q_0}\\ \dot{q_1}\\ \dot{q_2}\\ \dot{q_3}\end{array}\right)=\frac{1}{2}\left(\begin{array}{cccc}0& -P& -Q& -R\\ P& 0& R& -Q\\ Q& -R& 0& P\\ R& Q& -P& 0\end{array}\right)\left(\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right)$  7)

If we are able, knowing the three components of the instantaneous angular velocity (P, Q, R), to solve the system of differential equations 7), we know the components of the quaternion, and thus the matrix 1) and finally the attitude angles. In the same post we developed an easy way to calculate the component of a quaternion able to rotate the reference system once the three attitude angles $\varphi ,\theta ,\psi$ are given. This is very useful for initializing a quaternion knowing the attitude angles.

The Local Linearization Algorithm

To solve system 7) we make use of an efficient integration algorithm explained in detail in the post LL Algo, the local linearization algorithm. This post is a pure mathematical treatment and cannot be easily summarized: it implies knowledge in differential equation solution, Taylor series expansion of function of multiple variables, matrices and function of matrices. The integration process is suppose to be done numerically on a digital computer, adopting a proper integration step (in flight simulation typical integration frequency are form 25 to 100 Hz with corresponding integration step varying from 40 to 10 msec). We here report simply the results of the post LL Algo:

$q_{0(k+1)}=q_{0k}\ast H-q_{1k}\ast K-q_{2k}\ast J-q_{3k}\ast G$

$q_{1(k+1)}=q_{0k}\ast K+q_{1k}\ast H-q_{2k}\ast G-q_{3k}\ast J$

$q_{2(k+1)}=q_{0k}\ast J+q_{1k}\ast G+q_{2k}\ast H+q_{3k}\ast K$

$q_{3(k+1)}=-q_{0k}\ast G+q_{1k}\ast J-q_{2k}\ast K+q_{3k}\ast H$

At each step the quaternion components are calculated as a linear combination of the quaternion components at the previous step, using particular coefficients which are functions of P, Q, R and their derivatives. In the same post we report also a code snippet in C/C++ where the LL Algo, in its essential core (excluding initialization of parameters, condition checking etc) is about 100 lines of code, so very compact, with minimal CPU load and above all, “gimbal lock” condition free, solving completely the problem. Nevertheless this result comes from a rather complex path that I wanted to explain in detail, hoping that it will be useful to someone. Often these algorithms are already proposed in the libraries of many tools used in scientific computing, but it is also true that these tools are normally incompatible with real-time applications so their detailed knowledge can be useful.