# Attitude from angular rate¶

A quaternion is updated via integration of angular rate measurements of a gyroscope. The easiest way to update the quaternions is by integrating the differential equation for a local rotation rate [Sola].

In a kinematic system, the angular velocity $$\boldsymbol\omega$$ of a rigid body at any instantaneous time is described with respect to a fixed frame coinciding instantaneously with its body frame. Thus, this angular velocity is in terms of the body frame [Jia].

First, let’s remember the definition of a vector or matrix exponential as a power series:

$e^\mathbf{X} = \sum_{k=0}^\infty \frac{1}{k!} \mathbf{X}^k$

Letting $$\mathbf{v}=\mathbf{u}\theta$$ be the rotation vector, representing a rotation of $$\theta$$ radians around the unitary  axis $$\mathbf{u}=\begin{bmatrix}u_x & u_y & u_z\end{bmatrix}^T$$, we can get its exponential series as:

$e^\mathbf{v} = e^{\mathbf{u}\theta} = \Big(1 - \frac{\theta^2}{2!} + \frac{\theta^4}{4!} + \cdots\Big) + \Big(\mathbf{u}\theta - \frac{\mathbf{u}\theta^3}{3!} + \frac{\mathbf{u}\theta^5}{5!} + \cdots\Big)$

We recognize the power-series expansion of Euler’s formula, which helps to map the quaternion $$\mathbf{q}$$ from a rotation vector $$\mathbf{v}$$. This exponential map [Sola] is formerly defined as:

$\begin{split}\mathbf{q} = e^\mathbf{v} = \begin{bmatrix}\cos\frac{\theta}{2} \\ \mathbf{u}\sin\frac{\theta}{2}\end{bmatrix}\end{split}$

Accumulating rotation over time in quaternion form is done by integrating the differential equation of $$\mathbf{q}$$ to the rotation rate definition. This constant augmentation is sometimes termed the Attitude Propagation.

Because exact closed-form solutions of the integration are not available, an approximation method is required. Numerical solutions, however, don’t give a continuous solution for $$\mathbf{q}$$, but a discrete set of values $$\mathbf{q}_t$$, $$n=1,2,\dots$$

In our case, the angular rates are measured by gyroscopes, providing local measurements, $$\boldsymbol\omega(t_n)=\begin{bmatrix}\omega_x&\omega_y&\omega_z\end{bmatrix}^T$$, in rad/s, at discrete times $$t_n = n\Delta t$$ in the local sensor frame. The parameter $$\Delta t$$ is called time step or step size of the numerical integration.

## Quaternion Derivative¶

An orientation is described by $$\mathbf{q} (t+\Delta t)$$ at a time $$t+\Delta t$$. This is after a rotation change $$\Delta\mathbf{q}$$ during $$\Delta t$$ seconds is performed on the local frame [Jia].

This rotation change about the instantaneous axis $$\mathbf{u}=\frac{\boldsymbol\omega}{\|\boldsymbol\omega\|}$$ through the angle $$\theta=\|\boldsymbol\omega\|\Delta t$$ can be described by a quaternion too:

$\begin{split}\begin{array}{rcl} \Delta\mathbf{q} &=& \cos\frac{\theta}{2} + \mathbf{u}\sin\frac{\theta}{2} \\ &=& \cos\frac{\|\boldsymbol\omega\|\Delta t}{2} + \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|}\sin\frac{\|\boldsymbol\omega\|\Delta t}{2} \end{array}\end{split}$

implying that $$\mathbf{q}(t+\Delta t)=\Delta\mathbf{qq}(t)$$. To obtain the derivative we consider $$\mathbf{q} = \mathbf{q}(t)$$ as the original state at a time $$t$$, and the difference between states as:

$\begin{split}\begin{array}{rcl} \mathbf{q}(t+\Delta t)-\mathbf{q}(t) &=& \Big(\cos\frac{\|\boldsymbol\omega\|\Delta t}{2} + \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|}\sin\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\mathbf{q} - \mathbf{q} \\ &=& \Big(-2\sin^2\frac{\|\boldsymbol\omega\|\Delta t}{4} + \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|}\sin\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\mathbf{q} \end{array}\end{split}$

