VQF.m

class matlab.VQF

A Versatile Quaternion-based Filter for IMU Orientation Estimation.

This class implements 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. It performs rest detection, gyroscope bias estimation during rest and motion, and magnetic disturbance detection and rejection. 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 a number of tuning parameters.

To use this class for online (sample-by-sample) processing,

create a instance of the class and provide the sampling time and, optionally, parameters
for every sample, call one of the update functions to feed the algorithm with IMU data
access the estimation results with getQuat6D(), getQuat9D() and the other getter methods.

If the full data is available in matrices, you can use updateBatch().

This class is a pure Matlab implementation of the algorithm. Note that the C++ implementation VQF and the Python wrapper vqf.VQF are much faster than this pure Matlab implementation. Depending on use case and programming language of choice, the following alternatives might be useful:

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

The constructor accepts the following arguments:

vqf = VQF(gyrTs);
vqf = VQF(gyrTs, accTs);
vqf = VQF(gyrTs, accTs, magTs);
vqf = VQF(gyrTs, params);
vqf = VQF(gyrTs, accTs, params);
vqf = VQF(gyrTs, accTs, magTs, params);

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

vqf = VQF(0.01); % 0.01 s sampling time, i.e. 100 Hz

Example code to create an object with magnetic disturbance rejection disabled:

vqf = VQF(0.01, struct('magDistRejectionEnabled', false)); % 0.01 s sampling time, i.e. 100 Hz

To use this class as a replacement for the basic version BasicVQF, pass the following parameters:

vqf = VQF(0.01, struct('motionBiasEstEnabled', false, 'restBiasEstEnabled', false, 'magDistRejectionEnabled', false));

See VQFParams for a detailed description of all parameters.

This class can be used in Simulink via a Matlab function block. See the following minimum example to get an idea of how to get started:

function quat = vqf_block(gyr, acc)
    persistent vqf
    if isempty(vqf)
        vqf = VQF(0.01, struct('magDistRejectionEnabled', false));
    end
    vqf.update(gyr', acc');
    quat = vqf.getQuat6D();
end

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)
params – struct containing optional parameters to override the defaults (see VQFParams for a full list and detailed descriptions)

Property Summary

params: Struct containing the current parameters (see VQFParams).

state: Struct containing the current state (see VQFState).

coeffs: Struct containing the coefficients used by the algorithm (see VQFCoefficients).

Method Summary

updateGyr(gyr)

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 – 1x3 matrix

updateAcc(acc)

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² – 1x3 matrix

updateMag(mag)

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 – 1x3 matrix

update(gyr, acc, mag)

Performs filter update step for one sample.

Parameters:

gyr – gyr gyroscope measurement in rad/s – 1x3 matrix
acc – acc accelerometer measurement in m/s² – 1x3 matrix
mag – optional mag magnetometer measurement in arbitrary units – 1x3 matrix

updateBatch(gyr, acc, mag)

Performs batch update for multiple samples at once.

In order to use this function, all input data must have the same sampling rate and be contained in Nx3 matrices. The output is a struct containing

quat6D – the 6D quaternion – Nx4 matrix
bias – gyroscope bias estimate – Nx4 matrix
biasSigma – uncertainty of gyroscope bias estimate in rad/s – Nx1 matrix
restDetected – rest detection state – boolean Nx1 matrix

in all cases and if magnetometer data is provided additionally

quat9D – the 9D quaternion – Nx4 matrix
delta – heading difference angle between 6D and 9D quaternion in rad – Nx1 matrix
magDistDetected – magnetic disturbance detection state – Nx1 boolean matrix

Parameters:

gyr – gyroscope measurement in rad/s – Nx3 matrix
acc – accelerometer measurement in m/s² – Nx3 matrix
mag – optional magnetometer measurement in arbitrary units – Nx3 matrix

Returns:

struct with entries as described above

getQuat3D()

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

Returns:: quaternion as 4x1 matrix

getQuat6D()

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

Returns:: quaternion as 4x1 matrix

getQuat9D()

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

Returns:: quaternion as 4x1 matrix

getDelta()

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)

getBiasEstimate()

Returns the current gyroscope bias estimate and the uncertainty.

The returned standard deviation sigma represents the estimation uncertainty in the worst direction and is based on an upper bound of the largest eigenvalue of the covariance matrix.

Returns:: gyroscope bias estimate (rad/s) as 4x1 matrix and standard deviation sigma of the estimation uncertainty (rad/s)

setBiasEstimate(bias, sigma)

Sets the current gyroscope bias estimate and the uncertainty.

