Struct eskf::ESKF

pub struct ESKF {
    pub position: Point3<f32>,
    pub velocity: Vector3<f32>,
    pub orientation: UnitQuaternion<f32>,
    pub accel_bias: Vector3<f32>,
    pub rot_bias: Vector3<f32>,
    pub gravity: Vector3<f32>,
    // some fields omitted
}

Error State Kalman Filter

The filter works by calling predict and one or more of the observe_ methods when data is available. It is expected that several calls to predict will be in between calls to observe_.

The predict step updates the internal state of the filter based on measured acceleration and rotation coming from an IMU. This step updates the states in the filter based on kinematic equations while increasing the uncertainty of the filter. When one of the observe_ methods are called, the filter updates the internal state based on this observation, which exposes the error state to the filter, which we can then use to correct the internal state. The uncertainty of the filter is also updated to reflect the variance of the observation and the updated state.

Fields

position: Point3<f32>

Estimated position in filter

velocity: Vector3<f32>

Estimated velocity in filter

orientation: UnitQuaternion<f32>

Estimated orientation in filter

accel_bias: Vector3<f32>

Estimated acceleration bias

rot_bias: Vector3<f32>

Estimated rotation bias

gravity: Vector3<f32>

Estimated gravity vector

Implementations

impl ESKF

pub fn variance_from_element(var: f32) -> Matrix3<f32>

Create a symmetric variance matrix based on a single variance element

This helper method can be used when the sensor being modelled has a symmetric variance around its three axis. Or if only an estimate of the variance is known.

pub fn variance_from_diagonal(var: Vector3<f32>) -> Matrix3<f32>

Create a symmetric variance matrix based on the diagonal vector

This helper method can be used when the sensor being modelled has a independent variance around its three axis.

pub fn position_uncertainty(&self) -> Vector3<f32>

Get the uncertainty of the position estimate

pub fn velocity_uncertainty(&self) -> Vector3<f32>

Get the uncertainty of the velocity estimate

pub fn orientation_uncertainty(&self) -> Vector3<f32>

Get the uncertainty of the orientation estimate

pub fn predict(
    &mut self,
    acceleration: Vector3<f32>,
    rotation: Vector3<f32>,
    delta: Delta
)

Update the filter, predicting the new state, based on measured acceleration and rotation from an IMU

pub fn update<R: Dim>(
    &mut self,
    jacobian: MatrixMN<f32, R, U18>,
    difference: VectorN<f32, R>,
    variance: MatrixN<f32, R>
) -> Result<()> where
    DefaultAllocator: Allocator<f32, R> + Allocator<f32, R, R> + Allocator<f32, R, U18> + Allocator<f32, U18, R>, 

Update the filter with a generic observation

Arguments

• jacobian is the measurement Jacobian matrix
• difference is the difference between the measured sensor and the filter’s internal state
• variance is the uncertainty of the observation

pub fn observe_position_velocity2d(
    &mut self,
    position: Point3<f32>,
    position_var: Matrix3<f32>,
    velocity: Vector2<f32>,
    velocity_var: Matrix2<f32>
) -> Result<()>

Observe the position and velocity in the X and Y axis

Most GPS units are capable of observing both position and velocity, by combining these two measurements into one update we should be able to reduce the computational complexity. Also note that GPS velocity tends to be more precise than position.

pub fn observe_position(
    &mut self,
    measurement: Point3<f32>,
    variance: Matrix3<f32>
) -> Result<()>

Update the filter with an observation of the position

pub fn observe_height(&mut self, measured: f32, variance: f32) -> Result<()>

Update the filter with an observation of the height alone

pub fn observe_velocity(
    &mut self,
    measurement: Vector3<f32>,
    variance: Matrix3<f32>
) -> Result<()>

Update the filter with an observation of the velocity

Note

If the observation comes from a sensor relative to the filter, e.g. an optical flow sensor that turns with the UAV, the sensor values needs to be rotated into the same frame as the filter, e.g. filter.orientation.transform_vector(&relative_measurement).

pub fn observe_velocity2d(
    &mut self,
    measurement: Vector2<f32>,
    variance: Matrix2<f32>
) -> Result<()>

Update the filter with an observation of the velocity in only the [X, Y] axis

Note

If the observation comes from a sensor relative to the filter, e.g. an optical flow sensor that turns with the UAV, the sensor values needs to be rotated into the same frame as the filter, e.g. filter.orientation.transform_vector(&relative_measurement).

pub fn observe_orientation(
    &mut self,
    measurement: UnitQuaternion<f32>,
    variance: Matrix3<f32>
) -> Result<()>

Update the filter with an observation of the orientation

Trait Implementations

impl Clone for ESKF

fn clone(&self) -> ESKF

Returns a copy of the value.

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

Performs copy-assignment from source.

impl Copy for ESKF

impl Debug for ESKF

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

Formats the value using the given formatter.