vqf.VQF

class vqf.VQF(gyrTs, accTs=-1.0, magTs=-1.0, tauAcc=None, tauMag=None, motionBiasEstEnabled=None, restBiasEstEnabled=None, magDistRejectionEnabled=None, biasSigmaInit=None, biasForgettingTime=None, biasClip=None, biasSigmaMotion=None, biasVerticalForgettingFactor=None, biasSigmaRest=None, restMinT=None, restFilterTau=None, restThGyr=None, restThAcc=None, magCurrentTau=None, magRefTau=None, magNormTh=None, magDipTh=None, magNewTime=None, magNewFirstTime=None, magNewMinGyr=None, magMinUndisturbedTime=None, magMaxRejectionTime=None, magRejectionFactor=None)

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,

  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 numpy arrays, you can use updateBatch() and updateBatchFullState().

This class is a Python wrapper (implemented in Cython) for the C++ implementation VQF. 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 (this class)

vqf.BasicVQF

vqf.offlineVQF()

Pure Python (slow)

vqf.PyVQF

Pure Matlab (slow)

VQF.m

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

from vqf import VQF
vqf = VQF(0.01)  # 0.01 s sampling time, i.e. 100 Hz

Example code to create an object with magnetic disturbance rejection disabled (use the **-operator to pass parameters from a dict):

from vqf import VQF
vqf = VQF(0.01, magDistRejectionEnabled=False)  # 0.01 s sampling time, i.e. 100 Hz

See VQFParams for a detailed description of all parameters.

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

Update Methods

updateGyr(self, ndarray gyr)

Performs gyroscope update step.

updateAcc(self, ndarray acc)

Performs accelerometer update step.

updateMag(self, ndarray mag)

Performs magnetometer update step.

update(self, ndarray gyr, ndarray acc, ...)

Performs filter update step for one sample.

updateBatch(self, ndarray gyr, ndarray acc, ...)

Performs batch update for multiple samples at once.

updateBatchFullState(self, ndarray gyr, ...)

Performs batch update and returns full state.

Methods to Get/Set State

getQuat3D(self)

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

getQuat6D(self)

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

getQuat9D(self)

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

getDelta(self)

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

getBiasEstimate(self)

Returns the current gyroscope bias estimate and the uncertainty.

setBiasEstimate(self, ndarray bias, ...)

Sets the current gyroscope bias estimate and the uncertainty.

getRestDetected(self)

Returns true if rest was detected.

getMagDistDetected(self)

Returns true if a disturbed magnetic field was detected.

getRelativeRestDeviations(self)

Returns the relative deviations used in rest detection.

getMagRefNorm(self)

Returns the norm of the currently accepted magnetic field reference.

getMagRefDip(self)

Returns the dip angle of the currently accepted magnetic field reference.

setMagRef(self, norm, dip)

Overwrites the current magnetic field reference.

Methods to Change Parameters

setTauAcc(self, tauAcc)

Sets the time constant for accelerometer low-pass filtering.

setTauMag(self, tauMag)

Sets the time constant for the magnetometer update.

setMotionBiasEstEnabled(self, enabled)

Enables/disabled gyroscope bias estimation during motion.

setRestBiasEstEnabled(self, enabled)

Enables/disables rest detection and bias estimation during rest.

setMagDistRejectionEnabled(self, enabled)

Enables/disables magnetic disturbance detection and rejection.

setRestDetectionThresholds(self, thGyr, thAcc)

Sets the current thresholds for rest detection.

Access to Full Params/Coeffs/State

params

Read-only property to access the current parameters.

coeffs

Read-only property to access the coefficients used by the algorithm.

state

Property to access the current state.

resetState(self)

Resets the state to the default values at initialization.

Static Utility Functions

quatMultiply(ndarray q1, ndarray q2)

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

quatConj(ndarray q)

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

quatSetToIdentity(ndarray out)

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

quatApplyDelta(ndarray q, vqf_real_t delta)

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

quatRotate(ndarray q, ndarray v)

Rotates a vector with a given quaternion.

norm(ndarray vec)

Calculates the Euclidean norm of a vector.

normalize(ndarray vec)

Normalizes a vector in-place.

clip(ndarray vec, min_, max_)

Clips a vector in-place.

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.

filterCoeffs(tau, Ts)

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

filterInitialState(x0, ndarray b, ndarray a)

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

filterAdaptStateForCoeffChange(...)

Adjusts the filter state when changing coefficients.

filterStep(x, ndarray b, ndarray a, ...)

Performs a filter step for a scalar value.

filterVec(ndarray x, tau, Ts, ndarray b, ...)

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

matrix3SetToScaledIdentity(scale, ndarray out)

Sets a 3x3 matrix to a scaled version of the identity matrix.

matrix3Multiply(ndarray in1, ndarray in2)

Performs 3x3 matrix multiplication (\(\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2\)).

matrix3MultiplyTpsFirst(ndarray in1, ndarray in2)

Performs 3x3 matrix multiplication after transposing the first matrix (\(\mathbf{M}_\mathrm{out} = \mathbf{M}_1^T\mathbf{M}_2\)).

