BasicVQF Class

class BasicVQF

A Versatile Quaternion-based Filter for IMU Orientation Estimation.

This class implements the basic version of the orientation estimation filter described in the following publication:

D. Laidig and T. Seel. “VQF: Highly Accurate IMU Orientation Estimation with Bias Estimation and Magnetic Disturbance Rejection.” Information Fusion 2023, 91, 187–204. doi:10.1016/j.inffus.2022.10.014. [Accepted manuscript available at arXiv:2203.17024.]

The filter can perform simultaneous 6D (magnetometer-free) and 9D (gyr+acc+mag) sensor fusion and can also be used without magnetometer data. Different sampling rates for gyroscopes, accelerometers and magnetometers are supported as well. While in most cases, the defaults will be reasonable, the algorithm can be influenced via two tuning parameters.

To use this C++ implementation,

1. create a instance of the class and provide the sampling time and, optionally, parameters

2. for every sample, call one of the update functions to feed the algorithm with IMU data

3. access the estimation results with getQuat6D(), getQuat9D() and the other getter methods.

If the full data is available in (row-major) data buffers, you can use updateBatch().

This class is the C++ implementation of the basic algorithm version. This version does not include rest detection, gyroscope bias estimation, and magnetic disturbance detection and rejection. It is equivalent to the full version when the parameters VQFParams::motionBiasEstEnabled, VQFParams::restBiasEstEnabled and VQFParams::magDistRejectionEnabled are set to false. Depending on use case and programming language of choice, the following alternatives might be useful:

	Full Version	Basic Version	Offline Version
C++	VQF	BasicVQF (this class)	offlineVQF()
Python/C++ (fast)	vqf.VQF	vqf.BasicVQF	vqf.offlineVQF()
Pure Python (slow)	vqf.PyVQF	–	–
Pure Matlab (slow)	VQF.m	–	–

Public Functions

BasicVQF(vqf_real_t gyrTs, vqf_real_t accTs = -1.0, vqf_real_t magTs = -1.0)

Initializes the object with default parameters.

