Requirement:

• quaternions - The tool to deal with 3D orientation & rotations

Quaternions - Application to drones orientation

When we deal with drones it’s always super useful to track how its own referential is oriented w.r.t. the one of the world. Indeed, we always want to know whether we go up or down, east or west.

The gyroscope

On tracking the drone orientation’s variation

A the gyroscope tells us how the drone’s referential orientation is varying w.r.t. the world’s referential. But be carful, those variations are expressed in the drone’s referential axes ! In math, we define the followings variables :

• Let $w^{(drone)}_{(t)} := w_{x(t)}^{(drone)} \cdot i + w_{y(t)}^{(drone)} \cdot j + w_{z(t)}^{(drone)} \cdot k$ be the rotation vector measured by the gyroscope at time $t$. Basically, $w^{(drone)}_{x(t)}$ is the rotation speed around the axis $x$ in the drone’s referential at time $t$, etc.
• Let $v^{(drone)}$ be a vector expressed in the drone’s referential,
• Let $v^{(world)}$ be a vector expressed in the world’s referential.

At a time $t$ the drone is oriented such that the mapping between a vector $v$ expressed in the drone’s referential $v^{(drone)}$ and the same vector expressed in the world’s referential is :

$$v^{(world)} = q^{(d2w)}_{r(t)} \cdot v^{(drone)}\cdot \overline{q^{(d2w)}_{r(t)}}, \qquad \qquad (\star)$$

where $q^{(d2w)}_{r(t)}$ is the quaternion quantifying the rotation between those two referential at time $t$.

If at this time $t$ the drone has an angular speed measured by its gyroscope $w^{(drone)}_{(t)}$, it means that at time $t+\Delta t$ a new mapping $v^{(drone)}\rightarrow v^{(world)}$ holds. Let’s first construct the rotation quaternion associated to an angular speed $w^{(drone)}_{(t)}$ in the drone’s referential :

$$q^{(drone)}_{w\Delta t} := f_q\left(\Delta t \|w_{(t)}^{(drone)}\|, \frac{w_{(t)}^{(drone)}}{\|w_{(t)}^{(drone)}\|}\right)$$

where $\Delta t \|w_{(t)}^{(drone)}\|$ is angle done around the direction $w_{(t)}^{(drone)}/\|w_{(t)}^{(drone)}\|$ in the drone’s referential over a time $\Delta t$. This rotation quaternion can also be expressed in the world referential by just using the mapping $drone\rightarrow world \ : \ q^{(d2w)}_{r(t)}$ we already know:

$$q^{(world)}_{w\Delta t} = q^{(d2w)}_{r(t)} \cdot q^{(drone)}_{w\Delta t} \cdot \overline{q^{(d2w)}_{r(t)}}.$$

That is enough to get the new mapping at $t+\Delta t$:

$$v^{(world)} = \underbrace{q^{(world)}_{w\Delta t} \cdot q^{(d2w)}_{r(t)}}_{q^{(d2w)}_{r(t+\Delta t)}} \cdot v^{(drone)} \cdot \overline{q^{(d2w)}_{r(t)}} \cdot \overline{q^{(world)}_{w\Delta t}}.$$

This results is all we need to know to continuously track a drone orientation over time with only a gyroscope. However, this results can be rearranged a little to facilitate computations on an embedded device. Indeed, observe that :

$$\begin{aligned} q^{(d2w)}_{r(t+\Delta t)} = \ & q^{(world)}_{w\Delta t} \cdot q^{(d2w)}_{r(t)}, \\ = \ & q^{(d2w)}_{r(t)} \cdot q^{(drone)}_{w\Delta t} \cdot \overline{q^{(d2w)}_{r(t)}} \cdot q^{(d2w)}_{r(t)}, \\ = \ & \underline{q^{(d2w)}_{r(t)} \cdot q^{(drone)}_{w\Delta t}}, \end{aligned}$$

which means that we don’t need to compute the projection $q^{(drone)}_{w\Delta t} \rightarrow q^{(world)}_{w\Delta t}$ to then compute $q^{(d2w)}_{r(t+\Delta t)}$. It can be done directly from $q^{(d2w)}_{r(t)}$ and $q^{(drone)}_{w\Delta t}$, which reduces the computation load !

Practical limits of the gyroscope - we need more sensors

Cool, the gyroscope helps us tracking the orientation’s variation of the drone w.r.t. the world, but we have at least two problems :

• Initialisation issue : The gyroscope tells us how the drone’s referential is varying, but it doesn’t provide initial values !
• Integration issue : The drone’s orientation variation is never perfectly integrated. Over each time period $\Delta t$ we assume the angular speed $w$ is constant - but in practice it isn’t. Also, this measurement is subject to noise and tiny errors. That means that over time the accumulation of all those small errors leads to large errors. To solve those two issues, we need sensors providing direct measures (not just variation measures) :
• An accelerometer
• A magnetometer

The accelerometer

The idea is to use the error between the ground direction measured with the accelerometer and its estimated value with the mapping $q^{(d2w)}$, to then correct this mapping.

