Matlab data processing functions

Basic quaternion functions

qmt.qmult(q1, q2)

Quaternion multiplication.

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

At the moment, this function is mostly useful to test the Matlab integration of the qmt toolbox. In Python code, use the equivalent Python function qmt.qmult().

Parameters:

• q1 – Nx4 or 1x4 quaternion input array

• q2 – Nx4 or 1x4 quaternion input array

Returns:

• output: Nx4 quaternion output array

Heading correction functions

qmt.headingCorrection(gyr1, gyr2, quat1, quat2, t, joint, jointInfo, estSettings, debug)

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 Python function: qmt.headingCorrection().

Parameters:

• gyr1 – Nx3 vector of angular velocities in rad/s of first segment.

• gyr2 – Nx3 vector of angular velocities in rad/s of second segment.

• quat1 – Nx4 vector of orientation quaternions of first segment.

• quat2 – Nx4 vector of orientation quaternions of second segment.

• t – Nx1 vector of the equidistant sampled time signal

• joint – dofx3 array containing the axes of the joint.

• jointInfo –

  structure containing additional joint information. only needed for 3D-joints. Fields:

  • 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

• estSettings –

  Structure containing the needed settings for the estimation. Fields:

  • 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 downsample 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

  • tau_delta: time constant for the filter in seconds

  • tau_bias: 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_1d, euler_2d, icorr).

    • Constraints for 2D: (euler,euler_lin, gyro, combined).

    • Constraints for 3D: (default)

  • optimizer_steps: Number of Gauss-Newton steps during optimization.

Returns:

• quat2_corr: Corrected orientation of the second segment in the reference frame of the first segment

• delta: value of the estimated heading offset

• delta_filt: 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

Joint estimation functions

qmt.jointAxisEstHingeOlsson(acc1, acc2, gyr1, gyr2, estSettings)

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

Equivalent Python 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.

  • 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 estimate value. Default value: [0, 0, 0, 0].

  • wa: Accelerometer residual weight.

  • wg: Gyroscope residual weight

Returns:

• jhat1: 33x1 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.