In the most common case (using the default parameters and all data being sampled with the same frequency, create the class like this:

BasicVQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz

Parameters:

• gyrTs – sampling time of the gyroscope measurements in seconds

• accTs – sampling time of the accelerometer measurements in seconds (the value of gyrTs is used if set to -1)

• magTs – sampling time of the magnetometer measurements in seconds (the value of gyrTs is used if set to -1)

BasicVQF(const BasicVQFParams &params, vqf_real_t gyrTs, vqf_real_t accTs = -1.0, vqf_real_t magTs = -1.0)

Initializes the object with custom parameters.

Example code to create an object with a different value for tauAcc:

BasicVQFParams params;
params.tauAcc = 1.0;
BasicVQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz

Parameters:

• params – BasicVQFParams struct containing the desired parameters

• gyrTs – sampling time of the gyroscope measurements in seconds

• accTs – sampling time of the accelerometer measurements in seconds (the value of gyrTs is used if set to -1)

• magTs – sampling time of the magnetometer measurements in seconds (the value of gyrTs is used if set to -1)

void updateGyr(const vqf_real_t gyr[3])

Performs gyroscope update step.

It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have different sampling rates. Otherwise, simply use update().

Parameters:

gyr – gyroscope measurement in rad/s

void updateAcc(const vqf_real_t acc[3])

Performs accelerometer update step.

It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have different sampling rates. Otherwise, simply use update().

Should be called after updateGyr and before updateMag.

Parameters:

acc – accelerometer measurement in m/s²

void updateMag(const vqf_real_t mag[3])

Performs magnetometer update step.

It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have different sampling rates. Otherwise, simply use update().

Should be called after updateAcc.

Parameters:

mag – magnetometer measurement in arbitrary units

void update(const vqf_real_t gyr[3], const vqf_real_t acc[3])

Performs filter update step for one sample (magnetometer-free).

Parameters:

• gyr – gyroscope measurement in rad/s

• acc – accelerometer measurement in m/s²

void update(const vqf_real_t gyr[3], const vqf_real_t acc[3], const vqf_real_t mag[3])

Performs filter update step for one sample (with magnetometer measurement).

Parameters:

• gyr – gyroscope measurement in rad/s

• acc – accelerometer measurement in m/s²

• mag – magnetometer measurement in arbitrary units

void updateBatch(const vqf_real_t gyr[], const vqf_real_t acc[], const vqf_real_t mag[], size_t N, vqf_real_t out6D[], vqf_real_t out9D[], vqf_real_t outDelta[])

Performs batch update for multiple samples at once.

In order to use this function, the input data must be available in an array in row-major order. A null pointer can be passed for mag in order to skip the magnetometer update. All data must have the same sampling rate.

The output pointer arguments must be null pointers or point to sufficiently large data buffers.

Example usage:

int N = 1000;
double gyr[N*3]; // fill with gyroscope measurements in rad/s
double acc[N*3]; // fill will accelerometer measurements in m/s²
double mag[N*3]; // fill with magnetometer measurements (arbitrary units)
double quat9D[N*4]; // output buffer

VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz
vqf.updateBatch(gyr, acc, mag, nullptr, quat9D);

Parameters:

• gyr – gyroscope measurement in rad/s (N*3 elements, must not be null)

• acc – accelerometer measurement in m/s² (N*3 elements, must not be null)

• mag – magnetometer measurement in arbitrary units (N*3 elements, can be a null pointer)

• N – number of samples

• out6D – output buffer for the 6D quaternion (N*4 elements, can be a null pointer)

• out9D – output buffer for the 9D quaternion (N*4 elements, can be a null pointer)

• outDelta – output buffer for heading difference angle (N elements, can be a null pointer)

void getQuat3D(vqf_real_t out[4]) const

Returns the angular velocity strapdown integration quaternion \(^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\).

Parameters:

out – output array for the quaternion

void getQuat6D(vqf_real_t out[4]) const

Returns the 6D (magnetometer-free) orientation quaternion \(^{\mathcal{S}_i}_{\mathcal{E}_i}\mathbf{q}\).

Parameters:

out – output array for the quaternion

void getQuat9D(vqf_real_t out[4]) const

Returns the 9D (with magnetometers) orientation quaternion \(^{\mathcal{S}_i}_{\mathcal{E}}\mathbf{q}\).

Parameters:

out – output array for the quaternion

vqf_real_t getDelta() const

Returns the heading difference \(\delta\) between \(\mathcal{E}_i\) and \(\mathcal{E}\).

\(^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & \sin\frac{\delta}{2}\end{bmatrix}^T\).

Returns:

delta angle in rad (VQFState::delta)

void setTauAcc(vqf_real_t tauAcc)

Sets the time constant for accelerometer low-pass filtering.

For more details, see VQFParams.tauAcc.

Parameters:

tauAcc – time constant \(\tau_\mathrm{acc}\) in seconds

void setTauMag(vqf_real_t tauMag)

Sets the time constant for the magnetometer update.

For more details, see VQFParams.tauMag.

Parameters:

tauMag – time constant \(\tau_\mathrm{mag}\) in seconds

const BasicVQFParams &getParams() const

Returns the current parameters.

const BasicVQFCoefficients &getCoeffs() const

Returns the coefficients used by the algorithm.

const BasicVQFState &getState() const

Returns the current state.

void setState(const BasicVQFState &state)

Overwrites the current state.

This method allows to set a completely arbitrary filter state and is intended for debugging purposes. In combination with getState, individual elements of the state can be modified.

Parameters:

state – A BasicVQFState struct containing the new state

void resetState()

Resets the state to the default values at initialization.

Resetting the state is equivalent to creating a new instance of this class.

Public Static Functions

static void quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4])

Performs quaternion multiplication ( \(\mathbf{q}_\mathrm{out} = \mathbf{q}_1 \otimes \mathbf{q}_2\)).

static void quatConj(const vqf_real_t q[4], vqf_real_t out[4])

Calculates the quaternion conjugate ( \(\mathbf{q}_\mathrm{out} = \mathbf{q}^*\)).

static void quatSetToIdentity(vqf_real_t out[4])

Sets the output quaternion to the identity quaternion ( \(\mathbf{q}_\mathrm{out} = \begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\)).

static void quatApplyDelta(vqf_real_t q[4], vqf_real_t delta, vqf_real_t out[4])

Applies a heading rotation by the angle delta (in rad) to a quaternion.

