src: implement ego tracking models, and port shaders from renderbug into here

src/ego/alignment.rs

@@ -0,0 +1,56 @@
+use log::*;
+use nalgebra::{Matrix3, Rotation3, Unit, Vector3};
+
+type Sample = Vector3<f32>; // [ax, ay, az] in m/s^2
+
+#[derive(Debug, Clone, Copy)]
+pub struct SensorAlignment {
+    /// 3x3 transform matrix: raw -> body
+    /// includes permutation, sign, and bias subtraction.
+    pub transform: Rotation3<f32>,
+    /// bias vector in body frame
+    pub accel_bias: Vector3<f32>,
+
+    pub gyro_bias: Vector3<f32>,
+}
+
+impl SensorAlignment {
+    pub fn apply(&self, raw: &Sample) -> Vector3<f32> {
+        self.transform * raw - self.accel_bias
+    }
+
+    pub fn apply_gyro(&self, raw: &Sample) -> Vector3<f32> {
+        self.transform * raw - self.gyro_bias
+    }
+
+    pub fn new(accel_samples: &[Sample], gyro_samples: &[Sample]) -> SensorAlignment {
+        let avg: Vector3<f32> = accel_samples.iter().sum::<Vector3<f32>>() / (accel_samples.len() as f32);
+        let down_body = Unit::new_normalize(avg);
+        let down_world = Unit::new_normalize(Vector3::new(0.0, 0.0, -1.0));
+        let rot_down = Rotation3::rotation_between(&down_world, &down_body).unwrap_or_else(Rotation3::identity);
+        let forward_world = Vector3::x();
+
+        let forward_projected = (forward_world - forward_world.dot(&down_world) * down_world.into_inner()).normalize();
+
+        // Transform forward into the body frame
+        let forward_body = rot_down * forward_projected;
+
+        // Construct a proper body frame basis
+        let right_body = down_body.cross(&forward_body).normalize();
+        let forward_body = right_body.cross(&down_body).normalize();
+
+        // Construct rotation matrix from basis vectors
+        let rot = Matrix3::from_columns(&[forward_body, right_body, down_body.into_inner()]);
+        let transform = Rotation3::from_matrix_unchecked(rot);
+
+        let gyro_avg: Vector3<f32> = gyro_samples.iter().sum::<Vector3<f32>>() / (gyro_samples.len() as f32);
+
+        info!("down_body={down_body:?} down_world={down_world:?}");
+
+        Self {
+            transform,
+            accel_bias: transform * avg,
+            gyro_bias: transform * gyro_avg
+        }
+    }
+}

src/ego/heading.rs

@@ -0,0 +1,37 @@
+use core::f32::consts::PI;
+
+/// Normalize angle to [-PI, PI]
+fn wrap_angle(angle: f32) -> f32 {
+    let mut a = angle;
+    while a >  PI { a -= 2.0*PI; }
+    while a < -PI { a += 2.0*PI; }
+    a
+}
+
+/// Heading estimator using gyro yaw rate
+#[derive(Debug, Clone, Copy)]
+pub struct HeadingEstimator {
+    heading: f32, // radians
+}
+
+impl HeadingEstimator {
+    pub fn new(initial_heading: f32) -> Self {
+        Self { heading: wrap_angle(initial_heading) }
+    }
+
+    /// Integrate yaw rate (rad/s) over dt (s)
+    pub fn update(&mut self, yaw_rate: f32, dt: f32) {
+        self.heading = wrap_angle(self.heading + yaw_rate * dt);
+    }
+
+    /// Correct drift using an external absolute heading (e.g. magnetometer)
+    pub fn correct(&mut self, measured_heading: f32, alpha: f32) {
+        // alpha ∈ [0,1]: 0 = ignore measurement, 1 = full trust measurement
+        let err = wrap_angle(measured_heading - self.heading);
+        self.heading = wrap_angle(self.heading + alpha * err);
+    }
+
+    pub fn heading(&self) -> f32 {
+        self.heading
+    }
+}

src/ego/mod.rs

@@ -0,0 +1,3 @@
+pub mod alignment;
+pub mod tilt;
+pub mod heading;

src/ego/tilt.rs

