Struct KalmanFilterXY 

pub struct KalmanFilterXY {
    pub state: KalmanStateXY,
    pub P: Matrix3x3xM2x2f32,
    pub Q_velocity: f32,
    pub Q_bias: f32,
}

Linear Kalman Filter in 2 dimensions.

Fields

state: KalmanStateXY

2D Kinematic State Vectors.

P: Matrix3x3xM2x2f32

P: Prediction error covariance (aka state covariance matrix, the system’s internal uncertainty).

Q_velocity: f32

Process Noise spectral density mapping to Velocity variance.

Q_bias: f32

Process Noise spectral density mapping to Sensor Drift variance.

Implementations

impl KalmanFilterXY

pub fn new() -> Self

Constructor.

impl KalmanFilterXY

pub fn pos(&self) -> Vector2f32

pub fn vel(&self) -> Vector2f32

pub fn acc_bias(&self) -> Vector2f32

impl KalmanFilterXY

pub fn predict_state(&mut self, acc_measurement: Vector2f32, dt: f32)

Integrates raw IMU accelerometer data to predict new position and velocity vectors.

This forms the continuous dead reckoning pipeline driving vehicle kinetics forward.

Physical Mechanics

pos_k = pos_k₋₁ + vel_k₋₁ * dt + 0.5 * acc * dt²
vel_k = vel_k₋₁ + acc * dt

pub fn predict_covariance(&mut self, dt: f32)

Predict covariance:

P = F * P * Fᵀ + Q.

P: State covariance matrix F: State transition matrix Q: Process noise covariance.

impl KalmanFilterXY

pub fn correct_position_standard_form( &mut self, measurement: Vector2f32, predicted_measurement: Vector2f32, R: Vector2f32, )

Covariance correction step.

S = H * P * Hᵀ + R K = P * Hᵀ * S⁻¹ P = (I - K * H) * P.

R is measurement noise covariance.

pub fn correct_position_joseph_stabilized_form( &mut self, measurement: Vector2f32, predicted_measurement: Vector2f32, R: Vector2f32, )

Joseph’s Stabilized Form for the covariance update step:

P{k} = (I - KH) * P{k-1} * (I - KH)ᵀ + KRKᵀ).

While computationally more expensive, it guarantees the result remains positive-definite. That is it ensures the covariance matrix has positive eigenvalues and remains valid and invertible for future updates.

pub fn correct_position(&mut self, position: Vector2f32, R: Vector2f32)

pub fn correct_position_joseph(&mut self, position: Vector2f32, R: Vector2f32)

Trait Implementations

impl Clone for KalmanFilterXY

fn clone(&self) -> Self

Returns a duplicate of the value.

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

Performs copy-assignment from source.

impl Copy for KalmanFilterXY

impl Debug for KalmanFilterXY

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

Formats the value using the given formatter.

impl Default for KalmanFilterXY

fn default() -> Self

Returns the “default value” for a type.

impl PartialEq for KalmanFilterXY

fn eq(&self, other: &Self) -> bool

Equality operator ==.

fn ne(&self, other: &Rhs) -> bool

Inequality operator !=.

impl StructuralPartialEq for KalmanFilterXY