\(\mathbf{q}_\mathrm{out} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & \sin\frac{\delta}{2}\end{bmatrix} \otimes \mathbf{q}\)

static void quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3])

Rotates a vector with a given quaternion.

\(\begin{bmatrix}0 & \mathbf{v}_\mathrm{out}\end{bmatrix} = \mathbf{q} \otimes \begin{bmatrix}0 & \mathbf{v}\end{bmatrix} \otimes \mathbf{q}^*\)

static vqf_real_t norm(const vqf_real_t vec[], size_t N)

Calculates the Euclidean norm of a vector.

Parameters:

• vec – pointer to an array of N elements

• N – number of elements

static void normalize(vqf_real_t vec[], size_t N)

Normalizes a vector in-place.

Parameters:

• vec – pointer to an array of N elements that will be normalized

• N – number of elements

static void clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max)

Clips a vector in-place.

Parameters:

• vec – pointer to an array of N elements that will be clipped

• N – number of elements

• min – smallest allowed value

• max – largest allowed value

static vqf_real_t gainFromTau(vqf_real_t tau, vqf_real_t Ts)

Calculates the gain for a first-order low-pass filter from the 1/e time constant.

\(k = 1 - \exp\left(-\frac{T_\mathrm{s}}{\tau}\right)\)

The cutoff frequency of the resulting filter is \(f_\mathrm{c} = \frac{1}{2\pi\tau}\).

Parameters:

• tau – time constant \(\tau\) in seconds - use -1 to disable update ( \(k=0\)) or 0 to obtain unfiltered values ( \(k=1\))

• Ts – sampling time \(T_\mathrm{s}\) in seconds

Returns:

filter gain k

static void filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[3], double outA[2])

Calculates coefficients for a second-order Butterworth low-pass filter.

The filter is parametrized via the time constant of the dampened, non-oscillating part of step response and the resulting cutoff frequency is \(f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau}\).

Parameters:

• tau – time constant \(\tau\) in seconds

• Ts – sampling time \(T_\mathrm{s}\) in seconds

• outB – output array for numerator coefficients

• outA – output array for denominator coefficients (without \(a_0=1\))

static void filterInitialState(vqf_real_t x0, const double b[], const double a[], double out[2])

Calculates the initial filter state for a given steady-state value.

Parameters:

• x0 – steady state value

• b – numerator coefficients

• a – denominator coefficients (without \(a_0=1\))

• out – output array for filter state

static void filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[3], const double a_old[2], const double b_new[3], const double a_new[2], double state[])

Adjusts the filter state when changing coefficients.

This function assumes that the filter is currently in a steady state, i.e. the last input values and the last output values are all equal. Based on this, the filter state is adjusted to new filter coefficients so that the output does not jump.

Parameters:

• last_y – last filter output values (array of size N)

• N – number of values in vector-valued signal

• b_old – previous numerator coefficients

• a_old – previous denominator coefficients (without \(a_0=1\))

• b_new – new numerator coefficients

• a_new – new denominator coefficients (without \(a_0=1\))

• state – filter state (array of size N*2, will be modified)

static vqf_real_t filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2])

Performs a filter step for a scalar value.

Parameters:

• x – input value

• b – numerator coefficients

• a – denominator coefficients (without \(a_0=1\))

• state – filter state array (will be modified)

Returns:

filtered value

static void filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3], const double a[2], double state[], vqf_real_t out[])

Performs filter step for vector-valued signal with averaging-based initialization.

During the first \(\tau\) seconds, the filter output is the mean of the previous samples. At \(t=\tau\), the initial conditions for the low-pass filter are calculated based on the current mean value and from then on, regular filtering with the rational transfer function described by the coefficients b and a is performed.

Parameters:

• x – input values (array of size N)

• N – number of values in vector-valued signal

• tau – filter time constant \(\tau\) in seconds (used for initialization)

• Ts – sampling time \(T_\mathrm{s}\) in seconds (used for initialization)

• b – numerator coefficients

• a – denominator coefficients (without \(a_0=1\))

• state – filter state (array of size N*2, will be modified)

• out – output array for filtered values (size N)

Protected Functions

void setup()

Calculates coefficients based on parameters and sampling rates.

Protected Attributes

BasicVQFParams params

