Struct VQF

pub struct VQF { /* private fields */ }

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

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 quat_6d(), quat_9d() and the other getter methods.

This class is a port of the official, original C++ implementation of the algorithm. For usage in C++/Python/MATLAB, the original implementations should be used.

Implementations

impl VQF

pub fn new( gyr_ts: Float, acc_ts: Option<Float>, mag_ts: Option<Float>, params: Option<Params>, ) -> Self

Creates a new VQF instance.

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

let vqf = VQF::new(0.01, None, None, None); // 0.01 s sampling time, i.e. 100 Hz

Panics

Panics if gyr_ts, acc_ts, or mag_ts is negative.

pub fn update_gyr(&mut self, gyr: [Float; 3])

Performs gyroscope update step, using the measurement in rad/s.

It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have different sampling rates. Otherwise, simply use update().

pub fn update_gyr_with_ts(&mut self, gyr: [Float; 3], ts: Float)

Performs gyroscope update step, using a specified timestep for integration.

pub fn update_acc(&mut self, acc: [Float; 3])

Performs accelerometer update step, using the measurement in m/s².

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 update_gyr() and before update_mag().

pub fn update_mag(&mut self, mag: [Float; 3])

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

pub fn update( &mut self, gyr: [Float; 3], acc: [Float; 3], mag: Option<[Float; 3]>, )

Performs filter update step for one sample.

pub fn quat_3d(&self) -> Quaternion

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

pub fn quat_6d(&self) -> Quaternion

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

pub fn quat_9d(&self) -> Quaternion

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

pub fn delta(&self) -> Float

Returns the heading difference $\delta$ between $\mathcal{E}_i$ and $\mathcal{E}$ in radians.

$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & \sin\frac{\delta}{2}\end{bmatrix}^T$.

pub fn bias_estimate(&self) -> ([Float; 3], Float)

Returns the current gyroscope bias estimate and the uncertainty in rad/s.

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.

pub fn set_bias_estimate(&mut self, bias: [Float; 3], sigma: Option<Float>)

Sets the current gyroscope bias estimate and the uncertainty in rad/s.

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

pub fn rest_detected(&self) -> bool

Returns true if rest was detected.

pub fn mag_dist_detected(&self) -> bool

Returns true if a disturbed magnetic field was detected.

pub fn relative_rest_deviations(&self) -> [Float; 2]

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.

pub fn mag_ref_norm(&self) -> Float

Returns the norm of the currently accepted magnetic field reference.

pub fn mag_ref_dip(&self) -> Float

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

pub fn set_mag_ref(&mut self, norm: Float, dip: Float)

Overwrites the current magnetic field reference.

pub fn set_tau_acc(&mut self, tau_acc: Float)

Sets the time constant $\tau_\mathrm{acc}$ in seconds for accelerometer low-pass filtering.

For more details, see Params::tau_acc.

pub fn set_tau_mag(&mut self, tau_mag: Float)

Sets the time constant $\tau_\mathrm{mag}$ in seconds for the magnetometer update.

For more details, see Params::tau_mag.

pub fn set_motion_bias_est_enabled(&mut self, enabled: bool)

Enables/disables gyroscope bias estimation during motion.

pub fn set_rest_bias_est_enabled(&mut self, enabled: bool)

Enables/disables rest detection and bias estimation during rest.

pub fn set_mag_dist_rejection_enabled(&mut self, enabled: bool)

Enables/disables magnetic disturbance detection and rejection.

pub fn set_rest_detection_thresholds(&mut self, th_gyr: Float, th_acc: Float)

Sets the current thresholds for rest detection.

For details about the parameters, see Params::rest_th_gyr and Params::rest_th_acc.

pub fn params(&self) -> &Params

Returns the current parameters.

pub fn coeffs(&self) -> &Coefficients

Returns the coefficients used by the algorithm.

pub fn state(&self) -> &State

Returns the current state.

pub fn state_mut(&mut self) -> &mut State

Gets the current state for modification.

This method allows to set a completely arbitrary filter state and is intended for debugging purposes.

pub fn reset_state(&mut self)

Resets the state to the default values at initialization.

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