matrix3MultiplyTpsSecond(ndarray in1, ...)

Performs 3x3 matrix multiplication after transposing the second matrix (\(\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2^T\)).

matrix3Inv(ndarray in_)

Calculates the inverse of a 3x3 matrix (\(\mathbf{M}_\mathrm{out} = \mathbf{M}^{-1}\)).

updateGyr(self, ndarray 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 – numpy array with shape (3,)

Returns:

None

updateAcc(self, ndarray 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² – numpy array with shape (3,)

Returns:

None

updateMag(self, ndarray 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 – numpy array with shape (3,)

Returns:

None

update(self, ndarray gyr, ndarray acc, ndarray mag=None)

Performs filter update step for one sample.

Parameters:
  • gyr – gyr gyroscope measurement in rad/s – numpy array with shape (3,)

  • acc – acc accelerometer measurement in m/s² – numpy array with shape (3,)

  • mag – optional mag magnetometer measurement in arbitrary units – numpy array with shape (3,)

Returns:

None

updateBatch(self, ndarray gyr, ndarray acc, ndarray mag=None)

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 provided as a contiguous numpy array. As looping over the samples is performed in compiled code, it is much faster than using a Python loop. The output is a dictionary containing

  • quat6D – the 6D quaternion – numpy array with shape (N, 4)

  • bias – gyroscope bias estimate in rad/s – numpy array with shape (N, 3)

  • biasSigma – uncertainty of gyroscope bias estimate in rad/s – numpy array with shape (N,)

  • restDetected – rest detection state – boolean numpy array with shape (N,)

in all cases and if magnetometer data is provided additionally

  • quat9D – the 9D quaternion – numpy array with shape (N, 4)

  • delta – heading difference angle between 6D and 9D quaternion in rad – numpy array with shape (N,)

  • magDistDetected – magnetic disturbance detection state – boolean numpy array with shape (N,)

Parameters:
  • gyr – gyroscope measurement in rad/s – numpy array with shape (N,3)

  • acc – accelerometer measurement in m/s² – numpy array with shape (N,3)

  • mag – optional magnetometer measurement in arbitrary units – numpy array with shape (N,3)

Returns:

dict with entries as described above

updateBatchFullState(self, ndarray gyr, ndarray acc, ndarray mag=None)

Performs batch update and returns full state.

Works similar to updateBatch() but returns a dictionary containing quat6D, quat9D and every value of state at every sampling step in a numpy array. As looping over the samples is performed in compiled code, it is much faster than using a Python loop.

Parameters:
  • gyr – gyroscope measurement in rad/s – numpy array with shape (N,3)

  • acc – accelerometer measurement in m/s² – numpy array with shape (N,3)

  • mag – optional magnetometer measurement in arbitrary units – numpy array with shape (N,3)

Returns:

dict with full state as numpy array

getQuat3D(self)

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

Returns:

quaternion as numpy array with shape (4,)

getQuat6D(self)

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

Returns:

quaternion as numpy array with shape (4,)

getQuat9D(self)

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

Returns:

quaternion as numpy array with shape (4,)

getDelta(self)

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(self)

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 (3,) numpy array and standard deviation sigma of the estimation uncertainty (rad/s)

setBiasEstimate(self, ndarray bias, vqf_real_t sigma=-1.0)

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(self)

Returns true if rest was detected.

getMagDistDetected(self)

Returns true if a disturbed magnetic field was detected.

getRelativeRestDeviations(self)

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 (2,) numpy array

getMagRefNorm(self)

Returns the norm of the currently accepted magnetic field reference.

getMagRefDip(self)

Returns the dip angle of the currently accepted magnetic field reference.

setMagRef(self, 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(self, 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(self, 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(self, enabled)

Enables/disabled gyroscope bias estimation during motion.

setRestBiasEstEnabled(self, enabled)

Enables/disables rest detection and bias estimation during rest.

setMagDistRejectionEnabled(self, enabled)

Enables/disables magnetic disturbance detection and rejection.

setRestDetectionThresholds(self, thGyr, thAcc)

Sets the current thresholds for rest detection.

Parameters:
params

Read-only property to access the current parameters.

Returns:

dict with entries corresponding to VQFParams

coeffs

Read-only property to access the coefficients used by the algorithm.

Returns:

dict with entries corresponding to VQFCoefficients

state

Property to access the current state.

This property can be written to in order to set a completely arbitrary filter state, which is intended for debugging purposes. However, note that the returned dict is a copy of the state and changing elements of this dict will not influence the actual state. In order to modify the state, access the state, change some elements and then replace the whole state with the modified copy, e.g.

# does not work: v.state['delta'] = 0
state = vqf.state
state['delta'] = 0
vqf.state = state
Returns:

dict with entries corresponding to VQFState

resetState(self)

Resets the state to the default values at initialization.

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

static quatMultiply(ndarray q1, ndarray q2)

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

Parameters:
  • q1 – input quaternion 1 – numpy array with shape (4,)

  • q2 – input quaternion 2 – numpy array with shape (4,)

Returns:

output quaternion – numpy array with shape (4,)

static quatConj(ndarray q)

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

Parameters:

q – input quaternion – numpy array with shape (4,)

Returns:

output quaternion – numpy array with shape (4,)

static quatSetToIdentity(ndarray out)

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

Parameters:

out – output array that will be modified – numpy array with shape (4,)

Returns:

None

static quatApplyDelta(ndarray q, vqf_real_t 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 – numpy array with shape (4,)

  • delta – heading rotation angle in rad

Returns:

output quaternion – numpy array with shape (4,)

static quatRotate(ndarray q, ndarray 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 – numpy array with shape (4,)

  • v – input vector – numpy array with shape (3,)

Returns:

output vector – numpy array with shape (3,)

static norm(ndarray vec)

Calculates the Euclidean norm of a vector.

Parameters:

vec – input vector – one-dimensional numpy array

Returns:

Euclidean norm of the input vector

static normalize(ndarray vec)

Normalizes a vector in-place.

Parameters:

vec – vector – one-dimensional numpy array that will be normalized in-place

Returns:

None

static clip(ndarray vec, min_, max_)

Clips a vector in-place.

Parameters:
  • vec – vector – one-dimensional numpy array that will be clipped in-place

  • min – smallest allowed value

  • max – largest allowed value

Returns:

None

static 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 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 (3,) numpy array, denominator coefficients a (without \(a_0=1\)) as (2,) numpy array

static filterInitialState(x0, ndarray b, ndarray 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 – numpy array with shape (2,)

static filterAdaptStateForCoeffChange(ndarray last_y, ndarray b_old, ndarray a_old, ndarray b_new, ndarray a_new, ndarray 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 – numpy array with shape (N,)

  • b_old – previous numerator coefficients – numpy array with shape (3,)

  • a_old – previous denominator coefficients (without \(a_0=1\)) – numpy array with shape (2,)

  • b_new – new numerator coefficients – numpy array with shape (3,)

  • a_new – new denominator coefficients (without \(a_0=1\)) – numpy array with shape (2,)

  • state – filter state – numpy array with shape (N*2,), will be modified

Returns:

None

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

Performs a filter step for a scalar value.

Parameters:
  • x – input value

  • b – numerator coefficients – numpy array with shape (3,)

  • a – denominator coefficients (without \(a_0=1\)) – numpy array with shape (2,)

  • state – filter state – numpy array with shape (2,), will be modified

Returns:

filtered value

static filterVec(ndarray x, tau, Ts, ndarray b, ndarray a, ndarray 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 – numpy array with shape (N,)

  • 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 – numpy array with shape (3,)

  • a – denominator coefficients (without \(a_0=1\)) – numpy array with shape (2,)

  • state – filter state – numpy array with shape (N*2,), will be modified

Returns:

filtered values – numpy array with shape (N,)

static matrix3SetToScaledIdentity(scale, ndarray out)

Sets a 3x3 matrix to a scaled version of the identity matrix.

Parameters:
  • scale – value of diagonal elements

  • out – output array – numpy array with shape (3,3), will be modified

Returns:

None

static matrix3Multiply(ndarray in1, ndarray in2)

Performs 3x3 matrix multiplication (\(\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2\)).

Parameters:
  • in1 – input 3x3 matrix \(\mathbf{M}_1\) – numpy array with shape (3,3)

  • in2 – input 3x3 matrix \(\mathbf{M}_2\) – numpy array with shape (3,3)

Returns:

output 3x3 matrix \(\mathbf{M}_\mathrm{out}\) – numpy array with shape (3,3)

static matrix3MultiplyTpsFirst(ndarray in1, ndarray in2)

Performs 3x3 matrix multiplication after transposing the first matrix (\(\mathbf{M}_\mathrm{out} = \mathbf{M}_1^T\mathbf{M}_2\)).

Parameters:
  • in1 – input 3x3 matrix \(\mathbf{M}_1\) – numpy array with shape (3,3)

  • in2 – input 3x3 matrix \(\mathbf{M}_2\) – numpy array with shape (3,3)

Returns:

output 3x3 matrix \(\mathbf{M}_\mathrm{out}\) – numpy array with shape (3,3)

static matrix3MultiplyTpsSecond(ndarray in1, ndarray in2)

Performs 3x3 matrix multiplication after transposing the second matrix (\(\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2^T\)).

Parameters:
  • in1 – input 3x3 matrix \(\mathbf{M}_1\) – numpy array with shape (3,3)

  • in2 – input 3x3 matrix \(\mathbf{M}_2\) – numpy array with shape (3,3)

Returns:

output 3x3 matrix \(\mathbf{M}_\mathrm{out}\) – numpy array with shape (3,3)

static matrix3Inv(ndarray in_)

Calculates the inverse of a 3x3 matrix (\(\mathbf{M}_\mathrm{out} = \mathbf{M}^{-1}\)).

Parameters:

in – input 3x3 matrix \(\mathbf{M}\) – numpy array with shape (3,3)

Returns:

output 3x3 matrix \(\mathbf{M}_\mathrm{out}\) – numpy array with shape (3,3)