Add Kalman filter, non-linear sensitivity curve

firmware/include/Config.h

@@ -27,8 +27,17 @@ const int SIGN_AXIS[6] = {-1, +1, -1, +1, +1, +1};
 const float DEAD_T = 16.0;
 const float DEAD_R = 20.0;
 
-// Smoothing
-const float SMOOTH_TAU_S = 0.08;
+// Kalman filter tuning
+// Process noise: how much we expect the true value to change per step.
+// Higher = more responsive but noisier.
+const float KALMAN_Q = 0.5;
+// Measurement noise: how noisy the sensor readings are.
+// Higher = smoother but more latency.
+const float KALMAN_R = 4.0;
+
+// Sensitivity curve exponent.
+// 1.0 = linear, 3.0 = cubic (fine control at small deflections, fast at large).
+const float SENSITIVITY_EXP = 3.0;
 
 // Final axis output range
 const float AXIS_LIMIT = 350.0;

firmware/include/controllers/MotionController.h

@@ -8,9 +8,13 @@ class MotionController {
 
  private:
   static float clampf(float v, float lo, float hi);
-  static float hardZero(float v, float thr);
-  static float lowpass(float prev, float x, float dt, float tau);
   static float axisBaseDead(int i);
-  float filt_[6] = {};
+  static float sensitivityCurve(float value, float dead, float limit);
+
+  // Per-axis Kalman filter state
+  float kalmanX_[6] = {};  // Estimated state
+  float kalmanP_[6] = {};  // Estimate uncertainty (covariance)
+  float kalmanStep(int axis, float measurement);
+
   bool motionActive_ = false;
 };

firmware/include/controllers/SensorController.h

