sensor_fusion/
kalman_state_vector9.rs

1use core::ops::Mul;
2
3use num_traits::Zero;
4
5use vqm::{Matrix9x9, Matrix9x9Math, Vector3d};
6
7/// Kalman state vector of `f32` values<br>
8pub type KalmanStateVector9f32 = KalmanStateVector9<f32>;
9/// Kalman tate vector of `f64` values<br><br>
10pub type KalmanStateVector9f64 = KalmanStateVector9<f64>;
11
12/// Flattened representation of a 9-element state vector for Kalman filter matrix math.<br><br>
13#[derive(Clone, Copy, Debug, Default, PartialEq)]
14#[repr(C, align(64))]
15#[allow(missing_docs)]
16pub struct KalmanStateVector9<T> {
17    pub pos: Vector3d<T>,
18    pub vel: Vector3d<T>,
19    pub bias: Vector3d<T>,
20}
21
22impl<T> KalmanStateVector9<T>
23where
24    T: Copy,
25{
26    /// Constructor.
27    #[inline]
28    pub const fn new(v: (Vector3d<T>, Vector3d<T>, Vector3d<T>)) -> Self {
29        Self { pos: v.0, vel: v.1, bias: v.2 }
30    }
31}
32
33impl<T> From<(Vector3d<T>, Vector3d<T>, Vector3d<T>)> for KalmanStateVector9<T> {
34    #[inline]
35    fn from(v: (Vector3d<T>, Vector3d<T>, Vector3d<T>)) -> Self {
36        Self { pos: v.0, vel: v.1, bias: v.2 }
37    }
38}
39
40/// Implement vector-by-scalar multiplication to scale the Kalman Gain.
41impl Mul<f32> for KalmanStateVector9<f32> {
42    type Output = Self;
43
44    #[inline]
45    fn mul(self, rhs: f32) -> Self::Output {
46        Self { pos: self.pos * rhs, vel: self.vel * rhs, bias: self.bias * rhs }
47    }
48}
49
50impl Mul<f64> for KalmanStateVector9<f64> {
51    type Output = Self;
52
53    #[inline]
54    fn mul(self, rhs: f64) -> Self::Output {
55        Self { pos: self.pos * rhs, vel: self.vel * rhs, bias: self.bias * rhs }
56    }
57}
58
59impl<T> KalmanStateVector9<T>
60where
61    T: Copy + Zero + Matrix9x9Math + Mul<T, Output = T> + PartialEq,
62{
63    /// Calculates the outer product of two 9-element states in a compiler-friendly manner.
64    #[inline]
65    pub fn outer_product(self, row: KalmanStateVector9<T>) -> Matrix9x9<T> {
66        Self::outer_product_associated(self, row)
67    }
68
69    /// Calculates the outer product of two 9-element states in a compiler-friendly manner.
70    #[inline]
71    fn outer_product_associated(col: KalmanStateVector9<T>, row: KalmanStateVector9<T>) -> Matrix9x9<T> {
72        // Convert the 9 elements into three 4-lane logically padded chunks.
73        // The 4th element is padding to fulfill 128-bit SIMD constraints.
74        let r1 = [row.pos.x, row.pos.y, row.pos.z, T::zero()];
75        let r2 = [row.vel.x, row.vel.y, row.vel.z, T::zero()];
76        let r3 = [row.bias.x, row.bias.y, row.bias.z, T::zero()];
77
78        // Flatten `col` into an array  easily indexable via registers
79        let c = [col.pos.x, col.pos.y, col.pos.z, col.vel.x, col.vel.y, col.vel.z, col.bias.x, col.bias.y, col.bias.z];
80
81        let mut ret = <Matrix9x9<T>>::zero();
82
83        // Process each row. LLVM sees fixed loops of 4 elements and emits optimized parallel hardware vector instructions.
84        for (r_idx, &scalar) in c.iter().enumerate().take(9) {
85            // array::map creates a clean, fixed-size loop that LLVM easily unrolls
86            // and vectorizes since the size (4) and operation are perfectly predictable.
87            let chunk1 = r1.map(|val| scalar * val);
88            let chunk2 = r2.map(|val| scalar * val);
89            let chunk3 = r3.map(|val| scalar * val);
90
91            // Get a mutable reference to the row (9 elements)
92            let ret_row = &mut ret[r_idx * 9..(r_idx + 1) * 9];
93
94            // Write back to the matrix, dropping the 4th padding lane of each chunk.
95            // Since the sizes are matched the compiler can generate raw, grouped memory stream instructions.
96            ret_row[0..3].copy_from_slice(&chunk1[0..3]);
97            ret_row[3..6].copy_from_slice(&chunk2[0..3]);
98            ret_row[6..9].copy_from_slice(&chunk3[0..3]);
99        }
100
101        ret
102    }
103}
104
105#[cfg(test)]
106mod tests {
107    use super::*;
108    fn _is_normal<T: Sized + Send + Sync + Unpin>() {}
109    fn is_full<T: Sized + Send + Sync + Unpin + Copy + Clone + Default + PartialEq>() {}
110
111    #[test]
112    fn normal_types() {
113        is_full::<KalmanStateVector9f32>();
114    }
115}
sensor_fusion/kalman_state_vector9.rs

sensor_fusion/
kalman_state_vector9.rs
