vqf.BasicVQF

class vqf.BasicVQF(gyrTs, accTs=-1.0, magTs=-1.0, tauAcc=None, tauMag=None)

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

This class is a Python wrapper (implemented in Cython) for the C++ implementation BasicVQF. This basic 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`	`offlineVQF()`
Python/C++ (fast)	`vqf.VQF`	vqf.BasicVQF (this class)	`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 BasicVQF
vqf = BasicVQF(0.01)  # 0.01 s sampling time, i.e. 100 Hz

Example code to create an object with a different value for tauAcc (use the **-operator to pass parameters from a dict):

from vqf import BasicVQF
vqf = BasicVQF(0.01, tauAcc=1.0)  # 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 BasicVQFParams 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 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}\).

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.

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.

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)

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

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)

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

params

Read-only property to access the current parameters.

Returns:: dict with entries corresponding to BasicVQFParams

coeffs

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

Returns:: dict with entries corresponding to VBasicQFCoefficients

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 BasicVQFState

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