LL Algo

1) Preface

In this post we face the problem to solve the differential equation of quaternions in order to find the attitude of an aircraft without having the "gimbal lock" problem. We will first develop the numerical solution of the differential equation with the LL Algorithm (Local Linearization), than we will find a workable solution of the algorithm analyzing how a function of matrix can be expressed, and finally we will find the numerical algorithm to solve the entire problem.

2) LL (Local Linearization) Algo

We now re-write what already discovered previously relating to the calculation of the attitude of an aircraft, without having the problem to encounter the “gimbal lock” in the condition of theta = 90°. The differential equation to be solved in order to calculate the components of the quaternion describing the attitude is:

$\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)$   1)

The Local Linearization Algorithm (LL Algo) is proposed to solve this differential equation.

In order to follow this chapter a basic knowledge of matrices, functions of matrices, differential equations, functions of multiple variables e their development in the Taylor series is required. Not all the development will be described, but I believe it will be sufficient to the reader who wants to go in a deeper detail. This algorithm is taken by the NASA technical note TN D-7347 “Development and application of a local linearization algorithm for the integration of quaternion rate equations in real-time flight simulation problems”, December 1973 (Ref a). In that paper the 4X4 matrix in equation 1) is quiet different , coming from another approach for the solution of the attitude problem. Instead to use quaternions as I did in this post,they proposed to use Cayley-Klein parameters. The two methods, even producing two different systems of differential equations, lead to the same result, of course. I personally have a preference for the quaternion approach, that seems to me more elegant and direct.

Let’s start approaching the problem in very general terms considering the following expression:

$\dot{\overline{X}}=\overline{F}(\overline{X},t)$  where

$\overline{X}(t)$   is the n-dimensional vector representing system states and

$\overline{F}(\overline{X},t)$   is the n-dimensional vector composed of general nonlinear time-varying functions of the state vector $\overline{X}$ at time t.

Let $\mathrm{\Delta }\overline{X}(t)$ a small perturbation around $\overline{X_k}=X(T_k)$ defined as

$\mathrm{\Delta }\overline{X}(t)=\overline{X}-\overline{X_k}$ where $t_k\le t\le t_{(k+1)}$     2)

and let Δt be a small perturbation in time around t_kdefined as

$\mathrm{\Delta }t=t-t_k$

Therefore from 2)

$\mathrm{\Delta }\dot{\overline{X}}(t)=\dot{\overline{X}}=\overline{F}(\overline{X},t)$

Let’s develop the $\overline{F}(\overline{X},t)$ around the initial point ( $\overline{X_k}$ , t_k)until the first order (from here the term “Local Linearization”)

$\mathrm{\Delta }\dot{\overline{X}}(t)=\overline{F}(\overline{X_k},t_k)+\frac{\partial \overline{F}(\overline{X_k},t_k)}{\partial \overline{X}}\mathrm{\Delta }\overline{X}+\frac{\partial \overline{F}(\overline{X_k},t_k)}{\partial t}\mathrm{\Delta }t+\cdots \cdots \cdots$    3)

In order to simplify the notation let’s make the following assumptions

$\overline{A_k}=\frac{\partial \overline{F}(\overline{X_k},t_k)}{\partial \overline{X}}$     $\overline{B_k}=\overline{F}(x_k,t_k)$     $\overline{C_k}=\frac{\partial \overline{F}(\overline{X_k},t_k)}{\partial t}$

$\mathrm{\Delta }\dot{\overline{X}}(t)=\dot{\overline{y}}(t)$     $\mathrm{\Delta }\overline{X}(t)=\overline{y}(t)$     $\mathrm{\Delta }t=t-t_k$

where

$\overline{A_k}=\frac{\partial \overline{F}(\overline{X_k},t_k)}{\partial \overline{X}}$

is a matrix whose generic component (i,j) is

$a_{\mathit{ij}}=\frac{\partial \overline{F_i}(\overline{X_k},t_k)}{\partial x_j}$    where x_j is the j^th element of vector $\overline{X}$

Expression 3) becomes:

$\dot{\overline{y}}=\overline{B_k}+\overline{A_k}\overline{y}+\overline{C_k}(t-t_k)$   and therefore

$\dot{\overline{y}}-\overline{A_k}\overline{y}=\overline{B_k}-\overline{C_k}{t}_k+\overline{C_k}t$    4)

