Attitude from angular rate

Unitary quaternions [1] are used when representing an attitude. They can be updated via integration of angular rate measurements of a gyroscope.

The easiest way to do so 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].

Accumulating rotation over time in quaternion form is done by integrating the differential equation of \(\mathbf{q}\) with a defined rotation rate. This constant augmentation is sometimes termed the Attitude Propagation.

\[\hat{\mathbf{q}}_t = \mathbf{q}_{t-1} + \int_{t-1}^t\boldsymbol\omega\, dt\]

Exact closed-form solutions of the integration are not available. Thus, an approximation method is required. Besides, numerical methods don’t give a continuous solution for \(\mathbf{q}\), but a discrete set of values can: \(\mathbf{q}_t\), \(n=1,2,\dots\)

In the simplest practical case, the angular rates are measured by gyroscopes, reading instantaneous angular velocities, \(\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

We start by expressing our angular velocity in quaternion form, so that we can use the quaternion derivative to integrate it and estimate the new attitude.

An orientation (attitude) is described with a quaternion \(\mathbf{q} (t)\) at a time \(t\), and with \(\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 [2] 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 as a common matrix to multiply with the quaternion as any other linear operation.

Quaternion Integration

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}\]

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.

We can stop here and use this closed-form solution to update the quaternion, but it cannot be used in a linear system. For that, we need to linearize the quaternion update equation.

Quaternion Linearization

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 for \(\mathbf{q}_{t+1}\) also follows the 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}\|}\]

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

[1]

A vector \(\mathbf{x}\) is unitary if its norm is equal to one: \(\|\mathbf{x}\|=1\).

[2]

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: ndarray | None = None, q0: ndarray | None = None, frequency: float = 100.0, order: int = 1, **kw)

Quaternion update by integrating measured angular velocities.

All estimated quaternions are stored in the attribute Q as a numpy.ndarray of shape (N, 4), where N is the number of estimated attitudes. The first row is the initial attitude, and the last row is the final attitude.

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', 'closed', or 'integration'. The option 'integration' is a simple numerical integration of the angular velocity as roll-pitch-yaw angles.

• order (int) – Truncation order, if method 'series' is used.

• representation (str, default: 'quaternion') – Attitude representation. Options are 'quaternion', 'angles' or 'rotmat'.

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.

Initial Values

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]])

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]])

integrate_angular_positions(gyr: ndarray, dt: float | None = None) → ndarray

Integrate angular positions \(\mathbf{\theta}\) from instantaneous angular rates \(\mathbf{\omega}\) at a time \(t\) with a given time step \(\Delta t\):

\[\mathbf{\theta}_{t+1} = \mathbf{\theta}_t + \mathbf{\omega}_t \Delta t\]

Given the three main roll-pitch-yaw angles, it simply integrates them with a cumulative sum, and returns the tri-axial angular positions.

This method does not play a central role in this estimator, but can be used to obtain another reference to compare the results to.

Calling this method is equivalent to calling:

>>> angular_positions = numpy.cumsum(gyr * dt, axis=0)

Parameters:

• gyr (array_like) – Angular rates, in rad/s.

• dt (float, optional) – Time step, in seconds. If not given, the time step is set to \(1/f_s\), where \(f_s\) is the sampling frequency, which is defined as 100 Hz by default.

Returns:

theta – Tri-axial angular positions, in rad.

Return type:

numpy.ndarray

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

Update the quaternion estimation

Estimate quaternion \(\mathbf{q}_{t+1}\) from given a-priori quaternion \(\mathbf{q}_t\) with a measured instantaneous angular rate \(\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\]

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.

Returns:

q – Estimated quaternion.

Return type:

numpy.ndarray

Examples

>>> from ahrs.filters import AngularRate
>>> gyro_data.shape
(1000, 3)
>>> num_samples = gyro_data.shape[0]
>>> Q = np.zeros((num_samples, 4))      # Allocation of quaternions
>>> Q[0] = [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]])