Estimated ground direction - According to the mapping d2w :

The mapping between the ground directions $v_g$ w.r.t. the world $v_g^{(world)}$ and the drone $v_g^{(drone)}$ referential is :

$$\left. \begin{array}{l} v^{(world)} := q^{(d2w)}\cdot v^{drone}\cdot \overline{q^{(d2w)}} \\ v_{g}^{(world)} := k \end{array} \right\} \quad \Rightarrow \quad k = q^{(d2w)}\cdot v_{g,est}^{(drone)}\cdot \overline{q^{(d2w)}}, \quad \Rightarrow \quad v_{g,est}^{(drone)} = \overline{q^{(d2w)}} \cdot k \cdot q^{(d2w)}.$$

Which, after the following detailed computation :

$$\begin{aligned} q^{(d2w)} := \ & q_0+q_1i+q_2j + q_3k, \qquad \text{(setup the notations used next)}, \\ v_{g,est}^{(drone)} := \ & \left(q_0-q_1i-q_2j-q_3k\right)\cdot k \cdot \left(q_0+q_1i+q_2j+q_3k\right), \\ = \ & \left(q_0k+q_1j-q_2i+q_3\right)\cdot \left(q_0+q_1i+q_2j+q_3k\right), \\ = \ & kq_0^2+jq_0q_1 - i q_0q_2 - q_0q_3 + j q_0q_1 + jiq_1^2 - q_1q_2 + jkq_1q_3 - iq_0q_2 +q_1q_2 -ijq_2^2 -ikq_2q_3 + q_0q_3 + i q_1q_3 +jq_2q_3 + kq_3^2, \\ = \ & kq_0^2+jq_0q_1 - i q_0q_2 - q_0q_3 + j q_0q_1 - kq_1^2 - q_1q_2 + i q_1q_3 - iq_0q_2 +q_1q_2 -kq_2^2 + jq_2q_3 + q_0q_3 + i q_1q_3 +jq_2q_3 + kq_3^2, \\ = \ & (-q_0q_3 - q_1q_2 + q_1q_2 +q_0q_3) + i\cdot(-q_0q_2 + q_1q_3 - q_0q_2 + q_1q_3) + j \cdot (q_0q_1 + q_0q_1 + q_2q_3 + q_2q_3) + k \cdot (q_0^2 -q_1^2 -q_2^2 +q_3^2), \end{aligned}$$

which leads to the following important result:

$$v_{g,est}^{(drone)} = i\cdot 2 (q_1q_3 - q_0q_2) + j\cdot2(q_0q_1 + q_2q_3) + k\cdot(q_0^2 -q_1^2 -q_2^2 +q_3^2). \qquad \qquad (\text{ground direction})$$

Measured ground direction - According to the accelerometer

The accelerometer measures the acceleration affecting the drone, and returns the vector $a^{(drone)}$ in the drone’s referential. When the drone has a linear movement, the only acceleration affecting it is the gravity, which reveals the direction of the ground :

$$v_{g,meas}^{(drone)} = ( a_x\cdot i + a_y \cdot j + a_z \cdot k )/ \|a^{(drone)}\|, \qquad a^{(drone)} := [a_x; a_y; a_z].$$

Reconciliation of measured and estimated values

The error between those two directions can be quantified with the following vectorial product:

$$ err := v_{g,est}^{(drone)} \times v_{g,meas}^{(drone)} = \begin{pmatrix}2 (q_1q_3 - q_0q_2) \\ 2(q_0q_1 + q_2q_3) \\ q_0^2 -q_1^2 -q_2^2 +q_3^2 \end{pmatrix} \times \begin{pmatrix} a_x/ \|a^{(drone)}\| \\ a_y/ \|a^{(drone)}\| \\ a_z/ \|a^{(drone)}\| \end{pmatrix} = \begin{pmatrix} 2(q_0q_1 + q_2q_3)a_z - (q_0^2 -q_1^2 -q_2^2 +q_3^2)a_y \\ (q_0^2 -q_1^2 -q_2^2 +q_3^2)a_x - 2 (q_1q_3 - q_0q_2)a_z \\ 2 (q_1q_3 - q_0q_2)a_y - 2(q_0q_1 + q_2q_3)a_x \end{pmatrix} \frac{1}{\|a^{drone}\|}$$

where $\|err\|$ is the sinus of the angle separating the vectors $v_{g,est}^{(drone)}$ and $v_{g,meas}^{(drone)}$, and the direction $err/ \|err\|$ tells how this rotation has to be implemented to move $v_{g,est}^{(drone)}$ from its position to $v_{g,meas}^{(drone)}$’s position.

Now 2 options are possible :

Option 1 : Direct correction (works in theory)

We correct $v_{g,est}^{(drone)}$ :

$$q_{corr} := f_q\left(K_p\cdot arcsin(\|err\|), \frac{err}{\|err\|}\right), \qquad v_{g,corr}^{(drone)} = q_{corr} \cdot v_{g,est}^{(drone)} \cdot \overline{q_{corr}} = q_{corr} \cdot \overline{q^{(d2w)}} \cdot k \cdot \underbrace{q^{(d2w)} \cdot \overline{q_{corr}}}_{\text{corrected }q^{(d2w)}},$$