The relation 4) is a linear first order differential equation. As known from the general theory, the solution of such an equation is found summing up the solution of the homogeneous equation to a particular solution of the equation.

The homogeneous equation associated to 4) is

$\dot{\overline{y}}-\overline{A_k}\overline{y}=0$   whom solution is of type

$\overline{y_h}(t)=\overline{k}\ast \exp (\overline{A_k}t)$    5)

where k is a arbitrary constant vector depending from boundary condition.

A particular solution of 4) is of type

$\overline{y_p}(t)=\overline{M}\ast t+\overline{N}$     6)

In order to identify the the constant vectors $\overline{M}$ and $\overline{N}$ let’s substitute general solution 6) in 4)

$\overline{M}-\overline{A_k}\ast \overline{M}\ast t-\overline{A_k}\ast \overline{N}=\overline{B_k}-\overline{C_k}{t}_k+\overline{C_k}t$    7)

Matching both terms in t

$-\overline{A_k}\ast \overline{M}=\overline{C_k}$    and therefore

$\overline{M}=-\overline{A_k^{-1}}\ast \overline{C_k}$     8)

Matching the other terms leads to

$-\overline{A_k}\ast \overline{N}=\overline{B_k}-\overline{C_k}{t}_k-\overline{M}$     $\overline{N}=-\overline{A_k^{-1}}\ast (\overline{B_k}-\overline{C_k}{t}_k-\overline{M})$   and finally

$\overline{N}=-\overline{A_k^{-1}}\ast \overline{B_k}+\overline{A_k^{-1}}\ast \overline{C_k}\ast t_k-\overline{A_k^{-2}}\ast \overline{C_k}$    9)

considering 6), 8) and 9) a particular solution of differential equation 4) is

$\overline{y_p}(t)=\overline{A_k^{-1}}\ast \overline{C_k}\ast t-\overline{A_k^{-1}}\ast \overline{B_k}+\overline{A_k^{-1}}\ast \overline{C_k}\ast t_k-\overline{A_k^{-2}}\ast \overline{C_k}$    10)

According to the general rule, the solution of 4) is the sum of 5) and 10), that is

$\overline{y}(t)=\overline{y_h}(t)+\overline{y_p}(t)=\overline{k}\ast \exp (\overline{A_k}\ast t)-\overline{A_k^{-1}}\ast \overline{C_k}\ast (t-t_k)-\overline{A_k^{-1}}\ast \overline{B_k}-\overline{A_k^{-2}}\ast \overline{C_k}$   11)

in order to find the arbitrary constant k, let’s consider that for t=t_{k ,}(see 2))

$\overline{y}=\mathrm{\Delta }\overline{X}=\overline{X}(t)-\overline{X_k}=0$

in this special case 11) becomes

$\overline{k}\ast \exp (\overline{A_k}\ast t_k)-\overline{A_k^{-1}}\ast \overline{B_k}-\overline{A_k^{-2}}\ast \overline{C_k}=0$    and therefore

$\overline{k}=\exp (\overline{-A_k}\ast t_k)\ast (\overline{A_k^{-1}}\ast \overline{B_k}+\overline{A_k^{-2}}\ast \overline{C_k})$

Under this assumption solution 11) becomes

$\overline{y}(t)=\exp (\overline{A_k}\ast (t-t_k))\ast ((\overline{A_k^{-1}}\ast \overline{B_k}+\overline{A_k^{-2}}\ast \overline{C_k}))-\overline{A_k^{-1}}\ast \overline{C_k}\ast (t-t_k)-\overline{A_k^{-1}}\ast \overline{B_k}-\overline{A_k^{-2}}\ast \overline{C_k}$   12)

let’ remember that

$y(t)=\mathrm{\Delta }X(t)=X(t)-X_k$    and therefore

$y(t_{k+1})=X(t_{k+1})-X_k$

relation 12) becomes

$\overline{X}(t_{k+1})=\overline{X}(t_k)+\exp (\overline{A_k}\ast h)\ast (({\overline{A}}_k^{-1}\ast \overline{B_k}+{\overline{A_k}}^{-2}\ast \overline{C_k}))-{\overline{A}}_k^{-1}\ast \overline{C_k}\ast h-{\overline{A}}_k^{-1}\ast \overline{B_k}-{\overline{A}}_k^{-2}\ast \overline{C_k}$

