angular_velocities

ahrs.common.quaternion.QuaternionArray.angular_velocities(self, dt: float) → ndarray

Compute the angular velocity between N Quaternions.

It assumes a constant sampling rate of dt seconds, and returns the angular velocity around the X-, Y- and Z-axis (roll-pitch-yaw angles), in radians per second.

The angular velocities \(\omega_x\), \(\omega_y\), and \(\omega_z\) are computed from quaternions \(\mathbf{q}_t=\Big(q_w(t), q_x(t), q_y(t), q_z(t)\Big)\) and \(\mathbf{q}_{t+\Delta t}=\Big(q_w(t+\Delta t), q_x(t+\Delta t), q_y(t+\Delta t), q_z(t+\Delta t)\Big)\) as:

\[\begin{split}\begin{array}{rcl} \omega_x &=& \frac{2}{\Delta t}\Big(q_w(t) q_x(t+\Delta t) - q_x(t) q_w(t+\Delta t) - q_y(t) q_z(t+\Delta t) + q_z(t) q_y(t+\Delta t)\Big) \\ \\ \omega_y &=& \frac{2}{\Delta t}\Big(q_w(t) q_y(t+\Delta t) + q_x(t) q_z(t+\Delta t) - q_y(t) q_w(t+\Delta t) - q_z(t) q_x(t+\Delta t)\Big) \\ \\ \omega_z &=& \frac{2}{\Delta t}\Big(q_w(t) q_z(t+\Delta t) - q_x(t) q_y(t+\Delta t) + q_y(t) q_x(t+\Delta t) - q_z(t) q_w(t+\Delta t)\Big) \end{array}\end{split}\]

where \(\Delta t\) is the time step between consecutive quaternions [Gar22].

Parameters:

dt (float) – Time step, in seconds, between consecutive Quaternions.

Returns:

w – (N-1)-by-3 array with angular velocities in rad/s.

Return type:

numpy.ndarray

Examples

>>> qts = np.tile([1., -2., 3., -4], (5, 1))    # Five equal arrays
>>> v = np.random.randn(5, 4)*0.1               # Gaussian noise
>>> Q = QuaternionArray(qts + v)
>>> Q.view()
QuaternionArray([[ 0.18873724, -0.36700234,  0.57194646, -0.70891804],
                 [ 0.21652608, -0.37263592,  0.54594733, -0.71847091],
                 [ 0.19481676, -0.38515671,  0.54045061, -0.72222841],
                 [ 0.16899238, -0.3725492 ,  0.56720371, -0.71479271],
                 [ 0.17139691, -0.36373225,  0.5687926 , -0.71749351]])
>>> Q.angular_velocities(0.01)
array([[ 6.60605698, -4.25771063,  1.02663571],
       [-0.96002505,  0.61326399, -5.05901551],
       [-6.1661511 ,  5.08207   , -0.0169264 ],
       [ 0.55639671,  1.24262468,  1.37105397]])