Struct KalmanFilterXY 

pub struct KalmanFilterXY {
    pub pos: Vector2f32,
    pub vel: Vector2f32,
    pub acc_bias: Vector2f32,
    pub P: Matrix3x3xM2x2f32,
    pub Q_velocity: f32,
    pub Q_bias: f32,
}

Linear Kalman Filter in 2 dimensions.

Fields

pos: Vector2f32

Position (x, y).

vel: Vector2f32

Velocity (x, y).

acc_bias: Vector2f32

Accelerometer Bias (x, y).

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_true * dT²
vel_k = vel_k₋₁ + acc_true * dT

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

Predict covariance:

P = F * P * F^T + Q.

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

impl KalmanFilterXY

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

Covariance correction step.

S = H * P * H^T + R K = P * H^T * S^-1 P = (I - K * H) * P.

R is measurement noise covariance.

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

Joseph’s Stabilized Form for the covariance update step:

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

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

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: &KalmanFilterXY) -> bool

Equality operator ==.

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

Inequality operator !=.

impl StructuralPartialEq for KalmanFilterXY