where h = t_k+1 – t_k, represents the integration step

giving to $\overline{B_k}$ and $\overline{C_k}$ their values and after some simple elaboration

$\overline{X}(t_{k+1})=\overline{X}(t_k)+{\overline{A}}_k^{-1}\ast (\exp (\overline{A_k}\ast h)-I)\ast \overline{F}(x_k,t_k)+{\overline{A}}_k^{-2}\ast (\exp (\overline{A_k}\ast h)-I-\overline{A_k}\ast h)\ast \partial \overline{F}\frac{(\overline{X_k},t_k)}{\partial t}$

where I is the identity matrix. Finally, assuming

$\overline{P_k}={\overline{A}}_k^{-1}\ast ((\exp (\overline{A_k}\ast h)-I))$      13)

$\overline{Q_k}={\overline{A_k}}^{-2}\ast (\exp (\overline{A_k}\ast h)-I-\overline{A_k}\ast h)$     14)

$\overline{X}(t_{k+1})=\overline{X}(t_k)+\overline{P_k}\ast \overline{F}(\overline{X_k},t_k)+\overline{Q_k}\ast \partial \overline{F}\frac{(\overline{X_k},t_k)}{\partial t}$    15)

Now let’s use 15) in order to solve the particular differential equation:

$\dot{\overline{X}}=\overline{F}(\overline{x},t)=A(t)\ast \overline{X}$    16)

It is quit obvious that:

$\partial \overline{F}\frac{(\overline{X_k},t_k)}{\partial t}=\dot{A}(t_k)\ast \overline{X_k}=\dot{A_k}\ast \overline{X_k}$     17)

The solution 15) applied to problem 16) and considering17) becomes

${\overline{X}}_{(k+1)}=\overline{X_k}+\overline{P_k}\ast \overline{F}(\overline{X_k},t_k)+\overline{Q_k}\ast \dot{A_k}\ast \overline{X_k}$    18)

Remembering the values of $\overline{P_k}$ and $\overline{Q_k}$ from 13) and 14) and after some elaboration we arrive at the following result

${\overline{X}}_{(k+1)}=A_k^{-1}\ast \exp (A_k\ast h)\ast A_k\ast \overline{X_k}+\overline{Q_k}\ast \dot{A_k}\ast \overline{X_k}$    19)

where h = t_k+1 – t_k, represents the integration step

We need now to improve expression 19) to a form more manageable. In order to do that let’s consider the term

$A_k^{-1}\ast \exp (A_k\ast h)\ast A_k$        20)

and let’s remember some matrix properties that can be found in any good book on the argument (for example Matrix Theory by Gantmacher, Chelsey publishing Company, New York)

Let matrix B be the contragredient transformation of matrix A by means of matrix T: by definition

$B=T^{-1}\ast A\ast T$ in this case we have that

$f(B)=T^{-1}\ast f(A)\ast T$      21

let’s apply the same transformation to $A_k$

$B=T^{-1}\ast A_k\ast T$       22)

and let’s specify as transformation matrix $T=A_k$

Relation 22) becomes

$B=A_k^{-1}\ast A_k\ast A_k=A_k$ and thus, applying property 21)

$\exp (B\ast h)=A_k^{-1}\ast \exp (A_k\ast h)\ast A_k=\exp (A_k\ast h)$

expression 19) now becomes

${\overline{X}}_{(k+1)}=\exp (A_k\ast h)\ast \overline{X_k}+\overline{Q_k}\ast \dot{A_k}\ast \overline{X_k}$    23)

Let’s use 23) to solve differential equation 16) in case that

$A(t)=\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)$     24)

where P, Q, R are the angular velocities of a body around its X-Y-Z reference system

This differential equation is identical to differential equation 1)

It is easy to show that

$\mathit{det}(A)=\frac{1}{16}{[P^2+Q^2+R^2]}^2$

assuming

$P^2+Q^2+R^2={\omega }^2$   previous expression becomes

$\mathit{det}(A)=\frac{1}{16}{\omega }^4$   where ω is the modulus of the overall angular velocity

To apply solution 23) we need to understand the meaning of the expression

exp(A_k * h)

and to do that we need to introduce briefly in the next paragraph, the concept of “function of matrix (for a complete and exhaustive treatment, see specialized books/articles on matrix theory)