Contains the current parameters.

See getParams. To set parameters, pass them to the constructor. The parameters can be changed with setTauAcc and setTauMag.

BasicVQFState state

Contains the current state.

See getState, getState and resetState.

BasicVQFCoefficients coeffs

Contains the current coefficients (calculated in setup).

See getCoeffs.

struct BasicVQFParams

Struct containing all tuning parameters used by the BasicVQF class.

The parameters influence the behavior of the algorithm and are independent of the sampling rate of the IMU data. The constructor sets all parameters to the default values.

The basic version of this algoirthm only has two parameters: The time constants tauAcc and tauMag can be tuned to change the trust on the accelerometer and magnetometer measurements, respectively.

Public Functions

BasicVQFParams()

Constructor that initializes the struct with the default parameters.

Public Members

vqf_real_t tauAcc

Time constant \(\tau_\mathrm{acc}\) for accelerometer low-pass filtering in seconds.

Small values for \(\tau_\mathrm{acc}\) imply trust on the accelerometer measurements and while large values of \(\tau_\mathrm{acc}\) imply trust on the gyroscope measurements.

The time constant \(\tau_\mathrm{acc}\) corresponds to the cutoff frequency \(f_\mathrm{c}\) of the second-order Butterworth low-pass filter as follows: \(f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau_\mathrm{acc}}\).

Default value: 3.0 s

vqf_real_t tauMag

Time constant \(\tau_\mathrm{mag}\) for magnetometer update in seconds.

Small values for \(\tau_\mathrm{mag}\) imply trust on the magnetometer measurements and while large values of \(\tau_\mathrm{mag}\) imply trust on the gyroscope measurements.

The time constant \(\tau_\mathrm{mag}\) corresponds to the cutoff frequency \(f_\mathrm{c}\) of the first-order low-pass filter for the heading correction as follows: \(f_\mathrm{c} = \frac{1}{2\pi\tau_\mathrm{mag}}\).

Default value: 9.0 s

struct BasicVQFState

Struct containing the filter state of the BasicVQF class.

The relevant parts of the state can be accessed via functions of the BasicVVQF class, e.g. BasicVQF::getQuat6D() and BasicVQF::getQuat9D(). To reset the state to the initial values, use VQF::resetState().

Direct access to the full state is typically not needed but can be useful in some cases, e.g. for debugging. For this purpose, the state can be accessed by BasicVQF::getState() and set by BasicVQF::setState().

Public Members

vqf_real_t gyrQuat[4]

Angular velocity strapdown integration quaternion \(^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\).

vqf_real_t accQuat[4]

Inclination correction quaternion \(^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\).

vqf_real_t delta

Heading difference \(\delta\) between \(\mathcal{E}_i\) and \(\mathcal{E}\).

\(^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & \sin\frac{\delta}{2}\end{bmatrix}^T\).

vqf_real_t lastAccLp[3]

Last low-pass filtered acceleration in the \(\mathcal{I}_i\) frame.

double accLpState[3 * 2]

Internal low-pass filter state for lastAccLp.

vqf_real_t kMagInit

Gain used for heading correction to ensure fast initial convergence.

This value is used as the gain for heading correction in the beginning if it is larger than the normal filter gain. It is initialized to 1 and then updated to 0.5, 0.33, 0.25, … After VQFParams::tauMag seconds, it is set to zero.

struct BasicVQFCoefficients

Struct containing coefficients used by the BasicVQF class.

Coefficients are values that depend on the parameters and the sampling times, but do not change during update steps. They are calculated in BasicVQF::setup().

Public Members

vqf_real_t gyrTs

Sampling time of the gyroscope measurements (in seconds).

vqf_real_t accTs

Sampling time of the accelerometer measurements (in seconds).

vqf_real_t magTs

Sampling time of the magnetometer measurements (in seconds).

double accLpB[3]

Numerator coefficients of the acceleration low-pass filter.

The array contains \(\begin{bmatrix}b_0 & b_1 & b_2\end{bmatrix}\).

double accLpA[2]

Denominator coefficients of the acceleration low-pass filter.

The array contains \(\begin{bmatrix}a_1 & a_2\end{bmatrix}\) and \(a_0=1\).

vqf_real_t kMag

Gain of the first-order filter used for heading correction.