bump a lot of big changes I dont want to break down into individual commits

src/ego/engine.rs

@@ -0,0 +1,264 @@
+use embassy_sync::channel::DynamicSender;
+use embassy_time::{Duration, Instant};
+use nalgebra::{Rotation3, Vector2, Vector3};
+use log::*;
+//use micromath::F32Ext;
+use nalgebra::{ComplexField, RealField};
+
+use core::fmt::Debug;
+
+use crate::{ego::{heading::HeadingEstimator, kalman::Ekf2D, orientation::OrientationEstimator}, events::{Notification, Prediction}, Breaker, CircularBuffer, idle::IdleClock};
+
+#[derive(PartialEq, Debug, Default, Clone, Copy)]
+pub enum MotionState {
+    #[default]
+    Stationary,
+    Steady,
+    Accelerating,
+    Decelerating
+}
+
+pub struct BikeStates {
+    /// Prediction models
+    orientation: OrientationEstimator,
+    heading: HeadingEstimator,
+    speedo: CircularBuffer<f32, 300>,
+    kf: Ekf2D,
+    
+    last_stamp: Instant,
+    last_fix: Vector2<f64>, // The most recent GPS value
+    reference_fix: Option<Vector2<f64>>, // The first GPS value, which is used to make sense out of the EKF output which is in meters offset from this point
+
+    // State switches
+    is_calibrated: Breaker<bool>,
+    motion_state: Breaker<MotionState>,
+    // FIXME: pub
+    pub has_gps_fix: Breaker<bool>,
+    predicted_velocity: Breaker<f32>,
+    reported_velocity: Breaker<f32>,
+    predicted_location: Breaker<Vector2<f64>>,
+    wake_requested: Breaker<bool>,
+    steady_timer: IdleClock
+}
+
+impl Debug for BikeStates {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        f.debug_struct("BikeStates")
+            .field("has_down", &self.orientation.has_down())
+            .field("has_orientation", &self.orientation.is_ready())
+            .field("heading", &self.heading.heading())
+            .field("motion_state", &self.motion_state)
+            .field("predicted_location", &self.predicted_location)
+            .field("predicted_velocity", &self.predicted_velocity)
+            .field("wake_requested", &self.wake_requested)
+            .finish()
+    }
+}
+
+impl BikeStates {
+    pub fn insert_gps(&mut self, gps_pos: Vector2<f64>) {
+        match self.reference_fix {
+            None => {
+                self.reference_fix = Some(gps_pos);
+                self.last_fix = gps_pos;
+                self.has_gps_fix.set(true);
+            },
+            Some(coords) => {
+                if self.last_fix != coords {
+                    let gps_heading = self.last_fix.angle(&coords);
+                    self.heading.correct(gps_heading as f32, 0.9);
+                }
+
+                let delta = gps_to_local_meters_haversine(&coords, &gps_pos);
+
+                self.kf.update_gps(delta.x, delta.y);
+
+                self.last_fix = coords;
+            }
+        }
+    }
+
+    pub fn insert_imu(&mut self, accel: Vector3<f32>, gyro: Vector3<f32>) {
+        self.orientation.insert(accel);
+
+        if self.orientation.has_down() {
+            let body_accel = self.orientation.apply(accel);
+            let body_gyro = self.orientation.apply(gyro);
+
+            let cur_stamp = Instant::now();
+            let dt = (cur_stamp - self.last_stamp).as_millis() as f32 / 1000.0;
+            self.last_stamp = cur_stamp;
+
+            self.heading.update(body_gyro.z, dt);
+            let heading_rotation = Rotation3::from_axis_angle(&Vector3::z_axis(), self.heading.heading());
+
+            let enu_rotated = heading_rotation * body_accel;
+            if accel.xy().magnitude() >= 0.8 {
+                self.kf.predict(enu_rotated.xy(), body_gyro.z, dt);
+            } else {
+                // Otherwise, we are standing stationary and should insert accel=0 data into the model
+                self.kf.update_zupt();
+            }
+
+            self.is_calibrated.set(true);
+        }
+    }
+
+    pub async fn commit(&mut self, predictions: &DynamicSender<'static, Prediction>, notifications: &DynamicSender<'static, Notification>) {
+        if let Some(true) = self.is_calibrated.read_tripped() {
+            notifications.send(Notification::SensorOnline(crate::events::SensorSource::IMU)).await
+        }
+
+        match self.has_gps_fix.read_tripped() {
+            None => (),
+            Some(true) => notifications.send(Notification::SensorOnline(crate::events::SensorSource::GPS)).await,
+            Some(false) => notifications.send(Notification::SensorOffline(crate::events::SensorSource::GPS)).await,
+        }
+
+        let est = self.kf.x;
+
+        let position = self.reference_fix.as_ref().map(|start_coords| local_to_gps(*start_coords, Vector2::new(est.x as f64, est.y as f64)));
+        let velocity = Vector2::new(est[2], est[3]);
+
+        // If we have a new location, update the predictions
+        if let Some(pos) = position {
+            self.predicted_location.set(pos);
+            if let Some(pos) = self.predicted_location.read_tripped() {
+                predictions.send(Prediction::Location(pos)).await;
+            }
+        }
+
+        // If we have a new velocity, update the acceleration status
+        self.predicted_velocity.set(velocity.norm());
+        if let Some(v) = self.predicted_velocity.read_tripped() {
+            self.wake_requested.set(true);
+            if self.wake_requested.read_tripped().is_some() {
+                predictions.send(Prediction::WakeRequested).await;
+            }
+            self.speedo.insert(v);
+            if self.speedo.is_filled() {
+                let threshold = 1.0;
+                let trend = self.speedo.data().windows(2).map(|n| {
+                    n[1] - n[0]
+                }).sum::<f32>();
+                let mean = self.speedo.mean();
+
+                // Reported velocity is kept only to the first decimal
+                self.reported_velocity.set((mean *10.0).round() / 10.0);
+                if let Some(v) = self.reported_velocity.read_tripped() {
+                    predictions.send(Prediction::Velocity(v)).await;
+                }
+
+                // If the total slope is more upwards than not, we are accelerating.
+                if trend >= threshold {
+                    self.motion_state.set(MotionState::Accelerating);
+                    self.steady_timer.wake();
+                } else if trend <= -threshold {
+                    self.steady_timer.wake();
+                    self.motion_state.set(MotionState::Decelerating);
+                } else if self.steady_timer.check() && mean > 1.0 {
+                    // If we haven't changed our acceleration for a while, and we still have speed, we are moving at a steady pace
+                    self.motion_state.set(MotionState::Steady);
+                } else if v <= 1.0 && mean <= 1.0 {
+                    // If the average and instantaneous speed is rather low, we are probably stationary!
+                    self.motion_state.set(MotionState::Stationary);
+                }
+
+                // And if the motion status has changed, send it out
+                if let Some(state) = self.motion_state.read_tripped() {
+                    debug!("state={state:?} trend={trend} mean={mean} v={v}");
+                    predictions.send(Prediction::Motion(state)).await
+                }
+            }
+        } else {
+            self.wake_requested.set(false);
+        }
+    }
+}
+
+impl Default for BikeStates {
+    fn default() -> Self {
+        Self {
+            motion_state: Default::default(),
+            orientation: Default::default(),
+            last_stamp: Instant::now(),
+            speedo: Default::default(),
+            heading: Default::default(),
+            is_calibrated: Default::default(),
+            kf: Default::default(),
+            steady_timer: IdleClock::new(Duration::from_secs(3)),
+            last_fix: Default::default(),
+            reference_fix: Default::default(),
+            has_gps_fix: Default::default(),
+            predicted_location: Default::default(),
+            predicted_velocity: Default::default(),
+            reported_velocity: Default::default(),
+            wake_requested: Default::default()
+        }
+    }
+}
+
+/// Compute displacement vector (east, north) in meters between two GPS coordinates
+/// Input: Vector2::new(lat_deg, lon_deg)
+pub fn gps_to_local_meters_haversine(a: &Vector2<f64>, b: &Vector2<f64>) -> Vector2<f32> {
+    let r = 6_371_000.0; // Earth radius in meters
+
+    // Convert to radians
+    let lat1 = a.x.to_radians();
+    let lon1 = a.y.to_radians();
+    let lat2 = b.x.to_radians();
+    let lon2 = b.y.to_radians();
+
+    // Differences
+    let dlat = lat2 - lat1;
+    let dlon = lon2 - lon1;
+
+    // Haversine formula for great-circle distance
+    let hav = (dlat / 2.0).sin().powi(2)
+        + lat1.cos() * lat2.cos() * (dlon / 2.0).sin().powi(2);
+    let c = 2.0 * hav.sqrt().atan2((1.0 - hav).sqrt());
+    let distance = r * c;
+
+    // Bearing from point A to B
+    let y = dlon.sin() * lat2.cos();
+    let x = lat1.cos() * lat2.sin() - lat1.sin() * lat2.cos() * dlon.cos();
+    let bearing = y.atan2(x); // radians, from north, clockwise
+
+    // Convert to local east/north vector
+    let dx = distance * bearing.sin(); // east
+    let dy = distance * bearing.cos(); // north
+
+    Vector2::new(dx as f32, dy as f32)
+}
+
+/// Convert a local displacement (east, north) in meters back to GPS coordinates.
+/// Input:
+///   ref_gps = Vector2::new(lat_deg, lon_deg)
+///   disp    = Vector2::new(dx_east_m, dy_north_m)
+/// Output:
+///   Vector2::new(lat_deg, lon_deg)
+pub fn local_to_gps(ref_gps: Vector2<f64>, disp: Vector2<f64>) -> Vector2<f64> {
+    let r = 6_371_000.0; // Earth radius in meters
+
+    let lat0 = ref_gps.x.to_radians();
+    let lon0 = ref_gps.y.to_radians();
+
+    // Displacement in meters
+    let dx = disp.x; // east
+    let dy = disp.y; // north
+
+    // Distance and bearing
+    let distance = (dx.powi(2) + dy.powi(2)).sqrt();
+    let bearing = dx.atan2(dy); // atan2(east, north)
+
+    // Destination point on sphere
+    let lat = (lat0.sin() * (distance / r).cos()
+        + lat0.cos() * (distance / r).sin() * bearing.cos())
+        .asin();
+
+    let lon = lon0
+        + (bearing.sin() * (distance / r).sin() * lat0.cos())
+            .atan2((distance / r).cos() - lat0.sin() * lat.sin());
+
+    Vector2::new(lat.to_degrees(), lon.to_degrees())
+}

