Struct PositionKalmanFilter 

pub struct PositionKalmanFilter {
    pub pos: Vector3df32,
    pub vel: Vector3df32,
    pub acc_bias: Vector3df32,
    pub P: Matrix9x9f32,
    pub E: Matrix9x9f32,
    pub q_velocity: f32,
    pub q_bias: f32,
    pub r_gps_horizontal: f32,
    pub r_gps_vertical: f32,
    pub r_barometer: f32,
    pub r_rangefinder: f32,
    pub r_optical_flow: Vector3df32,
}

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

1. avoids managing a massive 15x15 state matrix.
2. linearizes the attitude so a Kalman Filter (rather than an Extended Kalman Filter) can be used.

  ┌──────────────┐
  │ IMU Acc/Gyro ├──► [ 1. ATTITUDE (MADGWICK) FILTER ] ──► Attitude Quaternion
  └──────────────┘                │
                                  ▼
  ┌───────────┐       [    Transform Body ]
  │ IMU Acc   ├─────► [ 2. Acceleration   ] ──► [ 3. POSITION KALMAN FILTER ]
  └───────────┘       [    to Earth Frame ]                  ▲
                                                             │
                       GPS & Barometer Measurements ─────────┘

Fields

pos: Vector3df32

Position (x, y, z).

vel: Vector3df32

Velocity (x, y, z).

acc_bias: Vector3df32

Accelerometer Bias (x, y, z).

P: Matrix9x9f32

Predicted System Uncertainty Covariance Matrix (P).

E: Matrix9x9f32

Estimated Post-Correction Error Covariance Matrix (E).

q_velocity: f32

Process Noise spectral density mapping to Velocity variance.

q_bias: f32

Process Noise spectral density mapping to Sensor Drift variance.

r_gps_horizontal: f32

Absolute Measurement Noise variance for horizontal GPS channels.

r_gps_vertical: f32

Absolute Measurement Noise variance for vertical GPS channels.

r_barometer: f32

Absolute Measurement Noise variance for barometric pressure altimeter.

r_rangefinder: f32

Absolute Measurement Noise variance for rangefinder.

r_optical_flow: Vector3df32

Absolute Measurement Noise variance for optical flow.

Implementations

impl PositionKalmanFilter

pub const Z_POS_ROW: usize = 2

pub const Z_POS_COL: usize = 2

pub const S_XX: usize = Matrix3x3f32::M11

pub const S_YY: usize = Matrix3x3f32::M22

pub const S_ZZ: usize = Matrix3x3f32::M33

impl PositionKalmanFilter

pub const fn new() -> Self

Constructor.

impl PositionKalmanFilter

pub fn predict_states(&mut self, acc_measurement: Vector3df32, 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)

Propagates the 9x9 covariance matrix forward in time.

A full 9x9 matrix multiplication involves 729 individual matrix multiplications. 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     │ │
│ └          ┘└          ┘└          ┘ │
└                                      ┘

Formula

• P_k = A * E_k₋₁ * Aᵀ + Q

impl PositionKalmanFilter

pub fn correct_altitude(&mut self, altitude: 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)
• E = P - K * H * P (Covariance correction step)

pub fn correct_altitude_using_barometer(&mut self, altitude: f32)

Phase 2: Correct altitude using the barometer measurement.

pub fn correct_altitude_using_rangefinder(&mut self, altitude: f32)

Phase 2: Correct altitude using the rangefinder measurement.

pub fn correct_altitude_using_gps(&mut self, altitude: f32)

Phase 2: Correct altitude using GPS vertical measurement.

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

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)

pub fn correct_position_using_gps(&mut self, position: Vector3df32)

Phase 2: Correct altitude using GPS position measurement.

pub fn correct_position_using_optical_flow(&mut self, position: Vector3df32)

Phase 2: Correct altitude using optical flow position measurement.

Trait Implementations

impl Clone for PositionKalmanFilter

fn clone(&self) -> PositionKalmanFilter

Returns a duplicate of the value.

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

Performs copy-assignment from source.

impl Copy for PositionKalmanFilter

impl Debug for PositionKalmanFilter

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

Formats the value using the given formatter.

impl Default for PositionKalmanFilter

fn default() -> Self

Returns the “default value” for a type.

impl PartialEq for PositionKalmanFilter

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

Tests for self and other values to be equal, and is used by ==.

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

Tests for !=. The default implementation is almost always sufficient, and should not be overridden without very good reason.

impl StructuralPartialEq for PositionKalmanFilter