The development of the time-derivative of the quaternions  follows the formal definition:

$\begin{split}\begin{array}{rcl} \dot{\mathbf{q}} &=& \underset{\Delta t\to 0}{\mathrm{lim}} \frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t} \\ &=& \underset{\Delta t\to 0}{\mathrm{lim}} \frac{1}{\Delta t}\Big(-2\sin^2\frac{\|\boldsymbol\omega\|\Delta t}{4} + \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|}\sin\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\mathbf{q} \\ &=& \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|} \underset{\Delta t\to 0}{\mathrm{lim}} \frac{1}{\Delta t}\sin\big(\frac{\|\boldsymbol\omega\|\Delta t}{2}\big) \mathbf{q} \\ &=& \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|} \frac{d}{dt} \sin\big(\frac{\|\boldsymbol\omega\|}{2}t\big) \Big |_{t=0} \mathbf{q} \\ &=& \frac{1}{2}\boldsymbol\omega \mathbf{q} \\ &=& \frac{1}{2} \begin{bmatrix} -\omega_x q_x -\omega_y q_y - \omega_z q_z\\ \omega_x q_w + \omega_z q_y - \omega_y q_z\\ \omega_y q_w - \omega_z q_x + \omega_x q_z\\ \omega_z q_w + \omega_y q_x - \omega_x q_y \end{bmatrix} \end{array}\end{split}$

Warning

The product between the angular velocity $$\boldsymbol\omega\in\mathbb{R}^3$$ and the quaternion $$\mathbf{q}\in\mathbb{H}^4$$ is done by representing the former as a pure quaternion $$\boldsymbol\omega= \begin{bmatrix}0 & \omega_x & \omega_y & \omega_z\end{bmatrix}\in\mathbb{H}^4$$, so that a Hamilton Product can be applied.

Defining the omega operator as

$\begin{split}\boldsymbol\Omega(\boldsymbol\omega) = \begin{bmatrix}0 & -\boldsymbol\omega^T \\ \boldsymbol\omega & \lfloor\boldsymbol\omega\rfloor_\times\end{bmatrix} = \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix}\end{split}$

Note

The expression $$\lfloor\mathbf{x}\rfloor_\times$$ expands a vector $$\mathbf{x}\in\mathbb{R}^3$$ into a $$3\times 3$$ skew-symmetric matrix:

$\begin{split}\lfloor\mathbf{x}\rfloor_\times = \begin{bmatrix} 0 & x_2 & -x_1 \\ -x_2 & 0 & x_0 \\ x_1 & -x_0 & 0\end{bmatrix}\end{split}$

we get an equivalent matrix expression for the derivative:

$\dot{\mathbf{q}} = \frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\mathbf{q}$

This definition does not require a Hamilton product and the Omega operator can be used to simply multiply with the quaternion as any other matrix operation.

## Quaternion Integration¶

Now we develop an $$n^{th}$$-order polynomial linearization method built from Taylor series of $$\mathbf{q}(t+\Delta t)$$ around the time $$t$$ for the quaternion:

$\mathbf{q}_{t+1} = \mathbf{q}_t + \dot{\mathbf{q}}_t\Delta t + \frac{1}{2!}\ddot{\mathbf{q}}_t\Delta t^2 + \frac{1}{3!}\dddot{\mathbf{q}}_t\Delta t^3 + \cdots$

Using the definition of $$\dot{\mathbf{q}}$$ the new orientation $$\mathbf{q}_{t+1}$$ is written as [Wertz]:

$\begin{split}\begin{array}{rcl} \mathbf{q}_{t+1} &=& \Bigg[\mathbf{I}_4 + \frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t + \frac{1}{2!}\Big(\frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t\Big)^2 + \frac{1}{3!}\Big(\frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t\Big)^3 + \cdots\Bigg]\mathbf{q}_t \\ && \qquad{} + \frac{1}{4}\dot{\boldsymbol\Omega}(\boldsymbol\omega)\Delta t^2\mathbf{q}_t + \Big[\frac{1}{12}\dot{\boldsymbol\Omega}(\boldsymbol\omega)\boldsymbol\Omega(\boldsymbol\omega) + \frac{1}{24}\boldsymbol\Omega(\boldsymbol\omega)\dot{\boldsymbol\Omega}(\boldsymbol\omega) + \frac{1}{12}\ddot{\boldsymbol\Omega}(\boldsymbol\omega)\Big]\Delta t^3\mathbf{q}_t + \cdots \end{array}\end{split}$

Assuming the angular rate is constant over the period $$[t, t+1]$$, we have $$\dot{\boldsymbol\omega}=0$$, and we can prescind from the derivatives of $$\boldsymbol\Omega$$ reducing the series to:

$\mathbf{q}_{t+1} = \Bigg[\mathbf{I}_4 + \frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t + \frac{1}{2!}\Big(\frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t\Big)^2 + \frac{1}{3!}\Big(\frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t\Big)^3 + \cdots\Bigg]\mathbf{q}_t$

The error of the approximation vanishes rapidly at higher orders, or when the time step $$\Delta t \to 0$$. The more terms we have, the better our approximation should be, assuming the sensor signals are unbiased and noiseless, with the downside of a big computational demand.

Notice the series has the known form of the matrix exponential:

$e^{\frac{\Delta t}{2}\boldsymbol\Omega(\boldsymbol\omega)} = \sum_{k=0}^\infty \frac{1}{k!} \Big(\frac{\Delta t}{2}\boldsymbol\Omega(\boldsymbol\omega)\Big)^k$

For our purpose a truncation up to the second term, making it of first order ($$k=1$$), is implemented.

$\mathbf{q}_{t+1} = \Bigg[\mathbf{I}_4 + \frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t\Bigg]\mathbf{q}_t$

Two good reasons for this truncation are:

• Simple architectures (like embedded systems) avoid the burden of the heavy computation, and still achieve fairly good results.
• If the sensor signals are not properly filtered and corrected, the estimation will not converge to a good result, and even worsening it at higher orders.

The resulting quaternion must be normalized to operate as a versor, and to represent a valid orientation.

$\mathbf{q}_{t+1} \gets \frac{\mathbf{q}_{t+1}}{\|\mathbf{q}_{t+1}\|}$

Assuming the gyroscope data is sampled at a fixed rate and that the angular velocity vector, $$\boldsymbol\omega$$, in local body coordinates is constant over the sampling interval $$\Delta t$$, we can alternatively integrate $$\dot{\mathbf{q}}$$ to obtain:

$\mathbf{q}_{t+1} = e^{\frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t}\mathbf{q}_t$

Using the Euler-Rodrigues rotation formula and the exponential map from above we find a closed-form solution [Wertz]:

$\mathbf{q}_{t+1} = \Bigg[ \cos\Big(\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\mathbf{I}_4 + \frac{1}{\|\boldsymbol\omega\|}\sin\Big(\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\boldsymbol\Omega(\boldsymbol\omega) \Bigg]\mathbf{q}_t$

where $$\mathbf{I}_4$$ is a $$4\times 4$$ Identity matrix. The large term inside the brackets, multiplying $$\mathbf{q}_t$$, is an orthogonal rotation retaining the normalization of the propagated attitude quaternions. Thus, it is not necessary to normalize $$\mathbf{q}_{t+1}$$, but it is highly recommended to do so in order to avoid any round-off errors inherent to all computers.

Numerical Integration based on Runge-Kutta methods can be employed to increase the accuracy, and are shown to be more effective. See [Sola] and [Zhao] for a comparison of the different methods, their accuracy, and their computational load.

## Footnotes¶

  A vector $$\mathbf{x}$$ is unitary if its norm is equal to one: $$\|\mathbf{x}\|=1$$.
  The successive derivatives of $$\mathbf{q}_n$$ are obtained by repeatedly applying the expression of the quaternion derivative, with $$\ddot{\boldsymbol\omega}=0$$. $\begin{split}\begin{array}{rcl} \dot{\mathbf{q}} &=& \frac{1}{2}\mathbf{q}\boldsymbol\omega \\ \ddot{\mathbf{q}} &=& \frac{1}{4}\mathbf{q}\boldsymbol\omega^2 + \frac{1}{2}\mathbf{q}\dot{\boldsymbol\omega} \\ \dddot{\mathbf{q}} &=& \frac{1}{6}\mathbf{q}\boldsymbol\omega^3 + \frac{1}{4}\mathbf{q}\dot{\boldsymbol\omega}\boldsymbol\omega + \frac{1}{2}\mathbf{q}\boldsymbol\omega\dot{\boldsymbol\omega} \\ \mathbf{q}^{i\geq 4} &=& \frac{1}{2^i}\mathbf{q}\boldsymbol\omega^i + \cdots \end{array}\end{split}$

References

 [Jia] (1, 2) Yan-Bin Jia. Quaternions. 2018. (http://web.cs.iastate.edu/~cs577/handouts/quaternion.pdf)
 [Sola] (1, 2, 3) Solà, Joan. Quaternion kinematics for the error-state Kalman Filter. October 12, 2017. (http://www.iri.upc.edu/people/jsola/JoanSola/objectes/notes/kinematics.pdf)
 [Zhao] F. Zhao and B.G.M. van Wachem. A novel Quaternion integration approach for describing the behaviour of non-spherical particles. (https://link.springer.com/content/pdf/10.1007/s00707-013-0914-2.pdf)
class ahrs.filters.angular.AngularRate(gyr: numpy.ndarray = None, q0: numpy.ndarray = None, frequency: float = 100.0, order: int = 1, **kw)

Quaternion update by integrating angular velocity

Parameters: gyr (numpy.ndarray, default: None) – N-by-3 array with measurements of angular velocity in rad/s. q0 (numpy.ndarray) – Initial orientation, as a versor (normalized quaternion). frequency (float, default: 100.0) – Sampling frequency in Herz. Dt (float, default: 0.01) – Sampling step in seconds. Inverse of sampling frequency. Not required if frequency value is given. method (str, default: 'closed') – Estimation method to use. Options are: 'series' or 'closed'. order (int) – Truncation order, if method 'series' is used. gyr (numpy.ndarray) – N-by-3 array with N gyroscope samples. Q (numpy.ndarray) – Estimated quaternions. method (str) – Used estimation method. order (int) – Truncation order.

Examples

>>> gyro_data.shape             # NumPy arrays with gyroscope data in rad/s
(1000, 3)
>>> from ahrs.filters import AngularRate
>>> angular_rate = AngularRate(gyr=gyro_data)
>>> angular_rate.Q
array([[ 1.00000000e+00,  0.00000000e+00,  0.00000000e+00,  0.00000000e+00],
[ 9.99999993e-01,  2.36511228e-06, -1.12991334e-04,  4.28771947e-05],
[ 9.99999967e-01,  1.77775173e-05, -2.43529706e-04,  8.33144162e-05],
...,
[-0.92576208, -0.23633121,  0.19738534, -0.2194337 ],
[-0.92547793, -0.23388968,  0.19889139, -0.22187479],
[-0.92504595, -0.23174096,  0.20086376, -0.22414251]])
>>> angular_rate.Q.shape        # Estimated attitudes as Quaternions
(1000, 4)


The estimation of each attitude is built upon the previous attitude. This estimator sets the initial attitude equal to the unit quaternion [1.0, 0.0, 0.0, 0.0] by default, because we cannot obtain the first orientation with gyroscopes only.

We can use the class Tilt to estimate the initial attitude with a simple measurement of a tri-axial accelerometer:

>>> from ahrs.filter import Tilt
>>> tilt = Tilt()
>>> q_initial = tilt.estimate(acc=acc_sample)  # One tridimensional sample suffices
>>> angular_rate = AngularRate(gyr=gyro_data, q0=q_initial)
>>> angular_rate.Q
array([[ 0.77547502,  0.6312126 ,  0.01121595, -0.00912944],
[ 0.77547518,  0.63121388,  0.01110125, -0.00916754],
[ 0.77546726,  0.63122508,  0.01097435, -0.00921875],
...,
[-0.92576208, -0.23633121,  0.19738534, -0.2194337 ],
[-0.92547793, -0.23388968,  0.19889139, -0.22187479],
[-0.92504595, -0.23174096,  0.20086376, -0.22414251]])


The Tilt can also use a magnetometer to improve the estimation with the heading orientation.

>>> q_initial = tilt.estimate(acc=acc_sample, mag=mag_sample)
>>> angular_rate = AngularRate(gyr=gyro_data, q0=q_initial)
>>> angular_rate.Q
array([[ 0.66475674,  0.55050651, -0.30902706, -0.39942875],
[ 0.66473764,  0.5504497 , -0.30912672, -0.39946172],
[ 0.66470495,  0.55039529, -0.30924191, -0.39950193],
...,
[-0.90988476, -0.10433118,  0.28970402,  0.27802214],
[-0.91087203, -0.1014633 ,  0.28977124,  0.2757716 ],
[-0.91164416, -0.09861271,  0.2903888 ,  0.27359606]])

update(q: numpy.ndarray, gyr: numpy.ndarray, method: str = 'closed', order: int = 1, dt: float = None) → numpy.ndarray

Update the quaternion estimation

Estimate quaternion $$\mathbf{q}_{t+1}$$ from given a-priori quaternion $$\mathbf{q}_t$$ with a given angular rate measurement $$\mathbf{\omega}$$.

If method='closed', the new orienation is computed with the closed-form solution:

$\mathbf{q}_{t+1} = \Bigg[ \cos\Big(\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\mathbf{I}_4 + \frac{1}{\|\boldsymbol\omega\|}\sin\Big(\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\boldsymbol\Omega(\boldsymbol\omega) \Bigg]\mathbf{q}_t$

Otherwise, if method='series', it is computed with a series of the form:

$\mathbf{q}_{t+1} = \Bigg[\sum_{k=0}^\infty \frac{1}{k!} \Big(\frac{\Delta t}{2}\boldsymbol\Omega(\boldsymbol\omega)\Big)^k\Bigg]\mathbf{q}_t$

where the order $$k$$ in the series has to be set as non-negative integer in the parameter order. By default it is set equal to 1.

Parameters: q (numpy.ndarray) – A-priori quaternion. gyr (numpy.ndarray) – Array with triaxial measurements of angular velocity in rad/s. method (str, default: 'closed') – Estimation method to use. Options are: 'series' or 'closed'. order (int) – Truncation order, if method 'series' is used. dt (float, default: None) – Time step, in seconds, between consecutive Quaternions. q – Estimated quaternion. numpy.ndarray

Examples

>>> from ahrs.filters import AngularRate
>>> gyro_data.shape
(1000, 3)
>>> num_samples = gyro_data.shape
>>> Q = np.zeros((num_samples, 4))      # Allocation of quaternions
>>> Q = [1.0, 0.0, 0.0, 0.0]         # Initial attitude as a quaternion
>>> angular_rate = AngularRate()
>>> for t in range(1, num_samples):
...     Q[t] = angular_rate.update(Q[t-1], gyro_data[t])
...
>>> Q
array([[ 1.00000000e+00,  0.00000000e+00,  0.00000000e+00,  0.00000000e+00],
[ 9.99999993e-01,  2.36511228e-06, -1.12991334e-04,  4.28771947e-05],
[ 9.99999967e-01,  1.77775173e-05, -2.43529706e-04,  8.33144162e-05],
...,
[-0.92576208, -0.23633121,  0.19738534, -0.2194337 ],
[-0.92547793, -0.23388968,  0.19889139, -0.22187479],
[-0.92504595, -0.23174096,  0.20086376, -0.22414251]])