If a value for the uncertainty sigma is given, the covariance matrix is set to a corresponding scaled identity matrix.

Parameters:

bias – gyroscope bias estimate (rad/s)
sigma – standard deviation of the estimation uncertainty (rad/s) - set to -1 (default) in order to not change the estimation covariance matrix

getRestDetected(): Returns true if rest was detected.

getMagDistDetected(): Returns true if a disturbed magnetic field was detected.

getRelativeRestDeviations()

Returns the relative deviations used in rest detection.

Looking at those values can be useful to understand how rest detection is working and which thresholds are suitable. The output array is filled with the last values for gyroscope and accelerometer, relative to the threshold. In order for rest to be detected, both values must stay below 1.

Returns:: relative rest deviations as 2x1 matrix

getMagRefNorm(): Returns the norm of the currently accepted magnetic field reference.

getMagRefDip(): Returns the dip angle of the currently accepted magnetic field reference.

setMagRef(norm, dip)

Overwrites the current magnetic field reference.

Parameters:

norm – norm of the magnetic field reference
dip – dip angle of the magnetic field reference

setTauAcc(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

setTauMag(tauMag)

Sets the time constant for the magnetometer update.

For more details, see VQFParams::tauMag.

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

setMotionBiasEstEnabled(enabled): Enables/disabled gyroscope bias estimation during motion.

setRestBiasEstEnabled(enabled): Enables/disables rest detection and bias estimation during rest.

setMagDistRejectionEnabled(enabled): Enables/disables magnetic disturbance detection and rejection.

setRestDetectionThresholds(thGyr, thAcc)

Sets the current thresholds for rest detection.

Parameters:

thGyr – new value for VQFParams::restThGyr
thAcc – new value for VQFParams::restThAcc

resetState()

Resets the state to the default values at initialization.

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

static quatMultiply(q1, q2)

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

Parameters:

q1 – input quaternion 1 – 4x1 matrix
q2 – input quaternion 2 – 4x1 matrix

Returns:

output quaternion – 4x1 matrix

static quatConj(q)

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

Parameters:: q – input quaternion – 4x1 matrix
Returns:: output quaternion – 4x1 matrix

static quatApplyDelta(q, delta)

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

Parameters:

q – input quaternion – 4x1 matrix
delta – heading rotation angle in rad

Returns:

output quaternion – 4x1 matrix

static quatRotate(q, v)

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}^*\)

Parameters:

q – input quaternion – 4x1 matrix
v – input vector – 3x1 matrix

Returns:

output vector – 3x1 matrix

static normalize(vec)

Normalizes a vector.

Parameters:: vec – input vector – Nx1 matrix
Returns:: normalized vector – Nx1 matrix

static clip(vec, min, max)

Clips a vector.

Parameters:

vec – input vector – Nx1 matrix
min – smallest allowed value
max – largest allowed value

Returns:

clipped vector – Nx1 matrix

static gainFromTau(tau, 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 filterCoeffs(tau, Ts)

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

Returns:

numerator coefficients b as 1x3 matrix, denominator coefficients a (without \(a_0=1\)) as 2x1 matrix

static filterInitialState(x0, b, a)

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

Returns:

filter state – 1x2 matrix

static filterAdaptStateForCoeffChange(last_y, b_old, a_old, b_new, a_new, state)

r”””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 – 1xN matrix
b_old – previous numerator coefficients –1x3 matrix
a_old – previous denominator coefficients (without \(a_0=1\)) – 1x2 matrix
b_new – new numerator coefficients – 1x3 matrix
a_new – new denominator coefficients (without \(a_0=1\)) – 1x2 matrix
state – input filter state – 2xN matrix

Returns:

modified filter state – 2xN matrix

static filterStep(x, b, a, state)

Performs a filter step.

Note: Unlike the C++ implementation, this function is vectorized and can process multiple values at once.

Parameters:

x – input values – 1xN matrix
b – numerator coefficients – 1x3 matrix
a – denominator coefficients (without \(a_0=1\)) – 1x2 matrix
state – filter state – 2xN matrix

Returns:

filtered values as 1xN matrix and new filter state as 2xN matrix

static filterVec(x, tau, Ts, b, a, state)

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 – 1xN matrix
tau – filter time constant :math:tau in seconds (used for initialization)
Ts – sampling time \(T_\mathrm{s}\) in seconds (used for initialization)
b – numerator coefficients – 1x3 matrix
a – denominator coefficients (without \(a_0=1\)) – 1x2 matrix
state – filter state – 2xN matrix

Returns:

filtered values as 1xN matrix and new filter state as 2xN matrix