State and Coefficients

struct VQFState

Struct containing the filter state of the VQF class.

The relevant parts of the state can be accessed via functions of the VQF class, e.g. VQF::getQuat6D(), VQF::getQuat9D(), VQF::getGyrBiasEstimate(), VQF::setGyrBiasEstimate(), VQF::getRestDetected() and VQF::getMagDistDetected(). 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 VQF::getState() and set by VQF::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\).

bool restDetected

True if it has been detected that the IMU is currently at rest.

Used to switch between rest and motion gyroscope bias estimation.

bool magDistDetected: True if magnetic disturbances have been detected.

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 lastAccCorrAngularRate

Last inclination correction angular rate.

Change to inclination correction quaternion \(^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\) performed in the last accelerometer update, expressed as an angular rate (in rad/s).

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.

vqf_real_t lastMagDisAngle

Last heading disagreement angle.

Disagreement between the heading \(\hat\delta\) estimated from the last magnetometer sample and the state \(\delta\) (in rad).

vqf_real_t lastMagCorrAngularRate

Last heading correction angular rate.

Change to heading \(\delta\) performed in the last magnetometer update, expressed as an angular rate (in rad/s).

vqf_real_t bias[3]: Current gyroscope bias estimate (in rad/s).

vqf_real_t biasP[9]

Covariance matrix of the gyroscope bias estimate.

The 3x3 matrix is stored in row-major order. Note that for numeric reasons the internal unit used is 0.01 °/s, i.e. to get the standard deviation in degrees per second use \(\sigma = \frac{\sqrt{p_{ii}}}{100}\).

double motionBiasEstRLpState[9 * 2]: Internal state of the Butterworth low-pass filter for the rotation matrix coefficients used in motion bias estimation.

double motionBiasEstBiasLpState[2 * 2]: Internal low-pass filter state for the rotated bias estimate used in motion bias estimation.

vqf_real_t restLastSquaredDeviations[2]

Last (squared) deviations from the reference of the last sample used in rest detection.

Looking at those values can be useful to understand how rest detection is working and which thresholds are suitable. The array contains the last values for gyroscope and accelerometer in the respective units. Note that the values are squared.

The method VQF::getRelativeRestDeviations() provides an easier way to obtain and interpret those values.

vqf_real_t restT

The current duration for which all sensor readings are within the rest detection thresholds.

Rest is detected if this value is larger or equal to VQFParams::restMinT.

vqf_real_t restLastGyrLp[3]

Last low-pass filtered gyroscope measurement used as the reference for rest detection.

Note that this value is also used for gyroscope bias estimation when rest is detected.

double restGyrLpState[3 * 2]: Internal low-pass filter state for restLastGyrLp.

vqf_real_t restLastAccLp[3]: Last low-pass filtered accelerometer measurement used as the reference for rest detection.

double restAccLpState[3 * 2]: Internal low-pass filter state for restLastAccLp.

vqf_real_t magRefNorm

Norm of the currently accepted magnetic field reference.

A value of -1 indicates that no homogeneous field is found yet.

vqf_real_t magRefDip: Dip angle of the currently accepted magnetic field reference.

vqf_real_t magUndisturbedT

The current duration for which the current norm and dip are close to the reference.

The magnetic field is regarded as undisturbed when this value reaches VQFParams::magMinUndisturbedTime.

vqf_real_t magRejectT

The current duration for which the magnetic field was rejected.

If the magnetic field is disturbed and this value is smaller than VQFParams::magMaxRejectionTime, heading correction updates are fully disabled.

vqf_real_t magCandidateNorm: Norm of the alternative magnetic field reference currently being evaluated.

vqf_real_t magCandidateDip: Dip angle of the alternative magnetic field reference currently being evaluated.

vqf_real_t magCandidateT

The current duration for which the norm and dip are close to the candidate.

If this value exceeds VQFParams::magNewTime (or VQFParams::magNewFirstTime if magRefNorm < 0), the current candidate is accepted as the new reference.

vqf_real_t magNormDip[2]

Norm and dip angle of the current magnetometer measurements.

Slightly low-pass filtered, see VQFParams::magCurrentTau.

double magNormDipLpState[2 * 2]: Internal low-pass filter state for the current norm and dip angle.

struct VQFCoefficients

Struct containing coefficients used by the VQF class.

Coefficients are values that depend on the parameters and the sampling times, but do not change during update steps. They are calculated in VQF::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.

vqf_real_t biasP0: Variance of the initial gyroscope bias estimate.

vqf_real_t biasV: System noise variance used in gyroscope bias estimation.

vqf_real_t biasMotionW: Measurement noise variance for the motion gyroscope bias estimation update.

vqf_real_t biasVerticalW: Measurement noise variance for the motion gyroscope bias estimation update in vertical direction.

vqf_real_t biasRestW: Measurement noise variance for the rest gyroscope bias estimation update.

double restGyrLpB[3]: Numerator coefficients of the gyroscope measurement low-pass filter for rest detection.

double restGyrLpA[2]: Denominator coefficients of the gyroscope measurement low-pass filter for rest detection.

double restAccLpB[3]: Numerator coefficients of the accelerometer measurement low-pass filter for rest detection.

double restAccLpA[2]: Denominator coefficients of the accelerometer measurement low-pass filter for rest detection.

vqf_real_t kMagRef: Gain of the first-order filter used for to update the magnetic field reference and candidate.

double magNormDipLpB[3]: Numerator coefficients of the low-pass filter for the current magnetic norm and dip.

double magNormDipLpA[2]: Denominator coefficients of the low-pass filter for the current magnetic norm and dip.