3) Function of Matrices

Let’s start with some definition, let A_nxn be a square matrix:

definition 1

Let $\mathrm{\Psi }(\lambda )$ be a polynomial in $\lambda$ : it is defined minimal polynomial for the matrix A if the following conditions are met:

a) $\mathrm{\Psi }(A)=0$

b) among all the polynomials satisfying condition 1) it is the polynomial with minimal rank

c) Coefficient of maximum rank in $\lambda$ is equal 1 (it is a monic polynomial)

definition 2

The characteristic polynomial of A is defined as the determinant of the matrix

$c(\lambda )=\mathit{det}(\lambda I-A)$

definition 3

A principal minor of order k of a square matrix A is the determinant of the matrix obtained by A deleting n-k row and columns of the same order

It shall be proved that the characteristic polynomial of A is

$c(\lambda )={\lambda }^n-c_1\ast {\lambda }^{n-1}+c_2\ast {\lambda }^{n-2}+\mathrm{..........}+(-1^n)\ast c_n$

where c_r are the sum of all the principal minors of order r.

Let’s now explain a procedure in order to find the minimal polynomial.

Let’s consider the characteristic polynomial $c(\lambda )$ of a square matrix A. Due to the theorem of Hamilton-Cayley we can affirm that:

$c(A)=0$

In general $c(\lambda )$ is not necessarily the minimum polynomial, and in general $c(\lambda )$ shall be divided by a polynomial $D(\lambda )$ in order to obtain the minimal polynomial $\mathrm{\Psi }(\lambda )$ .

Let’s define now how to find $D(\lambda )$ and let’s define the following polynomial:

$\delta (\lambda ,\mu )=\frac{c(\mu )-c(\lambda )}{\mu -\lambda }$ and let’s consider the matrix

$B(\lambda )=\delta (\lambda ,A)$

The greatest common divisor (gcd) of all the elements of $B(\lambda )$ is $D(\lambda )$

Dividing $c(\lambda )$ by $D(\lambda )$ gives us the minimal polynomial $\mathrm{\Psi }(\lambda )$ (we omit the demonstration).

Let now ${\lambda }_1$ and ${\lambda }_2$ be for example the two solution of $\mathrm{\Psi }(\lambda )=0$ (the fact that they are only two does not influence the generality of the following statements). It shall be demonstrated that:

$f(A)=f({\lambda }_1)\ast Z_1+f({\lambda }_2)\ast Z_2$    25)

where Z₁and Z₂ (and in general Z_i) are the “constituent matrices” of A and they are independent from the particular function f(). They can be found assuming some particular simple functions as we will see later.

Let’s now apply the procedure just explained in order to give a meaning to the expression

exp(A * h)

where A is given by 24).

Let’s first calculate the characteristic polynomial of matrix 24)

$c(\lambda )={\lambda }^4-c_1\ast {\lambda }^3+c_2\ast {\lambda }^2-c_3\ast \lambda +c_4$    26)

where:

c₁ is the sum of all the principal minor of order 1

c₂ is the sum of all the principal minor of order 2

c₃ is the sum of all the principal minor of order 3

c₄ is the sum of all the principal minor of order 4

In practise:

c₁ is the sum of all the elements of the matrix diagonal and thus

c₁=0

$c_2=\left|\begin{array}{cc}0& \frac{-P}{2}\\ \frac{P}{2}& 0\end{array}\right|+\left|\begin{array}{cc}0& \frac{-Q}{2}\\ \frac{Q}{2}& 0\end{array}\right|+\left|\begin{array}{cc}0& \frac{-R}{2}\\ \frac{R}{2}& 0\end{array}\right|+\left|\begin{array}{cc}0& \frac{R}{2}\\ \frac{-R}{2}& 0\end{array}\right|+\left|\begin{array}{cc}0& \frac{-Q}{2}\\ \frac{Q}{2}& 0\end{array}\right|+\left|\begin{array}{cc}0& \frac{P}{2}\\ \frac{-P}{2}& 0\end{array}\right|=\frac{{\omega }^2}{2}$

