Struct UKF 

pub struct UKF { /* private fields */ }

Strapdown Unscented Kalman Filter Inertial Navigation Filter

This filter uses the Unscented Kalman Filter (UKF) algorithm to estimate the state of a strapdown inertial navigation system. It uses the strapdown equations to propagate the state in the local level frame based on IMU measurements in the body frame. The filter also uses a generic measurement model to update the state based on measurements in the local level frame.

Because of the generic nature of both the UKF and this toolbox, the filter requires the user to implement the measurement model(s). The measurement model must calculate the measurement sigma points ($\mathcal{Z} = h(\mathcal{X})$) and the measurement noise matrix ($R$) for the filter. Some basic GNSS-based are provided in this module (position, velocity, position and velocity, barometric altitude). In a given scenario’s implementation, the user should then call these measurement models. Please see the sim module for a reference implementation of a full state UKF INS with a position and velocity GPS-based measurement model and barometric altitude measurement model.

Note that, internally, angles are always stored in radians (both for the attitude and the position), however, the user can choose to convert them to degrees when retrieving the state vector and the UKF and underlying strapdown state can be constructed from data in degrees by using the boolean in_degrees toggle where applicable. Generally speaking, the design of this crate is such that methods that expect a WGS84 coordinate (e.g. latitude or longitude) will expect the value in degrees, whereas trigonometric functions (e.g. sine, cosine, tangent) will expect the value in radians.

Implementations

impl UKF

pub fn new( strapdown_state: StrapdownParams, imu_biases: Vec<f64>, other_states: Option<Vec<f64>>, covariance_diagonal: Vec<f64>, process_noise: DMatrix<f64>, alpha: f64, beta: f64, kappa: f64, ) -> UKF

Creates a new UKF with the given initial state, biases, covariance, process noise, any additional other states, and UKF hyper parameters.

Arguments

• position - The initial position of the strapdown state.
• velocity - The initial velocity of the strapdown state.
• attitude - The initial attitude of the strapdown state.
• imu_biases - The initial IMU biases.
• other_states - Any additional states the filter is estimating (ex: measurement or sensor bias).
• covariance_diagonal - The initial covariance diagonal.
• process_noise_diagonal - The process noise diagonal.
• alpha - The alpha parameter for the UKF.
• beta - The beta parameter for the UKF.
• kappa - The kappa parameter for the UKF.
• in_degrees - Whether the input vectors are in degrees or radians.

Returns

• A new UKF struct.

pub fn predict(&mut self, imu_data: IMUData, dt: f64)

Predicts the state using the strapdown equations and IMU measurements.

The IMU measurements are used to update the strapdown state in the local level frame. The IMU measurements are assumed to be in the body frame.

Arguments

• imu_data - The IMU measurements to propagate the state with (e.g. relative accelerations (m/s^2) and angular rates (rad/s)).
• dt - The time step for the propagation.

Returns

• none

pub fn get_mean(&self) -> DVector<f64>

Get the UKF mean state.

pub fn get_covariance(&self) -> DMatrix<f64>

Get the UKF covariance.

pub fn get_sigma_points(&self) -> DMatrix<f64>

Convert a Vec to a DMatrix

pub fn update<M: MeasurementModel>(&mut self, measurement: M)

Perform the Kalman measurement update step.

This method updates the state and covariance based on the measurement and measurement sigma points. The measurement model is specific to a given implementation of the UKF and must be provided by the user as the model determines the shape and quantities of the measurement vector and the measurement sigma points. Measurement models should be implemented as traits and applied to the UKF as needed.

This module contains some standard GNSS-aided measurements models (position_measurement_model, velocity_measurement_model, and position_and_velocity_measurement_model) that can be used. See the sim module for a canonical example of a GPS-aided INS implementation that uses these models.

Note: Canonical INS implementations use a position measurement model. Typically, position is reported in degrees for latitude and longitude, and in meters for altitude. Internally, the UKF stores the latitude and longitude in radians, and the measurement models make no assumptions about the units of the position measurements. However, the user should ensure that the provided measurement to this function is in the same units as the measurement model.

Arguments

• measurement - The measurement vector to update the state with.
• measurement_sigma_points - The measurement sigma points to use for the update.

Trait Implementations

impl Debug for UKF

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.