@@ -0,0 +1,89 @@
+use core::f32::consts::PI;
+use nalgebra::ComplexField;
+use nalgebra::RealField;
+
+const G: f32 = 9.80665;
+
+/// Complementary filter state
+pub struct TiltEstimator {
+    roll: f32,   // phi
+    pitch: f32,  // theta
+    yaw: f32,    // psi (integrated gyro yaw, will drift)
+    alpha: f32,  // complementary weight for gyro integration (0..1)
+}
+
+impl TiltEstimator {
+    pub fn new(alpha: f32) -> Self {
+        Self { roll: 0.0, pitch: 0.0, yaw: 0.0, alpha }
+    }
+
+    /// dt in seconds, accel in m/s^2 body frame, gyro in rad/s body rates (gx, gy, gz)
+    /// body axes assumed: x forward, y right, z down
+    pub fn update(&mut self, dt: f32, accel: (f32, f32, f32), gyro: (f32, f32, f32)) {
+        let (ax, ay, az) = accel;
+        let (gx, gy, gz) = gyro;
+
+        // 1) accel-based tilt estimates (avoid NaN by small epsilon)
+        let eps = 1e-6;
+        let roll_acc = (ay).atan2(az.max(eps)); // phi = atan2(ay, az)
+        let pitch_acc = (-ax).atan2((ay*ay + az*az).sqrt().max(eps)); // theta = atan2(-ax, sqrt(ay^2+az^2))
+
+        // 2) integrate gyro for angles (body rates -> approx angle rates)
+        // Note: for small angles, roll_dot ≈ gx, pitch_dot ≈ gy, yaw_dot ≈ gz in body frame if axes aligned.
+        // For better accuracy you'd convert rates to inertial derivatives but this is common and simple.
+        let roll_from_gyro = self.roll + gx * dt;
+        let pitch_from_gyro = self.pitch + gy * dt;
+        let yaw_from_gyro = self.yaw + gz * dt;
+
+        // 3) complementary combine
+        let alpha = self.alpha;
+        self.roll  = alpha * roll_from_gyro  + (1.0 - alpha) * roll_acc;
+        self.pitch = alpha * pitch_from_gyro + (1.0 - alpha) * pitch_acc;
+        self.yaw   = yaw_from_gyro; // we typically don't correct yaw with accel (no observability)
+
+        // normalize yaw to -pi..pi
+        if self.yaw > PI { self.yaw -= 2.0*PI; }
+        if self.yaw < -PI { self.yaw += 2.0*PI; }
+    }
+
+    pub fn angles(&self) -> (f32, f32, f32) {
+        (self.roll, self.pitch, self.yaw)
+    }
+}
+
+/// Rotate body accel into ENU and remove gravity; returns (east, north, up) linear accel in m/s^2
+pub fn accel_body_to_enu_lin(body_acc: (f32,f32,f32), roll: f32, pitch: f32, yaw: f32) -> (f32,f32,f32) {
+    let (ax, ay, az) = body_acc;
+
+    let (sr, cr) = roll.sin_cos();
+    let (sp, cp) = pitch.sin_cos();
+    let (sy, cy) = yaw.sin_cos();
+
+    // Build R = Rz(yaw) * Ry(pitch) * Rx(roll)
+    // multiply R * a_body
+    let r11 =  cy*cp;
+    let r12 =  cy*sp*sr - sy*cr;
+    let r13 =  cy*sp*cr + sy*sr;
+
+    let r21 =  sy*cp;
+    let r22 =  sy*sp*sr + cy*cr;
+    let r23 =  sy*sp*cr - cy*sr;
+
+    let r31 = -sp;
+    let r32 =  cp*sr;
+    let r33 =  cp*cr;
+
+    // a_nav = R * a_body
+    let ax_nav = r11*ax + r12*ay + r13*az; // east
+    let ay_nav = r21*ax + r22*ay + r23*az; // north
+    let az_nav = r31*ax + r32*ay + r33*az; // up
+
+    // remove gravity (nav frame gravity points down in NED; in our ENU 'up' axis positive up)
+    // If accel measured gravity as +9.8 on sensor when static and z down convention used above,
+    // then az_nav will include +g in 'up' direction; subtract G.
+    let ax_lin = ax_nav;
+    let ay_lin = ay_nav;
+    let az_lin = az_nav - G; // linear acceleration in up axis
+
+    (ax_lin, ay_lin, az_lin)
+}