$c_3=\left|\begin{array}{ccc}0& \frac{-P}{2}& \frac{-Q}{2}\\ \frac{P}{2}& 0& \frac{R}{2}\\ \frac{Q}{2}& \frac{-R}{2}& 0\end{array}\right|+\left|\begin{array}{ccc}0& \frac{-P}{2}& \frac{-R}{2}\\ \frac{P}{2}& 0& \frac{-Q}{2}\\ \frac{R}{2}& \frac{Q}{2}& 0\end{array}\right|+\left|\begin{array}{ccc}0& \frac{-Q}{2}& \frac{-R}{2}\\ \frac{Q}{2}& 0& \frac{P}{2}\\ \frac{R}{2}& \frac{-P}{2}& 0\end{array}\right|+\left|\begin{array}{ccc}0& \frac{R}{2}& \frac{-Q}{2}\\ \frac{-R}{2}& 0& \frac{P}{2}\\ \frac{Q}{2}& \frac{-P}{2}& 0\end{array}\right|=0$

$C_4=\mathit{det}(A)=\frac{1}{16}{[P^2+Q^2+R^2]}^2=\frac{{\omega }^4}{16}$

The characteristic polynomial 26) becomes:

$c(\lambda )={\lambda }^4+\frac{{\omega }^2}{2}\ast {\lambda }^2+\frac{{\omega }^4}{16}={({\lambda }^2+\frac{{\omega }^2}{4})}^2$     27)

Let’s continue to apply the procedure just explained and let’s calculate

$\delta (\lambda ,\mu )=\frac{c(\mu )-c(\lambda )}{\mu -\lambda }$

$\delta (\lambda ,\mu )=\frac{{({\mu }^2+\frac{{\omega }^2}{4})}^2-{({\lambda }^2+\frac{{\omega }^2}{4})}^2}{\mu -\lambda }$

after some simplification the expression becomes:

$\delta (\lambda ,\mu )={\mu }^3+{\mu }^2\ast \lambda +\mu \ast ({\lambda }^2+\frac{{\omega }^2}{2})+(\frac{{\omega }^2}{2}\ast \lambda +{\lambda }^3)$

Let’s calculate the matrix $B(\lambda )=\delta (\lambda ,A)$

$B(\lambda )=\delta (\lambda ,A)=A^3+A^2\ast \lambda +A\ast ({\lambda }^2+\frac{{\omega }^2}{2})+(\frac{{\omega }^2}{2}\ast \lambda +{\lambda }^3)\ast I$   28)

with simple calculations we obtain

$A^2=\frac{-{\omega }^2}{4}\ast I$  29)  and thus   $A^3=\frac{-{\omega }^2}{4}\ast A$  30)

Replacing 29) and 30) in 28) and after some calculation we obtained

$B(\lambda )=A\ast ({\lambda }^2+\frac{{\omega }^2}{2})+(\frac{{\omega }^2}{4}+{\lambda }^2)\ast \lambda \ast I$   and finally

$B(\lambda )=(\frac{{\omega }^2}{4}+{\lambda }^2)\ast (A+\lambda \ast I)$

$(\frac{{\omega }^2}{4}+{\lambda }^2)$ is the greatest common divisor (gcd) of all the elements of $B(\lambda )$ and thus as explained before

$D(\lambda )=(\frac{{\omega }^2}{4}+{\lambda }^2)$     31)

The minimal polynomial of matrix A is the ratio between the characteristic polynomial 27) and 31)

$\mathrm{\Psi }(\lambda )=\frac{{({\lambda }^2+\frac{{\omega }^2}{4})}^2}{(\frac{{\omega }^2}{4}+{\lambda }^2)}=(\frac{{\omega }^2}{4}+{\lambda }^2)$

The two solution of $\mathrm{\Psi }(\lambda )=0$ are   ${\lambda }_1=j\frac{\omega }{2}$  and  ${\lambda }_2=-j\frac{\omega }{2}$

As already explained any function f(A) can be expressed as

$f(A)=f(-j\frac{\omega }{2})\ast Z_1+f(j\frac{\omega }{2})\ast Z_2$     32)

Where Z₁ and Z₂ are independent from f(). In order to find them let’s calculate $f(\lambda )=1$

expression 32) becomes

I = Z₁ + Z₂    33)

Let’s now assume $f(\lambda )=\lambda +j\frac{\omega }{2}$

expression 32) now becomes

$A+j\frac{\omega }{2}I=j\omega Z_2$  and thus   $Z_2=\frac{1}{j\omega }A+\frac{I}{2}$   34)

and from 33) and 34)