where $K_p \in ]0,1]$ is used to filter the correction to “reduce the effects” measurements noises on the correction. Which means we do this replacement :

$$q^{(d2w)} \leftarrow q^{(d2w)}\cdot\overline{q_{corr}}.$$

Option 2 : Indirect correction (works well in practice on embedded systems : more computationally efficient)

At time $t$, we first correct the mapping according to the accelerometer measurement to then introduce the variation to predict the mapping at $t+\Delta t$. That means computing :

$$q_{r(t+\Delta t)}^{(d2w)} = \underbrace{q_{r(t)}^{(d2w)} \cdot \overline{q_{corr}}}_{\text{corrected: } q_{r(t)}^{(d2w)} } \cdot \underbrace{q_{w\Delta t}^{(drone)}}_{\text{varitation to apply}},\qquad \Rightarrow \qquad q_{r(t+\Delta t)}^{(d2w)} = q_{r(t)}^{(d2w)} \cdot \underbrace{\overline{q_{corr}}\cdot q_{w\Delta t}^{(drone)}}_{\text{update factor}}.$$

The update factor (on the right hand side) shows that tis correction could be assimilated to the gyroscope update. Ideally we want a simple way to compute this “update factor”.

• Simplifying $q_{w\Delta t}^{(drone)}$ : The first thing to remark is that, if $\Delta t$ is small, then $q_{w\Delta t}^{(drone)}$ should be a tiny rotation. That means that the angle $\Delta t \|w\|$ is small, which means that the following approximations are acceptable: $$\left.\begin{array}{l} cos(\theta) \approx 1 \\ sin(\theta) \approx\theta \end{array}\right\} \Rightarrow \quad q_{w\Delta t}^{(drone)} = f_q\left(\Delta t\|w\|, \frac{w}{\|w\|}\right) \approx 1 + \frac{\Delta t \|w\|}{2}\cdot \frac{w}{\|w\|} = \underline{1 + \frac{\Delta t}{2}\cdot w }.$$
• Simplifying $\overline{q_{corr}}$: At each time step $\Delta t$ we want to implement a small correction to the mapping $q_{r(t)}^{(d2w)}$ to “slowly” push it towards its correct value. To this end, we will take the error vector $err$ as a rotation speed to apply to the mapping, and define : $$q_{corr} := f_q\left(K_p\cdot\Delta t \cdot \|err\|, \frac{err}{\|err\|} \right) \approx 1 + \frac{\Delta t}{2} K_p \cdot err, \qquad \Rightarrow \qquad \underline{\overline{q_{corr}} \approx 1 - \frac{\Delta t}{2} K_p \cdot err}.$$
• Combining: Then we combine everything : $$\overline{q_{corr}}\cdot q_{w\Delta t}^{(drone)} \approx \left( 1 - \frac{\Delta t}{2} K_p \cdot err\right) \cdot \left( 1 + \frac{\Delta t}{2}\cdot w \right) = 1 + \frac{\Delta t}{2}\left(w - K_p \cdot err \right) + \underbrace{\frac{\Delta t^2}{4} K_p \cdot err \cdot w}_{\text{negligeable du to :} \Delta t^2 \approx 0} \approx \underline{1 + \frac{\Delta t}{2}\left(w - K_p \cdot err \right)}.$$
• We define the shifted gyroscope measurements as : $$g := w - K_p \cdot err := g_xi + g_yj + g_zk$$
• Simplified update : The update is now : $$\begin{aligned}q_{r(t+\Delta t)}^{(d2w)}= \ & q_{r(t)}^{(d2w)} \cdot \left( 1 + \frac{\Delta t}{2}g \right) \\ = \ & (q_0 + q_1i+q_2j + q_3k) \left( 1 + \frac{\Delta t}{2}(g_xi + g_yj + g_zk)\right) \\ = \ & \vdots \\ = \ & \Big(q_0 + \left(-q_1g_x - q_2g_y - q_3g_z \right) \Big) + i \cdot\Big(q_1 + \left(q_0g_x + q_2g_z - q_3g_y \right) \Big) + j \cdot \Big(q_2 + \left(q_0g_y - q_1g_z + q_3g_x\right) \Big) + \dots \\ & \ \dots \ k \cdot \Big(q_3 + \left(q_0g_z + q_1g_y - q_2g_x\right) \Big) \end{aligned}$$
• Normalization : Since we didn’t perfectly applied successive rotations, but instead approximated them, the resulting quaternion won’t have a perfected unitary norm, which is important to keep so that $q_{r(t+\Delta t)}^{(d2w)}$ remains a pur rotation. This can easily be enforced by doing the following update : $$q_{r(t+\Delta t)}^{(d2w)} \leftarrow \frac{q_{r(t+\Delta t)}^{(d2w)}}{\|q_{r(t+\Delta t)}^{(d2w)}\|}.$$

magnetometer

to do - but it’s very similar to using the accelerometer measurements to add virtual rotation to the gyroscope outputs.