@@ -9,7 +9,7 @@ class SensorController {
   SensorController();
 
   void begin();
-  void readRaw(float out[9]);
+  bool readRaw(float out[9]);
 
   void beginCalibration();
   void updateCalibration();

firmware/src/controllers/HIDController.cpp

@@ -13,7 +13,7 @@ const uint8_t kHidReportDescriptor[] PROGMEM = {
     0x85, 0x01,        // REPORT_ID (1)
     0x16, 0xA2, 0xFE,  // LOGICAL_MINIMUM  (-350)
     0x26, 0x5E, 0x01,  // LOGICAL_MAXIMUM  (350)
-    0x09, 0x30,        // USAGE (X)     
+    0x09, 0x30,        // USAGE (X)
     0x09, 0x31,        // USAGE (Y)
     0x09, 0x32,        // USAGE (Z)
     0x09, 0x33,        // USAGE (Rx)

firmware/src/controllers/MotionController.cpp

@@ -26,11 +26,24 @@ enum AxisIndex {
   AXIS_RY,
   AXIS_RZ
 };
+
+// Precomputed geometric constants for the equilateral sensor triangle.
+const float kOneThird = 1.0f / 3.0f;
+const float kSqrt3 = 1.7320508f;          // sqrt(3)
+const float kSqrt3Over6 = 0.28867513f;    // sqrt(3)/6
+const float kSqrt3Over3 = 0.57735027f;    // sqrt(3)/3
+const float kMag2PosX = -0.5f;
+const float kMag2PosY = kSqrt3Over6;
+const float kMag3PosX = 0.5f;
+const float kMag3PosY = kSqrt3Over6;
+const float kMag1PosX = 0.0f;
+const float kMag1PosY = -kSqrt3Over3;
 }  // namespace
 
 void MotionController::reset() {
   for (int i = 0; i < 6; i++) {
-    filt_[i] = 0.0;
+    kalmanX_[i] = 0.0f;
+    kalmanP_[i] = 1.0f;
   }
   motionActive_ = false;
 }
@@ -41,20 +54,42 @@ float MotionController::clampf(float v, float lo, float hi) {
   return v;
 }
 
-float MotionController::hardZero(float v, float thr) {
-  return (fabs(v) < thr) ? 0.0 : v;
-}
+float MotionController::kalmanStep(int axis, float measurement) {
+  // Predict step: uncertainty grows by process noise
+  kalmanP_[axis] += Config::KALMAN_Q;
+
+  // Update step: compute Kalman gain
+  const float K = kalmanP_[axis] / (kalmanP_[axis] + Config::KALMAN_R);
 
-float MotionController::lowpass(float prev, float x, float dt, float tau) {
-  if (tau <= 0.0) return x;
-  const float a = dt / (tau + dt);
-  return prev + a * (x - prev);
+  // Correct estimate with measurement
+  kalmanX_[axis] += K * (measurement - kalmanX_[axis]);
+
+  // Update uncertainty
+  kalmanP_[axis] *= (1.0f - K);
+
+  return kalmanX_[axis];
 }
 
 float MotionController::axisBaseDead(int i) {
   return (i < 3) ? Config::DEAD_T : Config::DEAD_R;
 }
 
+float MotionController::sensitivityCurve(float value, float dead, float limit) {
+  // Map the post-dead-zone range [dead, limit] onto [0, limit] with a power curve.
+  // This gives fine control at small deflections and fast motion at large ones.
+  const float sign = (value >= 0.0f) ? 1.0f : -1.0f;
+  const float abs_val = fabs(value);
+  if (abs_val <= dead) return 0.0f;
+
+  // Normalize to 0..1 within the active range
+  const float range = limit - dead;
+  if (range <= 0.0f) return 0.0f;
+  const float normalized = clampf((abs_val - dead) / range, 0.0f, 1.0f);
+
+  // Apply power curve and scale back to output range
+  return sign * powf(normalized, Config::SENSITIVITY_EXP) * limit;
+}
+
 void MotionController::compute(const float raw[9], const float* baseline, float dt,
                                float out[6]) {
   // Baseline subtraction converts magnetic deltas around the calibrated rest pose.
@@ -68,43 +103,21 @@ void MotionController::compute(const float raw[9], const float* baseline, float
   const float mag3y = raw[RAW_MAG3_Y] - baseline[RAW_MAG3_Y];
   const float mag3z = raw[RAW_MAG3_Z] - baseline[RAW_MAG3_Z];
 
-  // Translation:
-  //   Tx = (mag1x + mag2x + mag3x) / 3
-  //   Ty = (mag1y + mag2y + mag3y) / 3
-  //   Tz = (mag1z + mag2z + mag3z) / 3
-  const float tx = (mag1x + mag2x + mag3x) / 3.0;
-  const float ty = (mag1y + mag2y + mag3y) / 3.0;
-  const float tz = (mag1z + mag2z + mag3z) / 3.0;
-
-  // Physical PCB layout:
-  // MAG2 = top left, MAG3 = top right, MAG1 = bottom.
-  const float mag2PosX = -0.5;
-  const float mag2PosY = sqrt(3.0) / 6.0;
-
-  const float mag3PosX = 0.5;
-  const float mag3PosY = sqrt(3.0) / 6.0;
-
-  const float mag1PosX = 0.0;
-  const float mag1PosY = -sqrt(3.0) / 3.0;
-
-  // Rotation estimates:
-  //   Ry = mag3z - mag2z
-  //     right sensor minus left sensor
-  //     -> side to side tilt across the top edge
-  //
-  //   Rx = sqrt(3) * (mag2z + mag3z - 2 * mag1z) / 3
-  //     top pair minus bottom sensor
-  //     -> front/back tilt of the triangle
-  const float rx = (sqrt(3.0) * (mag2z + mag3z - 2.0 * mag1z)) / 3.0;
-  const float ry = (mag3z - mag2z);
+  // Translation: average of all three sensors.
+  const float tx = (mag1x + mag2x + mag3x) * kOneThird;
+  const float ty = (mag1y + mag2y + mag3y) * kOneThird;
+  const float tz = (mag1z + mag2z + mag3z) * kOneThird;
 
-  //   Rz = sum_i (posXi * magYi - posYi * magXi)
-  // Each sensor contributes according to its x/y position in the triangle.
-  const float swirlNum =
-      (mag2PosX * mag2y - mag2PosY * mag2x) +
-      (mag3PosX * mag3y - mag3PosY * mag3x) +
-      (mag1PosX * mag1y - mag1PosY * mag1x);
-  const float rz = swirlNum;
+  // Rotation estimates from sensor triangle geometry.
+  //   Ry: side-to-side tilt (right sensor minus left)
+  //   Rx: front/back tilt (top pair minus bottom)
+  //   Rz: twist (cross-product per sensor position)
+  const float rx = (kSqrt3 * (mag2z + mag3z - 2.0f * mag1z)) * kOneThird;
+  const float ry = (mag3z - mag2z);
+  const float rz =
+      (kMag2PosX * mag2y - kMag2PosY * mag2x) +
+      (kMag3PosX * mag3y - kMag3PosY * mag3x) +
+      (kMag1PosX * mag1y - kMag1PosY * mag1x);
 
   // Apply sign fixes and gains
   float y[6];
@@ -115,21 +128,23 @@ void MotionController::compute(const float raw[9], const float* baseline, float
   y[AXIS_RY] = Config::SIGN_AXIS[AXIS_RY] * ry * Config::GAIN_R[AXIS_RY - 3];
   y[AXIS_RZ] = Config::SIGN_AXIS[AXIS_RZ] * rz * Config::GAIN_R[AXIS_RZ - 3];
 
-  // Filter, clamp to range and dead zones.
+  // Kalman filter, sensitivity curve, dead zones, and clamp.
   motionActive_ = false;
   for (int i = 0; i < 6; i++) {
     const float dead = axisBaseDead(i);
 
     if (fabs(y[i]) < dead) {
-      filt_[i] = 0.0;
+      // Below dead zone: decay Kalman estimate toward zero gradually.
+      // Preserve covariance so the filter doesn't jitter at the boundary.
+      kalmanX_[i] *= 0.8f;
+      kalmanP_[i] = fmin(kalmanP_[i] + Config::KALMAN_Q * 0.1f, 1.0f);
     } else {
-      filt_[i] = lowpass(filt_[i], y[i], dt, Config::SMOOTH_TAU_S);
+      kalmanStep(i, y[i]);
     }
 
-    const float limited =
-        clampf(filt_[i], -Config::AXIS_LIMIT, Config::AXIS_LIMIT);
-    out[i] = hardZero(limited, dead);
-    if (out[i] != 0.0) {
+    // Apply sensitivity curve to filtered output
+    out[i] = sensitivityCurve(kalmanX_[i], dead, Config::AXIS_LIMIT);
+    if (out[i] != 0.0f) {
       motionActive_ = true;
     }
   }

firmware/src/controllers/SensorController.cpp

@@ -1,5 +1,7 @@
 #include "controllers/SensorController.h"
 
+#include <math.h>
+
 #include "Config.h"
 
 using namespace ifx::tlx493d;
@@ -51,7 +53,7 @@ void SensorController::begin() {
   delay(10);
 }
 
-void SensorController::readRaw(float out[9]) {
+bool SensorController::readRaw(float out[9]) {
   double mag1x = 0, mag1y = 0, mag1z = 0, temp1 = 0;
   double mag2x = 0, mag2y = 0, mag2z = 0, temp2 = 0;
   double mag3x = 0, mag3y = 0, mag3z = 0, temp3 = 0;
@@ -70,6 +72,20 @@ void SensorController::readRaw(float out[9]) {
   out[6] = mag3x;
   out[7] = mag3y;
   out[8] = mag3z;
+
+  // Validate: reject frames where any axis reads exactly zero across all
+  // components (likely I2C failure) or exceeds sane sensor range.
+  for (int i = 0; i < 9; i++) {
+    if (fabs(out[i]) > 500.0f) return false;  // Way outside EXTRA_SHORT_RANGE
+  }
+  // Check for dead sensor (all three axes exactly zero)
+  for (int s = 0; s < 3; s++) {
+    int base = s * 3;
+    if (out[base] == 0.0f && out[base + 1] == 0.0f && out[base + 2] == 0.0f) {
+      return false;
+    }
+  }
+  return true;
 }
 
 void SensorController::beginCalibration() {
@@ -95,7 +111,9 @@ void SensorController::updateCalibration() {
   lastCalibrationSampleMs_ = now;
 
   float raw[9] = {};
-  readRaw(raw);
+  if (!readRaw(raw)) {
+    return;  // Skip bad sample, try again next cycle
+  }
 
   for (int i = 0; i < 9; i++) {
     calibrationSum_[i] += raw[i];

firmware/src/states/IdleState.cpp

@@ -22,7 +22,9 @@ bool IdleState::handleCalibrationRequest() {
 
 void IdleState::runMotionPipeline(float dt, unsigned long now) {
   float raw[9] = {};
-  sensorController.readRaw(raw);
+  if (!sensorController.readRaw(raw)) {
+    return;  // Skip frame on sensor read failure
+  }
 
   float motion[6] = {};
   motionController.compute(raw, sensorController.baseline(), dt, motion);