src/ego/heading.rs

@@ -9,16 +9,12 @@ fn wrap_angle(angle: f32) -> f32 {
 }
 
 /// Heading estimator using gyro yaw rate
-#[derive(Debug, Clone, Copy)]
+#[derive(Debug, Clone, Copy, Default)]
 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);

src/ego/kalman.rs

@@ -1,87 +1,189 @@
-#![allow(non_snake_case)] // TODO, because most of this code came from chatgpt and I'm still not sure if it actually works or is completely wrong
-use nalgebra::{SMatrix, SVector};
+#![allow(non_snake_case)]
+use nalgebra::{Matrix2, Matrix5, Vector2, Vector5, Matrix2x5};
+use core::f32::consts::PI;
+use nalgebra::ComplexField;
 
-/// State: [px, py, vx, vy]
-pub type State = SVector<f32, 6>;
-pub type Control = SVector<f32, 3>; // dx, dx, yaw
-pub type GpsMeasurement = SVector<f32, 2>; // gps x, gps y
-
-pub struct Kalman {
-    x: State,                          // state estimate
-    P: SMatrix<f32, 6, 6>,             // covariance
-    F: SMatrix<f32, 6, 6>,             // state transition
-    B: SMatrix<f32, 6, 3>,             // control matrix
-    H: SMatrix<f32, 2, 6>,             // measurement matrix
-    Q: SMatrix<f32, 6, 6>,             // process noise
-    R: SMatrix<f32, 2, 2>,             // measurement noise
+/// 5-state EKF for planar vehicle: [px, py, vx, vy, theta]
+pub struct Ekf2D {
+    pub x: Vector5<f32>,   // state
+    pub P: Matrix5<f32>,   // covariance
+    pub Q: Matrix5<f32>,   // process noise
+    pub R_gps: Matrix2<f32>, // GPS measurement noise (px,py)
 }
 
-impl Kalman {
-    pub fn new(dt: f32) -> Self {
-        let mut kf = Kalman {
-            x: State::zeros(),
-            P: SMatrix::identity(),
-            F: SMatrix::identity(),
-            B: SMatrix::zeros(),
-            H: SMatrix::zeros(),
-            Q: SMatrix::identity() * 0.1,
-            R: SMatrix::identity() * 3.0, // GPS noise
-        };
-
-        // F (state transition)
-        kf.F[(0,2)] = dt;
-        kf.F[(1,3)] = dt;
-        kf.F[(4,5)] = dt;
-
-        // B (control matrix)
-        kf.B[(0,0)] = 0.5 * dt * dt;
-        kf.B[(1,1)] = 0.5 * dt * dt;
-        kf.B[(2,0)] = dt;
-        kf.B[(3,1)] = dt;
-        kf.B[(5,2)] = dt;
-
-        // H (GPS: positions only)
-        kf.H[(0,0)] = 1.0;
-        kf.H[(1,1)] = 1.0;
-
-        kf
+impl Ekf2D {
+    /// Small helper: rotation matrix R(theta)
+    fn rotation(theta: f32) -> Matrix2<f32> {
+        let c = theta.cos();
+        let s = theta.sin();
+        Matrix2::new(c, -s, s, c)
     }
 
-    pub fn predict(&mut self, u: &Control) {
-        self.x = self.F * self.x + self.B * u;
-        self.P = self.F * self.P * self.F.transpose() + self.Q;
+    /// Derivative dR/dtheta
+    fn rotation_derivative(theta: f32) -> Matrix2<f32> {
+        let c = theta.cos();
+        let s = theta.sin();
+        // derivative of [[c, -s],[s, c]] = [[-s, -c],[c, -s]]
+        Matrix2::new(-s, -c, c, -s)
     }
 
-    pub fn update(&mut self, z: &GpsMeasurement) {
-        let y = z - self.H * self.x; // innovation
-        let S = self.H * self.P * self.H.transpose() + self.R;
-        if let Some(s_inv) = S.try_inverse() {
-            let K = self.P * self.H.transpose() * s_inv;
-            self.x += K * y;
-            self.P = (SMatrix::<f32,6,6>::identity() - K * self.H) * self.P;
-        }
+    /// Predict step using IMU readings in body frame:
+    /// - accel_b: Vector2 (ax, ay) in body frame (m/s^2)
+    /// - gyro_z: yaw rate (rad/s)
+    /// - dt: seconds
+    pub fn predict(&mut self, accel_b: Vector2<f32>, gyro_z: f32, dt: f32) {
+        // Unpack state
+        let px = self.x[0];
+        let py = self.x[1];
+        let vx = self.x[2];
+        let vy = self.x[3];
+        let theta = self.x[4];
+
+        // rotation and rotated acceleration
+        let R = Self::rotation(theta);
+        let a_w = R * accel_b; // world-frame acceleration
+
+        // Nonlinear state propagation (discrete)
+        let p_new = Vector2::new(px, py) + Vector2::new(vx, vy) * dt + 0.5 * a_w * dt * dt;
+        let v_new = Vector2::new(vx, vy) + a_w * dt;
+        let theta_new = theta + gyro_z * dt;
+
+        // Updated state
+        self.x[0] = p_new[0];
+        self.x[1] = p_new[1];
+        self.x[2] = v_new[0];
+        self.x[3] = v_new[1];
+        self.x[4] = theta_new;
+
+        // Build Jacobian F = df/dx (5x5)
+        // Using ordering [px, py, vx, vy, theta]
+        let mut F = Matrix5::identity();
+
+        // ∂p'/∂v = dt * I2
+        F[(0,2)] = dt; F[(1,3)] = dt;
+
+        // ∂p'/∂theta = 0.5 * dt^2 * dR/dtheta * a_b (2x1 into columns)
+        let dR_dth = Self::rotation_derivative(theta);
+        let dp_dth = 0.5 * dt * dt * (dR_dth * accel_b);
+        F[(0,4)] = dp_dth[0];
+        F[(1,4)] = dp_dth[1];
+
+        // ∂v'/∂theta = dt * dR/dtheta * a_b
+        let dv_dth = dt * (dR_dth * accel_b);
+        F[(2,4)] = dv_dth[0];
+        F[(3,4)] = dv_dth[1];
+
+        // ∂v'/∂v = I2 (already identity on diagonal)
+        // ∂p'/∂p = I2 (already identity)
+
+        // ∂theta'/∂theta = 1 (identity)
+
+        // Covariance propagation: P = F P Fᵀ + Q (we might scale Q by dt if desired)
+        self.P = F * self.P * F.transpose() + self.Q;
     }
 
-    // Used to indicate stationary behavior and therefore velocities should be zero
+    /// GPS update: provide measured position (px,py) in world frame
+    pub fn update_gps(&mut self, z_px: f32, z_py: f32) {
+        // measurement matrix H (2x5) picks px,py
+        let mut H = Matrix2x5::zeros();
+        H[(0,0)] = 1.0;
+        H[(1,1)] = 1.0;
+
+        // y = z - H x
+        let z = Vector2::new(z_px, z_py);
+        let hx = Vector2::new(self.x[0], self.x[1]);
+        let y = z - hx;
+
+        // S = H P Hᵀ + R
+        let S = H * self.P * H.transpose() + self.R_gps;
+
+        // K = P Hᵀ S⁻¹  (5x2)
+        let K = self.P * H.transpose() * S.try_inverse().unwrap_or_else(|| {
+            // fallback to small-regularized inverse if S is singular
+            let mut Sreg = S;
+            Sreg[(0,0)] += 1e-6;
+            Sreg[(1,1)] += 1e-6;
+            Sreg.try_inverse().unwrap()
+        });
+
+        // x = x + K y
+        let dx = K * y;
+        self.x += dx;
+
+        // P = (I - K H) P
+        let I5 = Matrix5::identity();
+        let KH = K * H;
+        self.P = (I5 - KH) * self.P;
+    }
+
+    /// Optional: set a more informed initial pose when first GPS arrives.
+    /// This can reduce large initial uncertainty in position and optionally set heading.
+    pub fn initialize_with_gps(&mut self, px: f32, py: f32, pos_variance: f32) {
+        self.x[0] = px;
+        self.x[1] = py;
+        self.P[(0,0)] = pos_variance;
+        self.P[(1,1)] = pos_variance;
+        // leave velocity and heading as-is (or set to small uncertainty if you know them)
+    }
+
+        /// Zero-velocity update (ZUPT). Call this when you know the vehicle is stationary.
     pub fn update_zupt(&mut self) {
-        let z = SVector::<f32, 2>::zeros(); // velocity = 0
-        let mut H = SMatrix::<f32, 2, 6>::zeros();
-        H[(0,2)] = 1.0; // vx
-        H[(1,3)] = 1.0; // vy
+        // Measurement matrix H (2x5): extracts vx, vy
+        let mut H = Matrix2x5::zeros();
+        H[(0,2)] = 1.0;
+        H[(1,3)] = 1.0;
 
-        let R = SMatrix::<f32, 2, 2>::identity() * 0.01; // very confident
+        // Expected measurement: current velocity estimate
+        let hx = Vector2::new(self.x[2], self.x[3]);
 
-        let y = z - H * self.x;
+        // "Observed" velocity is zero
+        let z = Vector2::zeros();
+        let y = z - hx;
+
+        // Choose a small measurement noise R (you can tune this)
+        let mut R = Matrix2::zeros();
+        R[(0,0)] = 0.01;
+        R[(1,1)] = 0.01;
+
+        // S = H P Hᵀ + R
         let S = H * self.P * H.transpose() + R;
 
-        if let Some(s_inv) = S.try_inverse() {
-            let k = self.P * H.transpose() * s_inv;
-            self.x += k * y;
-            self.P = (SMatrix::<f32,6,6>::identity() - k * H) * self.P;
-        }
-    }
+        // Kalman gain
+        let K = self.P * H.transpose() * S.try_inverse().unwrap();
 
-    pub fn state(&self) -> &State {
-        &self.x
+        // State correction
+        self.x += K * y;
+
+        // Covariance correction
+        let I5 = Matrix5::identity();
+        self.P = (I5 - K * H) * self.P;
+    }
+}
+
+impl Default for Ekf2D {
+    fn default() -> Self {
+        let x = Vector5::zeros();
+        // default: unknown position/vel/heading. We'll set large covariance
+        let mut P = Matrix5::zeros();
+        P[(0,0)] = 1e6; // px
+        P[(1,1)] = 1e6; // py
+        P[(2,2)] = 1e3; // vx
+        P[(3,3)] = 1e3; // vy
+        P[(4,4)] = (2.0*PI).powi(2); // theta huge uncertainty
+
+        // Process noise - tune to your sensors:
+        // Represents uncertainty in the dynamics (accel noise affects v and p, gyro noise affects theta)
+        let mut Q = Matrix5::zeros();
+        Q[(0,0)] = 0.1;
+        Q[(1,1)] = 0.1;
+        Q[(2,2)] = 1.0;
+        Q[(3,3)] = 1.0;
+        Q[(4,4)] = 0.5; // heading diffusion
+
+        let mut R_gps = Matrix2::zeros();
+        R_gps[(0,0)] = 5.0; // variance in meters^2 (tune to your GPS)
+        R_gps[(1,1)] = 5.0;
+
+        Self { x, P, Q, R_gps }
     }
 }

src/ego/mod.rs

@@ -1,4 +1,5 @@
-pub mod alignment;
-pub mod tilt;
+//pub mod tilt;
 pub mod heading;
-pub mod kalman;
+pub mod kalman;
+pub mod orientation;
+pub mod engine;

src/ego/orientation.rs

@@ -0,0 +1,83 @@
+use nalgebra::{Vector3, Rotation3, Matrix3, Unit};
+use log::*;
+
+use crate::CircularBuffer;
+
+#[derive(Default, Debug, Clone, Copy)]
+pub struct OrientationEstimator {
+    sensor_bias: Vector3<f32>, // Bias of the sensors at rest, eg, this will include the pull of gravity and needs applied prior to rotating
+    down: Option<Unit<Vector3<f32>>>, 
+    forward: Option<Unit<Vector3<f32>>>,
+    rotation_sb: Rotation3<f32>, // sensor -> body
+    accel_history: CircularBuffer<Vector3<f32>, 200>
+}
+
+impl OrientationEstimator {
+    pub fn is_ready(&self) -> bool {
+        self.forward.is_some()
+    }
+
+    pub fn has_down(&self) -> bool {
+        self.down.is_some()
+    }
+
+    pub fn bias(&self) -> Vector3<f32> {
+        self.sensor_bias
+    }
+
+    pub fn insert(&mut self, accel: Vector3<f32>) {
+        self.accel_history.insert(accel);
+        match self.down {
+            None => {
+                if self.accel_history.is_filled() {
+                    // Estimate down from stationary accel
+                    let avg = self.accel_history.data().iter().sum::<Vector3<f32>>()
+                        / (self.accel_history.data().len() as f32);
+                    if avg.norm() >= 9.764 && avg.norm() <= 9.834 {
+                        self.down = Some(Unit::new_normalize(-avg));
+                        self.sensor_bias = avg;
+                        info!("Found down={:?} bias={:?}", self.down, self.sensor_bias);
+                    }
+                }
+            },
+            Some(down) => {
+                // Project motion into horizontal plane
+                let avg = self.accel_history.data().iter().sum::<Vector3<f32>>();
+                let horiz = avg - avg.dot(&down) * down.into_inner();
+
+                if horiz.norm() < 5.0 {
+                    return; // not enough motion
+                }
+
+                let mut forward = Unit::new_normalize(horiz);
+
+                // If we already had a forward, allow reversal check
+                if let Some(prev) = &self.forward {
+                    // Positive dot means similar direction, negative means opposite direction
+                    if forward.dot(prev) < -0.7 {
+                        // Strong opposite -> flip
+                        forward = Unit::new_unchecked(-forward.into_inner());
+                        warn!("Forwards is flipped!!! prev={prev:?} forward={forward:?}");
+                    }
+                } else {
+                    info!("Found forwards: {forward:?}")
+                }
+
+                self.forward = Some(forward);
+
+                // Build body axes
+                let x = forward.into_inner();
+                let z = down.into_inner();
+                let y = z.cross(&x).normalize();
+                let x = y.cross(&z).normalize(); // re-orthogonalize
+
+                let mat = Matrix3::from_columns(&[x, y, z]);
+                self.rotation_sb = Rotation3::from_matrix_unchecked(mat);
+            }
+        }
+    }
+
+    pub fn apply(&self, v_sensor: Vector3<f32>) -> Vector3<f32> {
+        self.rotation_sb * (v_sensor - self.sensor_bias)
+    }
+}