Python data processing functions

Basic quaternion functions

Basic operations

`qmt.qmult`(q1, q2[, strict, debug, plot])	Quaternion multiplication.
`qmt.qinv`(q[, debug, plot])	Calculates the inverse/conjugate of unit quaternions.
`qmt.qrel`(q1, q2[, debug, plot])	Calculates relative quaternions, i.e., `out = qmult(inv(q1), q2)`.
`qmt.rotate`(q, v[, debug, plot])	Rotates vectors with the given quaternions.
`qmt.quatTransform`(qTrafo, q[, debug, plot])	Transforms a rotation quaternion with a given transformation quaternion, i.e., `qTrafoqinv(qTrafo)`,
`qmt.quatProject`(q, axis[, debug, plot])	Calculate quaternion projection along a given axis.

Conversion from/to other representations

`qmt.eulerAngles`(q[, axes, intrinsic, debug, ...])	Calculate Euler angles from quaternions.
`qmt.quatFromEulerAngles`(angles[, axes, ...])	Calculate quaternions from Euler angles.
`qmt.quatAngle`(q[, debug, plot])	Returns the quaternion rotation angle in the range -pi..pi.
`qmt.quatAxis`(q[, debug, plot])	Returns the quaternion rotation axis.
`qmt.quatFromAngleAxis`(angle, axis[, debug, plot])	Create quaternion that represents a rotation of a given angle around a given axis.
`qmt.quatToRotMat`(q[, debug, plot])	Converts quaternions to rotation matrices.
`qmt.quatFromRotMat`(R[, method, debug, plot])	Convert rotation matrices to quaternions.
`qmt.quatToRotVec`(quat[, debug, plot])	Converts quaternions to rotation vectors, i.e., vectors whose length is the rotation angle and whose direction is the rotation axis.
`qmt.quatFromRotVec`(rotvec[, debug, plot])	Converts rotation vectors, i.e., vectors whose length is the rotation angle and whose direction is the rotation axis, to quaternions.
`qmt.quatToGyrStrapdown`(q, rate[, debug, plot])	Generate simulated gyroscope measurements from a quaternion timeseries.
`qmt.quatFromGyrStrapdown`(gyr, rate[, debug, ...])	Create strap-down integrated quaternion timeseries from gyroscope measurements.
`qmt.quatFrom2Axes`([x, y, z, exactAxis, ...])	Determine quaternion from two given basis vectors.
`qmt.quatFromVectorObservations`(v, w[, ...])	Calculates quaternion from multiple (weighted) vector observations (Wahba's problem).
`qmt.headingInclinationAngle`(quat[, debug, plot])	Decompose quaternion into heading and inclination angle.

Interpolation

`qmt.slerp`(q0, q1, t[, debug, plot])	Spherical linear interpolation of quaternions.
`qmt.quatInterp`(quat, ind[, extend, debug, plot])	Interpolates quaternion timeseries at (non-integer) sampling times using slerp.

Utilities

`qmt.randomQuat`([N, angle, debug, plot])	Generate random quaternions.
`qmt.averageQuat`(quats[, debug, plot])	Calculates the average of multiple quaternions.
`qmt.quatUnwrap`(quat[, init, debug, plot])	Unwraps quaternion timeseries to prevent unnecessary jumps in plots.
`qmt.posScalar`(q[, debug, plot])	Returns a copy of the input quaternion that uniquely describes rotations by making the scalar part positive.
`qmt.deltaQuat`(delta[, debug, plot])	Generates rotation quaternions around the vertical z-axis.
`qmt.addHeading`(q, delta[, debug, plot])	Add heading angle offset to existing quaternions.

Orientation estimation functions

`qmt.oriEstVQF`(gyr, acc[, mag, params, ...])	VQF orientation estimation algorithm.
`qmt.oriEstBasicVQF`(gyr, acc[, mag, params, ...])	Basic version of the VQF orientation estimation algorithm (no rest detection, no gyroscope bias estimation, no magnetic disturbance rejection).
`qmt.oriEstOfflineVQF`(gyr, acc[, mag, ...])	Offline version of the VQF orientation estimation algorithm.
`qmt.oriEstMadgwick`(gyr, acc[, mag, params, ...])	Madgwick's orientation estimation algorithm.
`qmt.oriEstMahony`(gyr, acc[, mag, params, ...])	Mahony's orientation estimation algorithm.
`qmt.oriEstIMU`(gyr, acc[, mag, params, ...])	OriEstIMU orientation estimation algorithm.

Reset alignment + heading reset

`qmt.resetAlignment`(q, reset[, x, xCs, y, ...])	Align multiple quaternions using a reset instant, e.g., from sensor orientations to segment orientations.
`qmt.resetHeading`(q, reset[, base, ...])	Adjust the heading of a orientation quaternion so that the relative angle to the other input quaternion becomes as small as possible.

Heading correction functions

`qmt.headingCorrection`(gyr1, gyr2, quat1, ...)	This function corrects the heading of a kinematic chain of two segments whose orientations are estimated without the use of magnetometers.
`qmt.removeHeadingDriftForStraightWalk`(t, ...)	A function that takes a quaternion with slow heading drift and removes that heading drift by assumimg that the heading remains constant except for a few large steps (turns) by multiples of pi.

Joint estimation functions

qmt.jointAxisEstHingeOlsson(acc1, acc2, ...)

This function estimates the 1 DoF joint axes of a kinematic chain of two segments in the local coordinates of the sensors attached to each segment respectively.

Calibration functions

qmt.calibrateMagnetometerSimple(gyr, acc, ...)

Performs a simple magnetometer calibration.

Optical mocap functions

`qmt.syncOptImu`(opt_quat, opt_rate, imu_gyr, ...)	Determines time offsets between data recorded using opticial motion capture and IMU data.
`qmt.alignOptImu`(imu_gyr, imu_acc, imu_mag, ...)	Determines sensor-to-segment and imu-to-opt reference frame alignment quaternions.
`qmt.alignOptImuByMinimizingRmse`(imu_quat, ...)	Determines sensor-to-segment and imu-to-opt reference frame alignment quaternions that minimize the orientation difference.

Utility functions

Angle utilities

`qmt.wrapToPi`(angles[, debug, plot])	Wraps angles to interval -π.
`qmt.wrapTo2Pi`(angles[, debug, plot])	Wraps angles to interval 0...2π by adding/subtracting multiples of 2π.
`qmt.nanUnwrap`(angle[, debug, plot])	Unwraps a signal that might contain NaNs.
`qmt.angleBetween2Vecs`(vec1, vec2[, debug, plot])	Calculates angle between two 3D vectors in rad.

Vector utilities

`qmt.timeVec`([N, T, rate, Ts, debug, plot])	Creates a time vector with a fixed sampling rate based on two parameters.
`qmt.vecnorm`(vec[, debug, plot])	Calculates the norm along the last axis.
`qmt.normalized`(vec[, debug, plot])	Divides each vector (along the last axis) by its norm.
`qmt.allUnitNorm`(vec[, debug, plot])	Checks if all elements have unit norm.
`qmt.randomUnitVec`([N, M, debug, plot])	Generate random unit vectors.
`qmt.vecInterp`(vec, ind[, extend, debug, plot])	Interpolates an array at (non-integer) indices using linear interpolation.
`qmt.nanInterp`(signal[, quatdetect, debug, plot])	Fills NaN samples by linear interpolation or quaternion slerp based on the previous and next non-NaN sample.

Other utilities

qmt.rms(x[, axis, debug, plot])

Calculates root-mean-square (RMS) values.

Basic quaternion functions

Basic operations

qmt.qmult(q1, q2, strict=True, debug=False, plot=False)[source]

Quaternion multiplication.

If two Nx4 arrays are given, they are multiplied row-wise. Alternatively, one of the inputs can be a single quaternion which is then multiplied to all rows of the other input array:

>>> qmt.qmult([[1, 0, 0, 0], [0, 0, 0, 1]], [0.5, 0.5, 0.5, 0.5])
array([[ 0.5,  0.5,  0.5,  0.5],
       [-0.5, -0.5,  0.5,  0.5]])

If one quaternion is NaN, the corresponding output quaternion will be NaN. If strict is True and inputs are not unit quaternions, a ValueError will be raised. For advanced use: input arrays with more dimensions (and different shapes) are also supported, as long as they can be broadcast to a common shape and the last dimension has length 4.

Equivalent Matlab function: qmt.qmult().

For examples, see Basic quaternion functions.

Parameters:

q1 – quaternion input array, shape: (…, 4)
q2 – quaternion input array, shape: (…, 4)
strict – if set to true, an error is raised if q1 or q2 do not contain unit quaternions
debug – enables debug output
plot – enables debug plot

Returns:

output: quaternion output array, shape: (…, 4)
debug: dict with debug values (only if debug==True)

qmt.qinv(q, debug=False, plot=False)[source]

Calculates the inverse/conjugate of unit quaternions.

Parameters:

q – quaternion input array, shape (…, 4)
debug – enables debug output
plot – enables debug plot

Returns:

output: quaternion output array, (…, 4)
debug: dict with debug values (only if debug==True)

qmt.qrel(q1, q2, debug=False, plot=False)[source]

Calculates relative quaternions, i.e., out = qmult(inv(q1), q2).

With respect to broadcasting and NaN handling, the function works like qmt.qmult(). Inputs are always checked for unit norm.

Parameters:

q1 – quaternion input array, shape: (…, 4)
q2 – quaternion input array, shape: (…, 4)
debug – enables debug output
plot – enables debug plot

Returns:

output: quaternion output array, shape: (…, 4)
debug: dict with debug values (only if debug==True)

qmt.rotate(q, v, debug=False, plot=False)[source]

Rotates vectors with the given quaternions.

The rotated vector out is calculated as

[0, out] = qmult(q, qmult([0, v], qinv(q)))

If a Nx4 quaternion array and a Nx3 vector array are given, rotation is calculated row-wise. Alternatively, one of the inputs can be a single quaternion or single vector, which is then applied to all rows of the other array. More complicated input shapes, following the numpy broadcasting rules, are also supported.

The input quaternions are checked to have unit norm. If the inputs are NaN, the corresponding output will be NaN.

Parameters:

q – quaternion input array, shape (…, 4)
v – vector input array, shape (…, 3)
debug – enables debug output
plot – enables debug plot

Returns:

output: vector output array, (…, 3)
debug: dict with debug values (only if debug==True)

qmt.quatTransform(qTrafo, q, debug=False, plot=False)[source]

Transforms a rotation quaternion with a given transformation quaternion, i.e., qTrafo*q*inv(qTrafo),

>>> qmt.quatTransform(np.array([0, 0, 1, 0]), np.array([0, 1, 0, 0]))
array([ 0., -1.,  0.,  0.])

Parameters:

qTrafo – transformation quaternion input array, (…, 4)
q – rotation quaternion input array, (…, 4)

Returns:

out: quaternion output array, (…, 4)
debug: dict with debug values (only if debug==True)

qmt.quatProject(q, axis, debug=False, plot=False)[source]

Calculate quaternion projection along a given axis.

The quaternion projection is defined as the rotation around the specified axis that results in the smallest possible residual rotation. The axis of this residual rotation is always orthogonal to the given projection axis, which is similar to vector projection (cf. https://en.wikipedia.org/wiki/Vector_projection).

The output is a dictionary that contains the angles and quaternions for both the projection rotation and the residual rotation. Both rotations can be combined so that q = qmult(projQuat, resQuat).

>>> qmt.quatProject(np.array([[0.5, 0.5, 0.5, 0.5], [0, 0, 0, 1]]), np.array([1, 1, 0]))
    {'projAngle': array([1.91063324, 0.        ]),
     'resAngle': array([ 1.04719755, -3.14159265]),
     'projQuat': array([[0.57735027, 0.57735027, 0.57735027, 0.        ],
            [1.        , 0.        , 0.        , 0.        ]]),
     'resQuat': array([[ 0.8660254 , -0.28867513,  0.28867513,  0.28867513],
            [ 0.        ,  0.        ,  0.        ,  1.        ]])}

Parameters:

q – quaternion input array, (…, 4)
axis – projection axis vector, (…, 3)
debug – enables debug output
plot – enables debug plot

Returns:

outputs: dict with projAngle (…,), resAngle (…,), projQuat (…, 4), resQuat (…, 4)
debug: dict with debug values (only if debug==True)

Conversion from/to other representations

qmt.eulerAngles(q, axes='zyx', intrinsic=True, debug=False, plot=False)[source]

Calculate Euler angles from quaternions.

All possible rotation sequences are supported and can be specified with the axes parameter. Examples for valid values (that all have the same meaning) are ‘zxy’, ‘ZXY’, 312, ‘312’. By default, intrinsic angles are calculated. Set intrinsic to False to calculate extrinsic angles.

In the case of gimbal lock (second angle is 90° or -90° for Tait-Byran angles, or 0° or -180° for proper Euler angles), the last angle is set to zero. Note that this is an arbitrary choice, but the returned angles still represent the correct rotation.

Parameters:

q – input quaternion array, (…, 4)
axes – rotation sequence
intrinsic – calculate intrinsic angles if True, extrinsic if False

Returns:

out: output array with Euler angles in rad, (…, 3)
debug: dict with debug values (only if debug==True)

qmt.quatFromEulerAngles(angles, axes='zyx', intrinsic=True, debug=False, plot=False)[source]

Calculate quaternions from Euler angles.

All possible rotation sequences are supported and can be specified with the axes parameter. Examples for valid values (that all have the same meaning) are ‘zxy’, ‘ZXY’, 312, ‘312’. By default, intrinsic angles are used. Set intrinsic to False to use extrinsic angles.

Parameters:

angles – input Euler angle array, (…, 3)
axes – rotation sequence
intrinsic – calculate intrinsic angles if True, extrinsic if False

Returns:

out: output array with Euler angles in rad, (…, 3)
debug: dict with debug values (only if debug==True)

qmt.quatAngle(q, debug=False, plot=False)[source]

Returns the quaternion rotation angle in the range -pi..pi.

Because the output range of the angle is -pi..pi, roundtrip conversion in combination with qmt.quatAxis() and qmt.quatFromAngleAxis() can return -q instead of q, which expresses the same rotation. To uniquely compare the outputs, use qmt.posScalar().

>>> np.rad2deg(qmt.quatAngle(np.array([[1, 0, 0, 0], [0.5, 0.5, 0.5, 0.5]])))
    array([  0., 120.])

Parameters:

q – input quaternion array, (…, 4)
debug – enables debug output
plot – enables debug plot

Returns:

output: rotation angles in rad, shape (…,)
debug: dict with debug values (only if debug==True)

qmt.quatAxis(q, debug=False, plot=False)[source]

Returns the quaternion rotation axis.

For identity quaternions, the arbitrary axis [1 0 0] is returned.

>>> qmt.quatAxis(np.array([[1, 0, 0, 0], [0.5, 0.5, 0.5, 0.5]]))
    array([[1.        , 0.        , 0.        ],
           [0.57735027, 0.57735027, 0.57735027]])

Parameters:

q – input quaternion array, (…, 4)
debug – enables debug output
plot – enables debug plot

Returns:

output: rotation axis, (…, 3)
debug: dict with debug values (only if debug==True)

qmt.quatFromAngleAxis(angle, axis, debug=False, plot=False)[source]

Create quaternion that represents a rotation of a given angle around a given axis.

If the same number of angles and axes are given, the quaternion is calculated row-wise based on the corresponding entries. It is possible to only supply a single angle which is then combined with all axes and vice versa. Furthermore, higher-dimensional inputs, following the numpy broadcasting rules, are also supported.

If angle is 0, the output will be an identity quaternion. If axis is zero vector, a ValueError will be raised unless the corresponding angle is 0.

>>> qmt.quatFromAngleAxis([0, 1, 2], [1, 0, 0])
array([[1.        , 0.        , 0.        , 0.        ],
       [0.87758256, 0.47942554, 0.        , 0.        ],
       [0.54030231, 0.84147098, 0.        , 0.        ]])

Parameters:

angle – angle in rad, shape (…,)
axis – rotation axis vector, (…, 3)
debug – enables debug output
plot – enables debug plot

Returns:

output: quaternion output array (…, 4)
debug: dict with debug values (only if debug==True)

qmt.quatToRotMat(q, debug=False, plot=False)[source]

Converts quaternions to rotation matrices.

>>> qmt.quatToRotMat([[1, 0, 0, 0], [0, 1, 0, 0]])
array([[[ 1.  0.  0.],
        [ 0.  1.  0.],
        [ 0.  0.  1.]]
        [[ 1.  0.  0.],
        [ 0. -1.  0.],
        [ 0.  0. -1.]]])

Parameters:

q – quaternion input array, (…, 4)
debug – enables debug output
plot – enables debug plot

Returns:

R: rotation matrix output array, (…, 3, 3)
debug: dict with debug values (only if debug==True)

qmt.quatFromRotMat(R, method='auto', debug=False, plot=False)[source]

Convert rotation matrices to quaternions.

>>> qmt.quatFromRotMat([[1,0,0],[0,1,0],[0,0,1]])
array([1.  0.  0. 0.])

The input must be a rotation matrix with shape (3, 3) or multiple rotation matrices with shape (N, 3, 3). Higher-dimensional inputs are also supported as long as the last two axes have length 3.,

Parameters:

R – rotation matrix input array, (…, 3, 3)
method – method to use, must be ‘auto’ (default), ‘copysign’, 0, 1, 2 or 3
debug – enables debug output
plot – enables debug plot

Returns:

output: quaternion output array, (…, 4)
debug: dict with debug values (only if debug==True)

qmt.quatToRotVec(quat, debug=False, plot=False)[source]

Converts quaternions to rotation vectors, i.e., vectors whose length is the rotation angle and whose direction is the rotation axis.

Each input quaternion is converted independently. In order to determine a rotation vector that represents the change of orientation in a time series, use quatToGyrStrapdown() and set the sampling rate to 1 Hz.

Parameters:

quat – input quaternions (…, 4)
debug – enables debug output
plot – enables debug plot

Returns:

quat: output rotation vectors in rad,, (…, 3)
debug: dict with debug values (only if debug==True)

qmt.quatFromRotVec(rotvec, debug=False, plot=False)[source]

Converts rotation vectors, i.e., vectors whose length is the rotation angle and whose direction is the rotation axis, to quaternions.

Each input rotation vector is converted independently. In order to determine a quaternion that represents the change of orientation in a time series, use quatFromGyrStrapdown() and set the sampling rate to 1 Hz.

This function is just a convenient way to call quatFromAngleAxis(vecnorm(rotvec), rotvec).

Parameters:

rotvec – input rotation vectors in rad, (…, 3)
debug – enables debug output
plot – enables debug plot

Returns:

quat: output quaternion array, (…, 4)
debug: dict with debug values (only if debug==True)

qmt.quatToGyrStrapdown(q, rate, debug=False, plot=False)[source]

Generate simulated gyroscope measurements from a quaternion timeseries.

Each output angular rate sample represents the change of orientation between the current quaternion and the previous quaternion. Use quatToRotVec() and multiply with the sampling rate if you want to independently convert rotation quaternions to the corresponding gyroscope measurements.

>>> qmt.quatToGyrStrapdown(np.array([[0, 0, 1, 0], [0, 0, 0, 1]]), 0.2)
array([[ 0.          0.          0.        ],
      [-0.62831853  0.          0.        ]])

Parameters:

q – quaternion input array, (N, 4)
rate – sampling rate in Hz
debug – enables debug output
plot – enables debug plot

Returns:

gyr: angular rate output array in rad/s, (N, 3)
debug: dict with debug values (only if debug==True)

qmt.quatFromGyrStrapdown(gyr, rate, debug=False, plot=False)[source]

Create strap-down integrated quaternion timeseries from gyroscope measurements.

Starting with an initial orientation of [1 0 0 0], each input angular rate sample is converted to a rotation quaternion and multiplied to the previous orientation. Use quatFromRotVec() after dividing by the sampling rate if you want to independently convert gyroscope measurements to the corresponding rotation quaternions.

Note that, to avoid looping over the data in Python, this function is implemented in C++.

Parameters:

gyr – angular rate input array, (N, 3)
rate – sampling rate in Hz
debug – enables debug output
plot – enables debug plot

Returns:

out: quaternion output array in rad/s, (N, 4)
debug: dict with debug values (only if debug==True)

qmt.quatFrom2Axes(x=None, y=None, z=None, exactAxis=None, verbose=False, debug=False, plot=False)[source]

Determine quaternion from two given basis vectors.

The orientation of CS A relative to CS B is determined from 2 basis vectors of CS A in B’s coordinates. The axes x, y and z are 3-dimensional vectors and one of them has to be None or [0, 0, 0]. exactAxis is None, ‘x’, ‘y’ or ‘z’ and describes which of the two non-zero axes is assumed to be exact. If None is passed, both axes will be adjusted equally.

>>> q = qmt.quatFrom2Axes(x=[1, 1, 0], y=[1, 0, 1])
array([0.44403692, 0.7690945 , 0.44403692, 0.11897933])

Parameters:

x – x-axis input array, None or (N, 3) or (3,)
y – y-axis input array, None or (N, 3) or (3,)
z – z-axis input array, None or (N, 3) or (3,)
exactAxis – describes which axis should be adjusted if they are non-orthogonal; options: None, ‘x’, ‘y’, ‘z’
verbose – enables printing of a non-orthogonality warning if the angle between the axes is < 30°
debug – enables debug output
plot – enables debug plot

Returns:

output: quaternion output array, (N, 4) or (4,)
debug: dict with debug values (only if debug==True)

qmt.quatFromVectorObservations(v, w, weights=None, debug=False, plot=False)[source]

Calculates quaternion from multiple (weighted) vector observations (Wahba’s problem).

The output is a quaternion that (tries to) satisfy qmt.rotate(quat, v) ≈ w. Note that the input vectors v and w will be normalized internally.

This function uses the SVD implementation as described in Markley, F. L. Attitude Determination using Vector Observations and the Singular Value Decomposition, Journal of the Astronautical Sciences, 1988, 38:245–258 (cf. https://en.wikipedia.org/wiki/Wahba%27s_problem).

Parameters:

v – input vectors v, shape (N, 3)
w – input vectors w, shape (N, 3)
weights – optional weight vector, shape (N,)
debug – enables debug output
plot – enables debug plot

Returns:

quat: output quaternion, shape (4,)
debug: dict with debug values (only if debug==True)

qmt.headingInclinationAngle(quat, debug=False, plot=False)[source]

Decompose quaternion into heading and inclination angle.

The heading angle describes a rotation around the vertical z-axis, and the inclination angle describes a rotation around a horizontal axis. Note that this decomposition is not reversible (a third angle that describes the horizontal axis is missing).

The result of this function can also be obtained via qmt.quatProject(quat, [0, 0, 1]): heading is equal to projAngle and inclination is equal to abs(resAngle).

Parameters:

quat – input quaternion array, (…, 4)
debug – enables debug output
plot – enables debug plot

Returns:

heading: heading angle in rad, shape (…,)
inclination: inclination angle in rad, shape (…,)
debug: dict with debug values (only if debug==True)

Interpolation

qmt.slerp(q0, q1, t, debug=False, plot=False)[source]

Spherical linear interpolation of quaternions.

This function interpolates between q0 and q1 at an interpolation parameter t that is between 0 (for q0) and 1 (for q1). Both q0 and q1 can either be a single quaternion or an array of multiple quaternions, and t can be scalar or an array with matching shape (numpy broadcasting rules are applied).

This function calculates the shortest path interpolation. If q0 and q1 are very similar, a fallback to linear interpolation (lerp) is implemented for numeric reasons.

See quatInterp() for an alternative function to use slerp that is useful for interpolating time series of quaternions at different sampling times.

Parameters:

q0 – first input quaternion (t=0), numpy array with shape (…, 4)
q1 – second input quaternion (t=1), numpy array with shape (…, 4)
t – interpolation factor (between 0 and 1), scalar or (…) numpy array
debug – enables debug output
plot – enables debug plot

Returns:

out: interpolated quaternion, shape (…, 4)
debug: dict with debug values (only if debug==True)

qmt.quatInterp(quat, ind, extend=True, debug=False, plot=False)[source]

Interpolates quaternion timeseries at (non-integer) sampling times using slerp.

Sampling indices are in the range 0..N-1. For values outside of this range, depending on “extend”, the first/last element or NaN is returned. If the input consists of 2 quaternions and ind=0.5, the result is the 50/50 interpolation between the two input quaternions.

See also qmt.vecInterp() for linear interpolation and slerp() for a more generic slerp implementation.

>>> qmt.quatInterp([[1, 0, 0, 0], [0, 0, 1, 0]], [0, 0.1, 0.5, 1])
array([[1.        , 0.        , 0.        , 0.        ],
   [0.98768834, 0.        , 0.15643447, 0.        ],
   [0.70710678, 0.        , 0.70710678, 0.        ],
   [0.        , 0.        , 1.        , 0.        ]])

Parameters:

quat – array of input quaternions (N, 4)
ind – vector containing the sampling indices for desired output, (M,)
extend – if True, the input data is virtually extended by the first/last value
debug – enables debug output
plot – enables debug plot

Returns:

out: interpolated quaternion output array, (M, 4)
debug: dict with debug values (only if debug==True)

Utilities

qmt.randomQuat(N=None, angle=None, debug=False, plot=False)[source]

Generate random quaternions.

If N is None (default), a single random quaternion is returned as an array with shape (4,). If N is an integer, N random quaternions are returned as an (N, 4) array. If N is a tuple, it denotes the shape of the output quaternions, e.g. N=(5, 20) returns 100 random quaternions as a (5, 20, 4) array.

If the optional parameter angle is not None, only the axis is randomly generated and quaternions with the random axis and the given angle are returned.

>>> qmt.randomQuat(N=2)
    array([[ 0.07232174  0.37107947 -0.27374854 -0.88438189]
          [ 0.35838808  0.25856014 -0.07512213  0.89390229]])

Orientation estimation functions

qmt.oriEstVQF(gyr, acc, mag=None, params=None, debug=False, plot=False)[source]

VQF orientation estimation algorithm.

See https://github.com/dlaidig/vqf, https://doi.org/10.1016/j.inffus.2022.10.014, and https://arxiv.org/abs/2203.17024 for more information about this algorithm.

If potential real-time capability is not needed, use the offline version qmt.oriEstOfflineVQF() for improved accuray. This algorithm is also available as an online data processing block: qmt.OriEstVQFBlock.

Parameters:

gyr – Nx3 array with gyroscope measurements [rad/s]
acc – Nx3 array with accelerometer measurements [m/s^2]
mag – Nx3 array with magnetometer measurements or None [any unit]
params – A dictionary of parameters for orientation estimation. Ts is mandatory and specifies the sample time of measurement data in seconds. All other parameters are optional and the default values should be sufficient for most applications. See https://vqf.readthedocs.io/ for the documentation of the all possible parameters.
debug – enables debug output
plot – enables debug plot

Returns:

quat: Nx4 quaternion output array
debug: dict with debug values (only if debug==True)

qmt.oriEstBasicVQF(gyr, acc, mag=None, params=None, debug=False, plot=False)[source]

Basic version of the VQF orientation estimation algorithm (no rest detection, no gyroscope bias estimation, no magnetic disturbance rejection).

See https://github.com/dlaidig/vqf, https://doi.org/10.1016/j.inffus.2022.10.014, and https://arxiv.org/abs/2203.17024 for more information about this algorithm.

Parameters:

gyr – Nx3 array with gyroscope measurements [rad/s]
acc – Nx3 array with accelerometer measurements [m/s^2]
mag – Nx3 array with magnetometer measurements or None [any unit]
params – A dictionary of parameters for orientation estimation. Ts is mandatory and specifies the sample time of measurement data in seconds. The other two parameters are tauAcc and tauMag, see https://vqf.readthedocs.io/ for more information.
debug – enables debug output
plot – enables debug plot

Returns:

quat: Nx4 quaternion output array
debug: dict with debug values (only if debug==True)

qmt.oriEstOfflineVQF(gyr, acc, mag=None, params=None, debug=False, plot=False)[source]

Offline version of the VQF orientation estimation algorithm.

See https://github.com/dlaidig/vqf, https://doi.org/10.1016/j.inffus.2022.10.014, and https://arxiv.org/abs/2203.17024 for more information about this algorithm.

Parameters:

gyr – Nx3 array with gyroscope measurements [rad/s]
acc – Nx3 array with accelerometer measurements [m/s^2]
mag – Nx3 array with magnetometer measurements or None [any unit]
params – A dictionary of parameters for orientation estimation. Ts is mandatory and specifies the sample time of measurement data in seconds. All other parameters are optional and the default values should be sufficient for most applications. See https://vqf.readthedocs.io/ for the documentation of the all possible parameters.
debug – enables debug output
plot – enables debug plot

Returns:

quat: Nx4 quaternion output array
debug: dict with debug values (only if debug==True)

qmt.oriEstMadgwick(gyr, acc, mag=None, params=None, debug=False, plot=False)[source]

Madgwick’s orientation estimation algorithm.

See https://doi.org/10.1109/ICORR.2011.5975346 for more information about this algorithm. Based on the C++ implementation by Sebastian Madgwick, available at https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/.

This algorithm is also available as an online data processing block: qmt.OriEstMadgwickBlock.

Parameters:

gyr – Nx3 array with gyroscope measurements [rad/s]
acc – Nx3 array with accelerometer measurements [m/s^2]
mag – Nx3 array with magnetometer measurements or None [any unit]
params –
A dictionary of parameters for orientation estimation. Ts is mandatory, all other values are optional. Possible values:
- Ts: sample time of measurement data in seconds
- beta: algorithm gain
- initQuat: 1x4 or (4,) array, quaternion of initial orientation
debug – enables debug output
plot – enables debug plot

Returns:

quat: Nx4 quaternion output array
debug: dict with debug values (only if debug==True)

qmt.oriEstMahony(gyr, acc, mag=None, params=None, debug=False, plot=False)[source]

Mahony’s orientation estimation algorithm.

See https://dx.doi.org/10.1109/TAC.2008.923738 for more information about this algorithm. Based on the C++ implementation by Sebastian Madgwick, available at https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/.

This algorithm is also available as an online data processing block: qmt.OriEstMahonyBlock.

Parameters:

gyr – Nx3 array with gyroscope measurements [rad/s]
acc – Nx3 array with accelerometer measurements [m/s^2]
mag – Nx3 array with magnetometer measurements or None [any unit]
params –
A dictionary of parameters for orientation estimation. Ts is mandatory, all other values are optional. Possible values:
- Ts: sample time of measurement data in seconds
- Kp: proportional gain
- Ki: integral gain (for gyroscope bias estimation)
- initQuat: 1x4 or (4,) array, quaternion of initial orientation
- initBias: 1x3 or (3,) array, estimated bias of data
debug – enables debug output
plot – enables debug plot

Returns:

quat: Nx4 quaternion output array
debug: dict with debug values (only if debug==True)

qmt.oriEstIMU(gyr, acc, mag=None, params=None, debug=False, plot=False)[source]

OriEstIMU orientation estimation algorithm.

See https://dx.doi.org/10.1016/j.ifacol.2017.08.1534 for more information about this algorithm.

This algorithm is also available as an online data processing block: qmt.OriEstIMUBlock.

Parameters:

gyr – Nx3 array with gyroscope measurements [rad/s]
acc – Nx3 array with accelerometer measurements [m/s^2]
mag – Nx3 array with magnetometer measurements or None [any unit]
params –
A dictionary of parameters for orientation estimation. Ts is mandatory, all other values are optional. Possible values:
- Ts: sample time of measurement data in seconds
- tauAcc: time constants for acc correction (50% time) [must be >0], [in seconds]
- tauMag: time constants for mag correction (50% time) [must be >0], [in seconds]
- zeta: bias estimation strength [no unit]
- accRating: enables raw rating of accelerometer, set to 0 to disable
- initQuat: 1x4 or (4,) array, quaternion of initial orientation
- initBias: 1x3 or (3,) array, estimated bias of data
debug – enables debug output
plot – enables debug plot

Returns:

quat: Nx4 quaternion output array
debug: dict with debug values (only if debug==True)

Reset alignment + heading reset

qmt.resetAlignment(q, reset, x=[1, 0, 0], xCs=0, y=[0, 1, 0], yCs=0, z=[0, 0, 0], zCs=0, exactAxis=None, debug=False, plot=False)[source]

Align multiple quaternions using a reset instant, e.g., from sensor orientations to segment orientations.

A prerequisite for using this method is that during the reset, all M target (segment) orientations are aligned (and that the quaternion have the correct heading). It is then only necessary to know two different axes of the target coordinate system, either in one of the local coordinate systems of any of the M sensors or in the global coordinate system (i.e., if the common target coordinate system has a vertical axis during reset).

If reset is set to 1, the all segment CSs are assumed to be aligned and new relative quaternions are calculated and stored. For values other than 1, the last stored value are applied. Two axes of the segment CS must be given in any sensor CS or in global CS. One of the 2 given axes is assumed to be exact and the second axis is transformed to be orthogonal to the exact axis. Alternatively, both axes can be adjusted equally. One of the three axes has to be [0 0 0]. The parameters xCs, yCs, zCs describe if the provided axes are given in any of the sensor CS (from 0 to M-1) or in global (-1) coordinates.

Parameters:

q – (M, N, 4) quaternion input array, M is the number of sensors, N is the number of samples
reset – (N,) boolean reset input array
x – (N, 3) or (3,) x-axis coordinates
xCs – coordinate system for x, valid values: -1..M-1
y – (N, 3) or (3,) y-axis coordinates
yCs – coordinate system for y, valid values: -1..M-1
z – (N, 3) or (3,) z-axis coordinates
zCs – coordinate system for z, valid values: -1..M-1
exactAxis – axis that is assumed to be exact (‘x’, ‘y’, ‘z’, or None to adjust both equally)
debug – enables debug output
plot – enables debug plot

Returns:

output: (M, N, 4) quaternion output array
debug: dict with debug values (only if debug==True)

qmt.resetHeading(q, reset, base=0, deltaOffset=0, debug=False, plot=False)[source]

Adjust the heading of a orientation quaternion so that the relative angle to the other input quaternion becomes as small as possible.

If reset is set to 1, the input quaternions are assumed to be aligned and the heading difference is calculated and stored. For values other than 1, the last stored value are applied. The heading of the base quaternion is assumed to be correct and therefore the other quaternion will be adjusted by the calculated heading difference. In addition, all quaternions are adjusted by the input angle deltaOffset (heading offset).

Parameters:

q – (M, N, 4) quaternion input array, M is the number of sensors, N is the number of samples
reset – (N,) boolean reset input array
deltaOffset – heading offset array in rad, (N,) or scalar
base – index of the quaternion that is not adjusted, valid value is a natural number from 0 to M-1
debug – enables debug output
plot – enables debug plot

Returns:

output: (M, N, 4) quaternion output array
debug: dict with debug values (only if debug==True)

Heading correction functions

qmt.headingCorrection(gyr1, gyr2, quat1, quat2, t, joint, jointInfo, estSettings=None, verbose=False, debug=False, plot=False)[source]

This function corrects the heading of a kinematic chain of two segments whose orientations are estimated without the use of magnetometers. It corrects the heading of the second segment in a way that its orientation is estimated in the reference of the first segment. It uses kinematic constraints for rotational joints to estimate the heading offset of both segments based on the limited set of possible relative orientations induced by the rotational joint. There are methods for 1D, 2D and 3D joints, based on different constraints.

Equivalent Matlab function: qmt.headingCorrection().

Parameters:

gyr1 – Nx3 array of angular velocities in rad/s
gyr2 – Nx3 array of angular velocities in rad/s
quat1 – Nx4 array of orientation quaternions
quat2 – Nx4 array of orientation quaternions
t – Nx1 vector of the equidistant sampled time signal
joint – Dofx3 array containing the axes of the joint.
jointInfo –
Dictionary containing additional joint information. Only needed for 3D-joints. Keys:
- convention: String of the chosen euler angles convention, e.g. ‘xyz’
- angle_ranges: 3x2 array with the minimum and maximum value for each joint angle in radians
verbose – show all logs in function. Disabled by Default.
estSettings –
structure containing the needed settings for the estimation:
- window_time: width of the window over which the estimation should be performed in seconds
- estimation_rate: rate in Hz at which the estimation should be performed. typically below 1Hz is sufficient
- data_rate: rate at which new data is fed into the estimation. Is used to down sample the data for a faster estimation. Typically values around 5Hz are sufficient
- alignment: String of the chosen type of alignment of the estimation window. It describes how the samples around the current sample at which the estimation takes place are chosen. Possible values: ‘backward’, ‘center’, ‘forward’
- enable_stillness: boolean value if the stillness detection should be enabled
- stillness_time: time in seconds which should pass for the stillness detection to be triggered
- stillness_threshold: threshold for the angular velocity under which the body is assumed to be at rest. In rad/s
- tauDelta: time constant for the filter in seconds
- tauBias: time constant for the filter in seconds
- delta_range: array of values that are supposed to be tested for the method for 3D joints.
- constraint: type of contraint used for the estimation.
  - Constraints for 1D: (proj, euler, euler_2d, 1d_corr).
  - Constraints for 2D: (euler, gyro, combined).
  - Constraints for 3D: (default)
- optimizer_steps: Number of Gauss-Newton steps during optimization.
debug – enables debug output
plot – enables debug plot

Returns:

quat2Corr: Corrected orientation of the second segment in the reference frame of the first segment
delta: value of the estimated heading offset
deltaFilt: value of the filtered heading offset
rating: value of the certainty of the estimation
state: state of the estimation. 1: regular, 2: startup, 3: stillness
debug: dict with debug values (only if debug==True)

qmt.removeHeadingDriftForStraightWalk(t, quat, winlength, stepthreshold, movmeanwidth, plot=False, debug=False)[source]

A function that takes a quaternion with slow heading drift and removes that heading drift by assumimg that the heading remains constant except for a few large steps (turns) by multiples of pi. The function removes the heading steps, then low-pass filters heading and uses it to correct the given quaternion.

Parameters:

t – time vector with a fixed sampling rate
quat – Nx4 input quaternion array
winlength – winlength is the time in which the turns happen (choose e.g. 5 sec).
stepthreshold – stepthreshold defines which heading steps are ignored (not removed).
movmeanwidth – movmeanwidth is the width of the moving average low-pass filter.
plot – enables debug plot
debug – enables debug output

Returns:

output: Nx3 or 1x3 vector output array
debug: dict with debug values (only if debug==True)

Joint estimation functions

qmt.jointAxisEstHingeOlsson(acc1, acc2, gyr1, gyr2, estSettings=None, debug=False, plot=False)[source]

This function estimates the 1 DoF joint axes of a kinematic chain of two segments in the local coordinates of the sensors attached to each segment respectively.

Ported to Python based on https://www.mdpi.com/1424-8220/20/12/3534 and the Matlab implementation from Fredrik Olsson available at https://github.com/frols88/sensor-to-segment.

Equivalent Matlab function: qmt.jointAxisEstHingeOlsson().

Parameters:

acc1 – Nx3 array of angular velocities in m/s^2
acc2 – Nx3 array of angular velocities in m/s^2
gyr1 – Nx3 array of angular velocities in rad/s
gyr2 – Nx3 array of angular velocities in rad/s
estSettings –
Dictionary containing settings for estimation. If no settings are given, the default settings will be used. Available options:
- w0: Parameter that sets the relative weighting of gyroscope to accelerometer residual, w0 = wg/wa. Default value: 50.
- wa: Accelerometer residual weight.
- wg: Gyroscope residual weight
- useSampleSelection: Boolean flag to use sample selection. Default value: False.
- dataSize: Maximum number of samples that will be kept after sample selection. Default value: 1000.
- winSize: Window size for computing the average angular rate energy, should be an odd integer. Default value: 21.
- angRateEnergyThreshold: Theshold for the angular rate energy. Default value: 1.
- x0: Initial state of joint axes in 2 sensors coordinates. Default value: [0, 0, 0, 0] in spherical coordinate.
debug – enables debug output
plot – enables debug plot

Returns:

jhat1: 3x1 array, estimated joint axis in imu1 frame in cartesian coordinate.
jhat2: 3x1 array, estimated joint axis in imu2 frame in cartesian coordinate.
debug: dict with debug values (only if debug==True).

Calibration functions

qmt.calibrateMagnetometerSimple(gyr, acc, mag, Ts, targetNorm=49.8, debug=False, plot=False)[source]

Performs a simple magnetometer calibration.

The input data should consist of slow rotations in arbitrary directions without any translation and far away from potential magnetic disturbances. Calibration is performed by optimizing a cost function that tries to ensure that the resulting magnetometer measurements have constant norm and a constant dip angle.

To apply the calibration parameters:

mag_calibrated = gain * mag - bias

Parameters:

gyr – Nx3 array with gyroscope measurements [rad/s]
acc – Nx3 array with accelerometer measurements [m/s^2]
mag – Nx3 array with magnetometer measurements or None [any unit]
Ts – sampling time in seconds
targetNorm – norm of the magnetic field in the calibration[µT]
debug – enables debug output
plot – enables debug plot

Returns:

gain: scaling factor, array with shape (3,)
bias: offset, array with shape (3,)
success: returns False if the data indicates that calibration failed
debugData: debug: dict with debug values (only if debug==True)

Optical mocap functions

qmt.syncOptImu(opt_quat, opt_rate, imu_gyr, imu_rate, params, debug=False, plot=False)[source]

Determines time offsets between data recorded using opticial motion capture and IMU data. The offsets are determined based on angular rates. For the optical data, the angular rates are derived from a quaternion. Two synchronization steps are performed:

First, a fixed offset between the data is determined via cross correlation (imu_offsetonly).
Based on the result of this, both a time offset and an adjusted IMU sampling rate is adjusted to account for clock drift. This is done by minimizing the RMSE of the gyr norms.

See Appendix B of https://doi.org/10.3390/data6070072 for more information about this algorithm.

Parameters:

opt_quat – orientation quaternion from optical mocap system, Nx4
opt_rate – sampling rate of opt_quat in Hz
imu_gyr – gyroscope measurements from IMU in rad/s, Nx3
imu_rate – sampling rate of imu_gyr in Hz
params – optional parameters, defaults: syncRate=1000.0, cut=0.15, fc=10.0, correlate=’rmse’, fast=False
debug – enables debug return value
plot – enables debug plots

Returns:

syncInfo dictionary that can be used by qmt.SyncMapper

qmt.alignOptImu(imu_gyr, imu_acc, imu_mag, opt_quat, opt_pos, rate, params=None, names=None, debug=False, plot=False)[source]

Determines sensor-to-segment and imu-to-opt reference frame alignment quaternions.

See Appendix C of https://doi.org/10.3390/data6070072 for more information about (a subset of) this algorithm.

Parameters:

imu_gyr – gyroscope measurements in rad/s
imu_acc – accelerometer measurements in m/s²
imu_mag – magnetometer measurements
opt_quat – orientation from optical motion capture
opt_pos – position from optical motion capture
rate – sampling rate in seconds
params – additional parameters, defaults: fullEOpt2EImu=True, accbias=True, r=0.5, fc_gyr=10.0, fc_grav=0.1, fc_acc=10.0, fc_pos=10.0, fc_mag=1.0, fast=False, init_quats=None, verbose=False
names – names for different segments
debug – enables debug output
plot – enables debug plot

Returns:

alignment: dict with alignment information
debug: dict with debug values (only if debug==True)

qmt.alignOptImuByMinimizingRmse(imu_quat, opt_quat, params=None, debug=False, plot=False)[source]

Determines sensor-to-segment and imu-to-opt reference frame alignment quaternions that minimize the orientation difference.

Warning: Use this function with care because systematic orientation estimation errors can easily be optimized away. The results are probably only trustworthy if the input orientations are quite accurate (slow movements, calibrated IMU, no magnetic disturbances, good time synchronization, …) and if the performed movement contains a large variety of different orientations.

Parameters:

imu_quat – IMU quaternion
opt_quat – quaternion from optical motion capture
params – optional parameters to control algorithm behavior, defaults: qImu2Seg=True, qEOpt2EImu=True, delta=True, initQuats=None, verbose=False
debug – enables debug output
plot – enables debug plot

Returns:

alignent: dict with alignment quaternions
debug: dict with debug values (only if debug==True)

Utility functions

Angle utilities

qmt.wrapToPi(angles, debug=False, plot=False)[source]

Wraps angles to interval -π… π by adding/subtracting multiples of 2π.

Parameters:

angles – input angles in rad, numpy array or scalar
debug – enables debug output
plot – enables debug plot

Returns:

output: output angles with same shape as input angles
debugData: debug: dict with debug values (only if debug==True)

qmt.wrapTo2Pi(angles, debug=False, plot=False)[source]

Wraps angles to interval 0…2π by adding/subtracting multiples of 2π.

Parameters:

angles – input angles in rad, numpy array or scalar
debug – enables debug output
plot – enables debug plot

Returns:

output: output angles with same shape as input angles
debugData: debug: dict with debug values (only if debug==True)

qmt.nanUnwrap(angle, debug=False, plot=False)[source]

Unwraps a signal that might contain NaNs. The angle is also shifted so that the mean is in the range -pi…pi.

The input can be a 1D or 2D array. Unwrapping is performed along the first axis. In the 2D case, it is expected that all angles in the same row will be NaN at the same time.

Parameters:

angle – input angle signal in rad, shape (N, 3) or (N,)
debug – enables debug output
plot – enables debug plot

Returns:

output: unwrapped output angles with same shape as input angles
debugData: debug: dict with debug values (only if debug==True)

qmt.angleBetween2Vecs(vec1, vec2, debug=False, plot=False)[source]

Calculates angle between two 3D vectors in rad.

Multiples vectors can be passed with an array with the last dimension having length 3, and numpy broadcasting is supported.

>>> qmt.angleBetween2Vecs([1, 0, 0], [0, 1, 0])
angle = array([1.57079633])

Parameters:

vec1 – first input vector, (…, 3)
vec2 – second input vector, (…, 3)
debug – enables debug output
plot – enables debug plot

Returns:

angle: angle output array, shape (…,)
debug: dict with debug values (only if debug==True)

Vector utilities

qmt.timeVec(N=None, T=None, rate=None, Ts=None, debug=False, plot=False)[source]

Creates a time vector with a fixed sampling rate based on two parameters.

Valid combinations of parameters:

N and rate
N and Ts
T and rate
T and Ts
N and T

>>> qmt.timeVec(N=10, rate=100)
array([0.  , 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09])
>>> qmt.timeVec(N=10, Ts=0.01)
array([0.  , 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09])
>>> qmt.timeVec(T=0.05, Ts=0.01)
array([0.  , 0.01, 0.02, 0.03, 0.04, 0.05])

Parameters:

N – number of samples
T – end time (the start time is always 0)
rate – sampling rate
Ts – sampling time, i.e. 1/rate
debug – enables debug output
plot – enables debug plot

Returns:

t: time vector as (N,) array
debug: dict with debug values (only if debug==True)

qmt.vecnorm(vec, debug=False, plot=False)[source]

Calculates the norm along the last axis.

Parameters:

vec – input array, shape (…, M)
debug – enables debug output
plot – enables debug plot

Returns:

norm: norm output array, shape (…,)
debug: dict with debug values (only if debug==True)

qmt.normalized(vec, debug=False, plot=False)[source]

Divides each vector (along the last axis) by its norm.

>>> qmt.normalized([1, 1, 1])
v_norm = array([0.57735027, 0.57735027, 0.57735027])

Parameters:

vec – input array, shape (…, M)
debug – enables debug output
plot – enables debug plot

Returns:

out: normalized output array, shape (…, M)
debug: dict with debug values (only if debug==True)

qmt.allUnitNorm(vec, debug=False, plot=False)[source]

Checks if all elements have unit norm.

Calculation of the norm is performed along the last axis (see qmt.vecnorm()) and True is returned if all entries have a norm that is close to one. Entries with NaNs are ignored.

Parameters:

vec – input vector or quaternion array
debug – enables debug output
plot – enables debug plot

Returns:

output: True if all elements have unit norm, else False
debug: dict with debug values (only if debug==True)

qmt.randomUnitVec(N=None, M=3, debug=False, plot=False)[source]

Generate random unit vectors.

M denotes the number of elements in the vector (default: 3).

If N is None (default), a single random quaternion is returned as an array with shape (M,). If N is an integer, N random quaternions are returned as an (N, M) array. If N is a tuple, it denotes the shape of the output vectors, e.g. N=(5, 20) returns 100 random vectors as a (5, 20, 3) array.

Other utilities

qmt.rms(x, axis=0, debug=False, plot=False)[source]

Calculates root-mean-square (RMS) values.

For a 1D array, the RMS over all elements is calculated. For arrays with more dimensions, the RMS is calculated along a specified axis (default: 0). NaNs in input array will be ignored.

Parameters:

x – input array
axis – axis along which the means are computed
debug – enables debug output
plot – enables debug plot

Returns:

out: root mean square
debug: dict with debug values (only if debug==True)