Struct State

pub struct State {

    pub gyr_quat: Quaternion,
    pub acc_quat: Quaternion,
    pub delta: Float,
    pub rest_detected: bool,
    pub mag_dist_detected: bool,
    pub last_acc_lp: [Float; 3],
    pub acc_lp_state: [[f64; 2]; 3],
    pub last_acc_corr_angular_rate: Float,
    pub k_mag_init: Float,
    pub last_mag_dis_angle: Float,
    pub last_mag_corr_angular_rate: Float,
    pub bias: [Float; 3],
    pub bias_p: Matrix<3, 3>,
    pub motion_bias_est_rlp_state: [[f64; 2]; 9],
    pub motion_bias_est_bias_lp_state: [[f64; 2]; 2],
    pub rest_last_squared_deviations: [Float; 2],
    pub rest_t: Float,
    pub rest_last_gyr_lp: [Float; 3],
    pub rest_gyr_lp_state: [[f64; 2]; 3],
    pub rest_last_acc_lp: [Float; 3],
    pub rest_acc_lp_state: [[f64; 2]; 3],
    pub mag_ref_norm: Float,
    pub mag_ref_dip: Float,
    pub mag_undisturbed_t: Float,
    pub mag_reject_t: Float,
    pub mag_candidate_norm: Float,
    pub mag_candidate_dip: Float,
    pub mag_candidate_t: Float,
    pub mag_norm_dip: [Float; 2],
    pub mag_norm_dip_lp_state: [[f64; 2]; 2],
}

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::quat_6d(), VQF::quat_9d(), VQF::bias_estimate(), VQF::set_bias_estimate(), VQF::rest_detected() and VQF::mag_dist_detected(). To reset the state to the initial values, use VQF::reset_state().

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::state() and set by VQF::state_mut().

Fields

gyr_quat: Quaternion

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

acc_quat: Quaternion

Inclination correction quaternion $^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}$.

delta: Float

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$.

rest_detected: bool

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

Used to switch between rest and motion gyroscope bias estimation.

mag_dist_detected: bool

True if magnetic disturbances have been detected.

last_acc_lp: [Float; 3]

Last low-pass filtered acceleration in the $\mathcal{I}_i$ frame.

acc_lp_state: [[f64; 2]; 3]

Internal low-pass filter state for last_acc_lp.

last_acc_corr_angular_rate: Float

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

k_mag_init: Float

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 Params::tau_mag seconds, it is set to zero.

last_mag_dis_angle: Float

Last heading disagreement angle.

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

last_mag_corr_angular_rate: Float

Last heading correction angular rate.

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

bias: [Float; 3]

Current gyroscope bias estimate (in rad/s).

bias_p: Matrix<3, 3>

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}$.

motion_bias_est_rlp_state: [[f64; 2]; 9]

Internal state of the Butterworth low-pass filter for the rotation matrix coefficients used in motion bias estimation.

motion_bias_est_bias_lp_state: [[f64; 2]; 2]

Internal low-pass filter state for the rotated bias estimate used in motion bias estimation.

rest_last_squared_deviations: [Float; 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::relative_rest_deviations() provides an easier way to obtain and interpret those values.

rest_t: Float

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 Params::rest_min_t.

rest_last_gyr_lp: [Float; 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.

rest_gyr_lp_state: [[f64; 2]; 3]

Internal low-pass filter state for rest_last_gyr_lp.

rest_last_acc_lp: [Float; 3]

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

rest_acc_lp_state: [[f64; 2]; 3]

Internal low-pass filter state for rest_last_acc_lp.

mag_ref_norm: Float

Norm of the currently accepted magnetic field reference.

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

mag_ref_dip: Float

Dip angle of the currently accepted magnetic field reference.

mag_undisturbed_t: Float

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 Params::mag_min_undisturbed_time.

mag_reject_t: Float

The current duration for which the magnetic field was rejected.

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

mag_candidate_norm: Float

Norm of the alternative magnetic field reference currently being evaluated.

mag_candidate_dip: Float

Dip angle of the alternative magnetic field reference currently being evaluated.

mag_candidate_t: Float

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

If this value exceeds Params::mag_new_time (or Params::mag_new_first_time if mag_ref_norm < 0), the current candidate is accepted as the new reference.

mag_norm_dip: [Float; 2]

Norm and dip angle of the current magnetometer measurements.

Slightly low-pass filtered, see Params::mag_current_tau.

mag_norm_dip_lp_state: [[f64; 2]; 2]

Internal low-pass filter state for the current norm and dip angle.

Trait Implementations

impl Clone for State

fn clone(&self) -> State

Returns a copy of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl Default for State

fn default() -> State

Returns the “default value” for a type.

impl Copy for State