Struct KalmanFilterXYZ 

pub struct KalmanFilterXYZ {
    pub state: KalmanStateXYZ,
    pub P: Matrix3x3xM3x3f32,
    pub Q_velocity: f32,
    pub Q_bias: f32,
}

Linear Kalman Filter in 3 dimensions.

The system is split into two cleanly decoupled steps. This:

1. reduces the covariance matrix from 15x15 to 9x9.
2. allows a Linear Kalman Filter (rather than an Extended Kalman Filter) to be used.

  ┌─────┐  Acc/Gyro  ┌─────────────────┐
  │ IMU ├──────────► │ MADGWICK FILTER ├──────► Orientation Quaternion
  └─────┘            └─────────────────┘
 Acc │────────────────────────────────────────► Acc
     │            ┌────────────────────────┐
     └──────────► │ POSITION KALMAN FILTER ├──► Position Vector
                  └────────────────────────┘    Velocity Vector
                               │
  GPS, Barometer etc ─────►────┘

Fields

state: KalmanStateXYZ

3D Kinematic State Vectors.

P: Matrix3x3xM3x3f32

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 KalmanFilterXYZ

pub fn new() -> Self

Constructor.

impl KalmanFilterXYZ

pub fn pos(&self) -> Vector3f32

pub fn vel(&self) -> Vector3f32

pub fn acc_bias(&self) -> Vector3f32

impl KalmanFilterXYZ

pub fn predict_state(&mut self, acc_measurement: Vector3f32, 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.

A full 9x9 matrix multiplication involves 729 individual arithmetic operations. Instead the matrix is divided into 9 separate 3x3 sub-matrices (blocks), and each block is processed separately.

┌                 9x9                  ┐
│ ┌   3x3    ┐┌   3x3    ┐┌   3x3    ┐ │
│ │ Position ││ Position ││ Position │ │
│ │ Position ││ Velocity ││ Bias     │ │
│ └          ┘└          ┘└          ┘ │
│ ┌   3x3    ┐┌   3x3    ┐┌   3x3    ┐ │
│ │ Velocity ││ Velocity ││ Velocity │ │
│ │ Position ││ Velocity ││ Bias     │ │
│ └          ┘└          ┘└          ┘ │
│ ┌   3x3    ┐┌   3x3    ┐┌   3x3    ┐ │
│ │ Bias     ││ Bias     ││ Bias     │ │
│ │ Position ││ Velocity ││ Bias     │ │
│ └          ┘└          ┘└          ┘ │
└                                      ┘

In a Kalman filter tracking Position (P), Velocity (V), and an Accelerometer Bias (B)
over a small time increment (dt), the physical laws of motion give:
    P_new = P + V * dt
    V_new = V - B * dt  (Bias is subtracted from acceleration)
    B_new = B (Bias is modeled as a constant or random walk)

Expressed as a 3x3 block state transition matrix, that gives:

    ┌                 9x9                  ┐
    │ ┌   3x3    ┐┌   3x3    ┐┌   3x3    ┐ │
    │ │    I     ││   dt*I   ││    0     │ │
    │ └          ┘└          ┘└          ┘ │
    │ ┌   3x3    ┐┌   3x3    ┐┌   3x3    ┐ │
F = │ │    0     ││    I     ││  -dt*I   │ │
    │ └          ┘└          ┘└          ┘ │
    │ ┌   3x3    ┐┌   3x3    ┐┌   3x3    ┐ │
    │ │    0     ││    0     ││    I     │ │
    │ └          ┘└          ┘└          ┘ │
    └                                      ┘

Since this is a sparse matrix, we can calculate F * P * Fᵀ without doing full 9x9 matrix multiplications.
Instead we perform the multiplication block by block.

Expanding F * P * Fᵀ algebraically gives the following block updates:

For the position columns (PP, VP, BP)
    PP_new = PP + dt * (VP + PV) + dt^2 * VV
    VP_new = VP + dt * VV - dt * BP - dt^2 * BV
    BP_new = BP + dt * BV
For the velocity columns (PV, VV, BV)
    PV_new = PV + dt * VV - dt * PB - dt^2 * VB
    VV_new = VV - dt * (BV + VB) + dt^2 * BB
    BV_new = BV - dt * BB
For the bias columns (PB, VB, BB)
    PB_new = PB + dt * VB
    VB_new = VB - dt * BB
    BB_new = BB

impl KalmanFilterXYZ

pub fn correct_altitude(&mut self, altitude_measurement: f32, R: f32)

Phase 2 Altitude Correction using new measurement. Updates only the vertical Z axis components across all tracking states.

Core Operations

• S = P₂₂ + R (Innovation Variance calculation)
• K = P_column_2 * (1.0 / S) (Kalman Gain column selection extraction)
• P = P - K * (H * P) (Covariance correction step)

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

Executes an asynchronous measurement update when a new 3D GPS reading arrives (typically at a slower 1Hz to 10Hz rate). The error becomes a 3D vector, and the 3D Position, Velocity, and Accelerometer Bias states.

Core Operations

• Extracts PositionPosition sub-block from P (top left 3x3 matrix).
• S = H * P * Hᵀ + R_gps (yields a 3x3 innovation matrix)
• S_inv = try_inverse(S)
• K = (P * Hᵀ) * S_inv (yields a 9x3 block matrix representation)

Performs a full 3D Position Correction (e.g., GPS or Optical Flow position packet). Observations sample the full x, y, and z position channels simultaneously.

Note: we assume the measurement errors are not cross-correlated (that is the x, y, and z sensor noises are independent), This means the 3x3 noise covariance matrix R is diagonal and so can be represented by a 3d vector.

Layouts:

• self.P: 9x9 Column-Major Covariance Matrix
• position: Vector3f32 observation [x, y, z]
• R: Vector3f32 diagonal measurement noise variance [R.x, R.y, R.z]

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

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_xy_standard_form( &mut self, position: Vector2f32, past_pos: Vector3f32, R: Vector2f32, )

pub fn correct_position_using_anchor_distance( &mut self, anchor_position: Vector3f32, measured_distance: f32, mahalanobis_squared_threshold: f32, R: f32, )

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

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

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

impl KalmanFilterXYZ

pub fn validate_measurement( &self, y: Vector3f32, R: Vector3f32, gate_threshold: f32, ) -> bool

Evaluates if an incoming innovation residual vector satisfies chi-squared gating thresholds. Formula: d² = yᵀ * S⁻¹ * y.

Layouts:

• self.P: 9x9 Column-Major Covariance Matrix (used to extract the 3x3 S matrix block)
• y: 3-element measurement innovation residual [y_x, y_y, y_z]
• R: 3-element diagonal measurement noise variance array [R_x, R_y, R_z]
• gate_threshold: Chi-squared limit (e.g., 7.815 for 3 DOF at 95% confidence)

Trait Implementations

impl Clone for KalmanFilterXYZ

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 KalmanFilterXYZ

impl Debug for KalmanFilterXYZ

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

Formats the value using the given formatter.

impl Default for KalmanFilterXYZ

fn default() -> Self

Returns the “default value” for a type.

impl PartialEq for KalmanFilterXYZ

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

Equality operator ==.

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

Inequality operator !=.

impl StructuralPartialEq for KalmanFilterXYZ