$Z_1=I-Z_2=\frac{I}{2}-\frac{1}{j\omega }A$    35)

We are now in the position to apply expression 25) considering the function we want to use:

$e^{\mathit{Ah}}=e^{-j\frac{\omega }{2}h}(\frac{I}{2}-\frac{1}{j\omega }A)+e^{j\frac{\omega }{2}h}(\frac{I}{2}+\frac{1}{j\omega }A)$

$e^{\mathit{Ah}}=\frac{I}{2}(e^{-j\frac{\omega }{2}h}+e^{j\frac{\omega }{2}h})+\frac{2A}{\omega }\frac{(e^{j\frac{\omega }{2}h}-e^{-j\frac{\omega }{2}h})}{2j}$   and finally

$e^{\mathit{Ah}}=I\cos (\frac{\omega h}{2})+\frac{2A}{\omega }\sin (\frac{\omega h}{2})$    36)

Now we have given a meaning to expression exp(A*h) we can recall the algo 23) to arrive to the final result

4) The final Algo

Taking into account the result achieved in 36) we can now recall 23) where we use A instead of A_K given by 24)

${\overline{X}}_{(k+1)}=e^{\mathit{Ah}}\ast \overline{X_k}+\overline{Q_k}\ast \dot{A}\ast \overline{X_k}$ that can be written as

${\overline{X}}_{(k+1)}=(e^{\mathit{Ah}}+\overline{Q_k}\ast \dot{A})\ast \overline{X_k}$

${\overline{X}}_{(k+1)}=M\ast \overline{X_k}$   where    $M=e^{\mathit{Ah}}+\overline{Q_k}\ast \dot{A}$

assuming   $\rho =\frac{\omega h}{2}$   and considering 36) and 14)

$M=I\cos (\rho )+\frac{2A}{\omega }\sin (\rho )+{\overline{A}}^{-2}\ast (I\cos (\rho )+\frac{2A}{\omega }\sin (\rho )-I-\overline{A}\ast h)\ast \dot{A}$

Knowing that    $A^2=-\frac{{\omega }^2}{4}I$   and thus    $A^{-2}=-\frac{4}{{\omega }^2}I$

$M=I\cos (\rho )+\frac{2A}{\omega }\sin (\rho )+\frac{4}{{\omega }^2}(1-\cos (\rho ))\dot{A}+\frac{4}{{\omega }^2}(h-\frac{2}{\omega }\sin (\rho ))A\dot{A}$   37)

Now let’s calculate the matrix   $A\dot{A}$

$A\dot{A}=\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)\frac{1}{2}\left(\begin{array}{cccc}0& -\dot{P}& -\dot{Q}& -\dot{R}\\ \dot{P}& 0& \dot{R}& -\dot{Q}\\ \dot{Q}& -\dot{R}& 0& \dot{P}\\ \dot{R}& \dot{Q}& -\dot{P}& 0\end{array}\right)$

$A\dot{A}=\frac{1}{4}\left(\begin{array}{cccc}-(P\dot{P}+Q\dot{Q}+R\dot{R})& (Q\dot{R}-R\dot{Q})& (R\dot{P}-P\dot{R})& (P\dot{Q}-Q\dot{P})\\ (R\dot{Q}-Q\dot{R})& -(P\dot{P}+Q\dot{Q}+R\dot{R})& (Q\dot{P}-P\dot{Q})& (R\dot{P}-P\dot{R})\\ (P\dot{R}-R\dot{P})& (P\dot{Q}-Q\dot{P})& -(P\dot{P}+Q\dot{Q}+R\dot{R})& R\dot{Q}-Q\dot{R}\\ (Q\dot{P}-P\dot{Q})& (P\dot{R}-R\dot{P})& (Q\dot{R}-R\dot{Q})& -(P\dot{P}+Q\dot{Q}+R\dot{R})\end{array}\right)$ 38)

The single components of matrix M in 37) are based of the 4 following expressions:

$H=\cos (\rho )-\frac{1}{{\omega }^2}(h-\frac{2}{\omega }\sin (\rho ))(P\dot{P}+Q\dot{Q}+R\dot{R})$      38 a)

$K=\frac{P}{\omega }\sin (\rho )+\frac{2}{{\omega }^2}(1-\cos (\rho ))\dot{P}-\frac{1}{{\omega }^2}(h-\frac{2}{\omega }\sin (\rho ))(Q\dot{R}-R\dot{Q})$   38 b)

