diff --git a/src/ego/orientation.rs b/src/ego/orientation.rs
index 90396cb..dd52431 100644
--- a/src/ego/orientation.rs
+++ b/src/ego/orientation.rs
@@ -9,7 +9,8 @@ pub struct OrientationEstimator {
     down: Option<Unit<Vector3<f32>>>, 
     forward: Option<Unit<Vector3<f32>>>,
     rotation_sb: Rotation3<f32>, // sensor -> body
-    accel_history: CircularBuffer<Vector3<f32>, 200>
+    accel_history: CircularBuffer<Vector3<f32>, 200>,
+    forwards_history: CircularBuffer<Vector3<f32>, 200>
 }
 
 impl OrientationEstimator {
@@ -42,37 +43,42 @@ impl OrientationEstimator {
             },
             Some(down) => {
                 // Project motion into horizontal plane
-                let avg = self.accel_history.data().iter().sum::<Vector3<f32>>();
+                let avg = self.accel_history.data().iter().sum::<Vector3<f32>>() - self.sensor_bias;
                 let horiz = avg - avg.dot(&down) * down.into_inner();
 
-                if horiz.norm() < 5.0 {
+                if horiz.norm() < 9.0 {
                     return; // not enough motion
                 }
+                //warn!("motion accel={accel:?} accel.norm={} horiz={horiz:?} horiz.norm={}", accel.norm(), horiz.norm());
 
-                let mut forward = Unit::new_normalize(horiz);
+                self.forwards_history.insert(horiz);
+                if self.forwards_history.is_filled() {
 
-                // 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:?}");
+                    let avg_forwards = self.forwards_history.data().iter().sum::<Vector3<f32>>() / (self.forwards_history.data().len() as f32);
+                    let unit_forward = Unit::new_normalize(avg_forwards);
+
+                    // 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 unit_forward.dot(prev) < -0.7 {
+                            // Strong opposite -> flip
+                            //warn!("Forwards is very different!!! prev={prev:?} forward={unit_forward:?} dot={}", unit_forward.dot(prev));
+                            self.forward = Some(Unit::new_unchecked(-unit_forward.into_inner()));
+                        }
+                    } else {
+                        info!("Found forwards: {unit_forward:?} avg.norm={}", horiz.norm());
+                        self.forward = Some(unit_forward);
                     }
-                } else {
-                    info!("Found forwards: {forward:?}")
+
+                    // Build body axes
+                    let x = unit_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);
                 }
-
-                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);
             }
         }
     }