vqf.PyVQF

class vqf.PyVQF(gyrTs, accTs=-1.0, magTs=-1.0, **params)

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

This class is a pure Python implementation of the algorithm that only depends on the Python standard library and numpy. Note that the wrapper vqf.VQF for the C++ implementation VQF is much faster than this pure Python 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.PyVQ (this class)	–	–
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 PyVQF
vqf = PyVQF(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 PyVQF
vqf = PyVQF(0.01, 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:

from vqf import PyVQF
vqf = PyVQF(0.01, motionBiasEstEnabled=False, restBiasEstEnabled=False, magDistRejectionEnabled=False)

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(gyr)	Performs gyroscope update step.
updateAcc(acc)	Performs accelerometer update step.
updateMag(mag)	Performs magnetometer update step.
update(gyr, acc[, mag])	Performs filter update step for one sample.
updateBatch(gyr, acc[, mag])	Performs batch update for multiple samples at once.

Methods to Get/Set State

getQuat3D()	Returns the angular velocity strapdown integration quaternion \(^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\).
getQuat6D()	Returns the 6D (magnetometer-free) orientation quaternion \(^{\mathcal{S}_i}_{\mathcal{E}_i}\mathbf{q}\).
getQuat9D()	Returns the 9D (with magnetometers) orientation quaternion \(^{\mathcal{S}_i}_{\mathcal{E}}\mathbf{q}\).
getBiasEstimate()	Returns the current gyroscope bias estimate and the uncertainty.
setBiasEstimate(bias, sigma)	Sets the current gyroscope bias estimate and the uncertainty.
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.
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.

Methods to Change Parameters

setTauAcc(tauAcc)	Sets the time constant for accelerometer low-pass filtering.
setTauMag(tauMag)	Sets the time constant for the magnetometer update.
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.

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()	Resets the state to the default values at initialization.

Static Utility Functions

quatMultiply(q1, q2)	Performs quaternion multiplication (\(\mathbf{q}_\mathrm{out} = \mathbf{q}_1 \otimes \mathbf{q}_2\)).
quatConj(q)	Calculates the quaternion conjugate (\(\mathbf{q}_\mathrm{out} = \mathbf{q}^*\)).
quatApplyDelta(q, delta)	Applies a heading rotation by the angle delta (in rad) to a quaternion.
quatRotate(q, v)	Rotates a vector with a given quaternion.
normalize(vec)	Normalizes a vector in-place.
gainFromTau(tau, 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, b, a)	Calculates the initial filter state for a given steady-state value.
filterAdaptStateForCoeffChange(last_y, ...)	Adjusts the filter state when changing coefficients.
filterStep(x, b, a, state)	Performs a filter step.
filterVec(x, tau, Ts, b, a, state)	Performs filter step for vector-valued signal with averaging-based initialization.

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

Returns:

None

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

Returns:

None

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

Returns:

None

update(gyr, acc, 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(gyr, acc, 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 numpy array. 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

getQuat3D()

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

Returns:

quaternion as numpy array with shape (4,)

getQuat6D()

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

Returns:

quaternion as numpy array with shape (4,)

getQuat9D()

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

Returns:

quaternion as numpy array with shape (4,)

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

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

property params

Read-only property to access the current parameters.

Returns:

dict with entries corresponding to VQFParams

property coeffs

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

Returns:

dict with entries corresponding to VQFCoefficients

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

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

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

Returns:

output quaternion – numpy array with shape (4,)

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

• delta – heading rotation angle in rad

Returns:

output quaternion – numpy array with shape (4,)

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

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

Returns:

output vector – numpy array with shape (3,)

static normalize(vec)

Normalizes a vector in-place.

Parameters:

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

Returns:

None

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

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

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

• 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, N), will be modified

Returns:

filtered values – numpy array with shape (N,)

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 – 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 (2, N), will be modified

Returns:

filtered values – numpy array with shape (N,)