$J=\frac{Q}{\omega }\sin (\rho )+\frac{2}{{\omega }^2}(1-\cos (\rho ))\dot{Q}+\frac{1}{{\omega }^2}(h-\frac{2}{\omega }\sin (\rho ))(P\dot{R}-R\dot{P})$    38 c)

$G=-\frac{R}{\omega }\sin (\rho )-\frac{2}{{\omega }^2}(1-\cos (\rho ))\dot{R}+\frac{1}{{\omega }^2}(h-\frac{2}{\omega }\sin (\rho ))(P\dot{Q}-Q\dot{P})$   38 d)

Assuming the following notation as in reference a):

$C_1=\cos (\rho )$   $C_4=\frac{4}{{\omega }^2}(h-\frac{2}{\omega }\sin (\rho ))$

$C{\text{'}}_2=\sin \frac{(\rho )}{\omega }$   $C{\text{'}}_3=\frac{2}{{\omega }^2}(1-\cos (\rho ))$

$A\text{'}=\frac{-(P\dot{P}+Q\dot{Q}+R\dot{R})}{4}$   $B\text{'}=\frac{(P\dot{Q}-Q\dot{P})}{4}$

$C\text{'}=\frac{(R\dot{P}-P\dot{R})}{4}$    $D\text{'}=\frac{(Q\dot{R}-R\dot{Q})}{4}$

expression 38 a),38 b),38 c), and 38 d) assume the following aspect:

$H=C_1+C_{4}A\text{'}$

$G=-C_2\text{'}R-C_3\text{'}\dot{R}+C_{4}B\text{'}$

$J=C_2\text{'}Q+C_3\text{'}\dot{Q}-C_{4}C\text{'}$

$K=C_2\text{'}P+C_3\text{'}\dot{P}-C_{4}D\text{'}$

The algo 23) can be written now as:

${\overline{X}}_{(k+1)}=\overline{M}\ast \overline{X_k}$

where $\overline{M}=\left(\begin{array}{cccc}H& -K& -J& G\\ K& H& -G& -J\\ J& G& H& K\\ -G& J& -K& H\end{array}\right)$   39

making the previous expression explicit leads to:

$\left(\begin{array}{c}{q}_{0(k+1)}\\ q_{1(k+1)}\\ q_{2(k+1)}\\ q_{3(k+1)}\end{array}\right)=\left(\begin{array}{cccc}H& -K& -J& G\\ K& H& -G& -J\\ J& G& H& K\\ -G& J& -K& H\end{array}\right)\left(\begin{array}{c}{q}_{0k}\\ q_{1k}\\ q_{2k}\\ q_{3k}\end{array}\right)$     40)

and more:

$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$

It’s evident now the advantage of this approach which leads to a very simple solution of a problem apparently complicated. The big advantage is the absence of the condition with $\theta =\pm {90}^{°}$ (gimbal lock condition) which leads to an undefined equation like in case of traditional approach.

Two more notation:

1. this algorithm shall be used taking into account good programming rules like for examples avoiding overflow condition while attempting to divide one number for a quantity very close to 0. It is the case when calculating the 4 quantities H,G,J,K where the variable ω act as a divisor. This means that when the aircraft is flying without maneuvering (ω almost equal 0), without proper protection, there is a risk of overflow.

2. Equation 40) is similar to the equation explained in paper a). However some of the components of the matrix 39) has a different collocation. It is interesting to note that matrix 39) becomes exactly equal to matrix M_k in paper a), just inverting the second and forth columns and rows. Thin means swapping the quaternion components q₁ and q₃, as clearly declared at pag 6 of ref a). This is due to a complete different mathematical approach : while in paper a), for the description of the rotation of a body, the Cayley-Cleyn complex matrices have been used (explained in ref B), in this post we used the quaternion approach, as explained in a previous post. This approach leads in my opinion to the clearest and most elegant solution of the problem

5) Code snippet

A simple implementation in C/C++ code is proposed. This routine is supposed to be called cyclically from a main procedure: first run should be with ilong and ilat = 0 to setup all the variable, after that ilong and ilat shall be set to 1: a typical value for dt is 20 ms (50 Hz), that means that to run a 10 seconds long simulation we need to iterate for 500 times the routine. Writing the main procedure is trivial and it is omitted. The routines start setting the quaternion component starting from some initial condition of the Euler angles psi, theta and phi; and at the and of the run a new calculation of psi, theta and phi using the new values of the quaternion components has been made.

