Struct KalmanState 

pub struct KalmanState {
    pub timestamp: f64,
    pub position_m: [f64; 3],
    pub velocity_ms: [f64; 3],
    pub quaternion: [f64; 4],
    pub accel_bias: [f64; 3],
    pub gyro_bias: [f64; 3],
    pub cov_diagonal: [f64; 15],
}

The fused navigation state from a Kalman filter.

State vector: position (3), velocity (3), orientation quaternion (4), accelerometer bias (3), gyroscope bias (3) → total 16 scalars. The covariance diagonal (15 terms) is stored separately.

Fields

timestamp: f64

Timestamp in seconds.

position_m: [f64; 3]

Position [x, y, z] in metres (ECEF or local NED depending on frame).

velocity_ms: [f64; 3]

Velocity [vx, vy, vz] in m/s.

quaternion: [f64; 4]

Orientation quaternion [qw, qx, qy, qz] (unit quaternion).

accel_bias: [f64; 3]

Accelerometer bias estimate [bax, bay, baz] in m/s².

gyro_bias: [f64; 3]

Gyroscope bias estimate [bgx, bgy, bgz] in rad/s.

cov_diagonal: [f64; 15]

Diagonal of the 15×15 error covariance matrix.

Implementations

impl KalmanState

pub fn identity(timestamp: f64) -> Self

Create a new Kalman state with identity quaternion and zero everything else.

pub fn quaternion_is_unit(&self) -> bool

Check that the quaternion is (approximately) normalised.

pub fn roll_rad(&self) -> f64

Roll angle in radians extracted from the quaternion.

pub fn pitch_rad(&self) -> f64

Pitch angle in radians extracted from the quaternion.

pub fn yaw_rad(&self) -> f64

Yaw angle in radians extracted from the quaternion.

pub fn to_bytes(&self) -> Vec<u8>

Serialize to bytes (timestamp 8B + 3+3+4+3+3 f64s + 15 cov f64s = 8+95×8 = 768B).

pub fn from_bytes(data: &[u8]) -> Option<Self>

Deserialize from bytes.

Trait Implementations

impl Clone for KalmanState

fn clone(&self) -> KalmanState

Returns a duplicate of the value.

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

Performs copy-assignment from source.

impl Debug for KalmanState

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

Formats the value using the given formatter.