#include "quat.h"  /* variable definitions */

#define limit(a) if ( a > 1.) a = 1.; else { if ( a < -1.) a = -1.;}

void Quat(void)

{

if( ilong == 0 && ilat == 0 ) { / no integration, initial condition setting */

/* Initial condition setting (conversion degrees to Radiant*/

fi_2 = (fio*D2R)/2.;

teta_2 =(thetao* D2R )/2.;

psi_2=(psio*D2R)/2.;

costeta0=cosf(teta_2);

sinteta0=sinf(teta_2);

cosfi0=cosf(fi_2);

sinfi0=sinf(fi_2);

cospsi0=cosf(psi_2);

sinpsi0=sinf(psi_2);

/*set initial condition for the 4 quaternion components*/

q1=cospsi0*costeta0*cosfi0+sinpsi0*sinteta0*sinfi0;

q2=sinpsi0*costeta0*cosfi0-cospsi0*sinteta0*sinfi0;

q3=cospsi0*sinteta0*cosfi0+sinpsi0*costeta0*sinfi0;

q4=cospsi0*costeta0*sinfi0-sinpsi0*sinteta0*cosfi0;

}

/* LL integration algo */

/*for variables definition see paper at reference 1)*/

omega2 = p*p + q*q + r*r;

omega = sqrtf ( omega2 );

/* avoid overflow condition*/

if(omega2 < .0000000001) { omega2 = .0000000001; omega= .00001; }

rhok= .5 * omega * dt;

c1 = cosf(rhok);

c2 = sinf(rhok) / omega;

c3 = ( 1. - c1 ) * 2. / omega2;

c4 = ( - 2. * c2 + dt ) / omega2;

h = - ( p * pdot + q * qdot + r * rdot) * c4 + c1;

g = - c2 * p - c3 * pdot + c4 * ( r * qdot - q * rdot );

j = c2 * q + c3 * qdot + c4 * ( r * pdot - p * rdot );

k = c2 * r + c3 * rdot + c4 * ( p * qdot - q * pdot );

q1n = h * q1 + g * q2 - j * q3 - k * q4;

q2n = - g * q1 + h * q2 - k * q3 + j * q4;

q3n = j * q1 + k * q2 + h * q3 + g * q4;

q4= k * q1 - j * q2 - g * q3 + h * q4;

/* Normalization */

qenne = 1. / sqrtf ( q1n*q1n + q2n*q2n + q3n*q3n + q4*q4 );

q1 = q1n * qenne;

q2 = q2n * qenne;

q3 = q3n * qenne;

q4 = q4* qenne;

/* direction cosine calculation */

q12=q1*q1;

q22=q2*q2;

q32=q3*q3;

q42=q4*q4;

c11=q12+q22-q32-q42;

c12=2.*(q1*q4+q2*q3);

c13=2.*(q2*q4-q1*q3);

c21=2.*(q2*q3-q1*q4);

c22=q12-q22+q32-q42;

c23=2.*(q3*q4+q1*q2);

c31=2.*(q2*q4+q1*q3);

c32=2.*(q3*q4-q1*q2);

c33=q12-q22-q32+q42;

/* limiting direction cosine between minus plus 1*/

limit(c11);

limit(c12);

limit(c13);

limit(c21);

limit(c22);

limit(c23);

limit(c31);

limit(c32);

limit(c33);

/*Euler angles Teta,Fi,Psi( radiants )*/

theta=-asinf(c13);

stheta=-c13;

ctheta=cosf(theta);

phi=atan2f(c23,c33);

sphi=sinf(phi);

cphi=cosf(phi);

psi=atan2f(c12,c11);

spsi=sinf(psi);

cpsi=cosf(psi);

}

6) Reference

1. NASA technical note TN D-7347 “Development and application of a local linearization algorithm for the integration of quaternion rate equations in real-time flight simulation problems”, December 1973.

2. Robinson,Alfred C.: On the Use of Quaternionsin Simulation of Rigid-Body Motion. WADC Tech. Rep. 58-17, U.S. Air Force, Dec. 1958. (Available from DDC as AD 234 422.