Update Kalman3D_Wave.h

Author: mgrouch

bbn_wave_freq_m5atomS3/Kalman3D_Wave.h

@@ -244,19 +244,6 @@ class EIGEN_ALIGN_MAX Kalman3D_Wave {
     Matrix3 R_wb() const { return qref.toRotationMatrix(); }               // world→body
     Matrix3 R_bw() const { return qref.toRotationMatrix().transpose(); }   // body→world
 
-    // world→body rotation that contains only roll & pitch (no yaw)
-    // Constructed by aligning world +Z (down) to the current body +Z,
-    // via the minimal rotation. This eliminates any rotation about +Z (yaw).
-    Matrix3 R_wb_tilt_only() const {
-        const Vector3 ez_w(0,0,1);                 // world down
-        const Matrix3 Rwb = R_wb();                 // full world→body
-        const Vector3 ez_b = (Rwb * ez_w).normalized();  // world down expressed in body
-        // Minimal rotation mapping ez_w -> ez_b
-        Eigen::Quaternion<T> q_tilt = Eigen::Quaternion<T>::FromTwoVectors(ez_w, ez_b);
-        q_tilt.normalize();
-        return q_tilt.toRotationMatrix();          // yaw-free world→body
-    }
-  
     // Helpers and original methods kept
     void measurement_update_partial(const Eigen::Ref<const Vector3>& meas, const Eigen::Ref<const Vector3>& vhat, const Eigen::Ref<const Matrix3>& Rm);
     Matrix3 skew_symmetric_matrix(const Eigen::Ref<const Vector3>& vec) const;
@@ -539,38 +526,26 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_parti
 }
 
 template<typename T, bool with_gyro_bias, bool with_accel_bias>
-void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_acc_only(
-    Vector3 const& acc_meas, T tempC)
-{
-    // Magnitude gate (unchanged)
+void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_acc_only(Vector3 const& acc_meas, T tempC) {
+
+    // Accel magnitude sanity check
     T g_meas = acc_meas.norm();
     if (std::abs(g_meas - gravity_magnitude_) > T(2.0 * gravity_magnitude_)) {
-        return;
+        return; // reject this update
     }
 
     const Vector3 v1hat = accelerometer_measurement_func(tempC);
 
-    // Build Cext
+    // Cext: (3 x NX)
     Matrix<T, 3, NX> Cext = Matrix<T, 3, NX>::Zero();
-
-    // Base attitude part before projection
-    Matrix3 J_att = -skew_symmetric_matrix(v1hat);
-
-    // Unit body "down" axis (yaw axis) — columns along this are yaw
-    const Vector3 gb = (R_wb() * Vector3(0,0,1)).normalized();
-
-    // Project Jacobian onto the roll/pitch subspace: (I - gb gbᵀ) kills yaw column
-    const Matrix3 Prollpitch = Matrix3::Identity() - gb * gb.transpose();
-    Cext.template block<3,3>(0,0) = Prollpitch * J_att;   // ← yaw-insensitive
-
-    // Sensitivity wrt a_w (unchanged, but use yaw-free rotation in the model)
-    Cext.template block<3,3>(0, OFF_AW) = R_wb_tilt_only();
-
+    // d f_b / d (attitude error)
+    Cext.template block<3,3>(0,0) = -skew_symmetric_matrix(v1hat);
+    // d f_b / d a_w
+    Cext.template block<3,3>(0, OFF_AW) = R_wb();
     if constexpr (with_accel_bias) {
-        Cext.template block<3,3>(0,OFF_BA) = Matrix3::Identity();
+        Cext.template block<3,3>(0,OFF_BA) = Matrix3::Identity(); // d f_b / d b_acc
     }
 
-    // Innovation & standard Kalman math (unchanged)
     Vector3 inno = acc_meas - v1hat;
 
     Matrix3 S_mat = Cext * Pext * Cext.transpose() + Racc;
@@ -590,13 +565,8 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_acc_o
     Pext = (I - K * Cext) * Pext * (I - K * Cext).transpose() + K * Racc * K.transpose();
     Pext = T(0.5) * (Pext + Pext.transpose());
 
-    // Apply quaternion correction; then explicitly remove any residual yaw error component
     applyQuaternionCorrectionFromErrorState();
-
-    // Keep only roll/pitch small-angle error; zero yaw component along gb
-    Eigen::Matrix<T,3,1> dth = xext.template head<3>();
-    T dpsi = dth.dot(gb);
-    xext.template head<3>() = dth - gb * dpsi;  // strip yaw
+    xext.template head<3>().setZero();
 }
 
 template<typename T, bool with_gyro_bias, bool with_accel_bias>
@@ -677,20 +647,19 @@ void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::measurement_update_mag_o
     xext.template head<3>().setZero();
 }
 
-// specific force prediction: f_b = R_tilt (a_w - g) + b_a(temp)
-// NOTE: R_tilt = roll/pitch only. Yaw does not enter this model.
+// specific force prediction: f_b = R_wb (a_w - g) + b_a(temp)
+// with temp correction: b_a(temp) = b_a0 + k_a * (tempC - tempC_ref)
 template<typename T, bool with_gyro_bias, bool with_accel_bias>
 Matrix<T,3,1>
 Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::accelerometer_measurement_func(T tempC) const {
     const Vector3 g_world(0,0,+gravity_magnitude_);
     const Vector3 aw = xext.template segment<3>(OFF_AW);
 
-    const Matrix3 Rt = R_wb_tilt_only();   // ← yaw-free rotation
-    Vector3 fb = Rt * (aw - g_world);
+    Vector3 fb = R_wb() * (aw - g_world);
 
     if constexpr (with_accel_bias) {
         Vector3 ba0 = xext.template segment<3>(OFF_BA);
-        Vector3 ba  = ba0 + k_a_ * (tempC - tempC_ref);
+        Vector3 ba  = ba0 + k_a_ * (tempC - tempC_ref); // temperature related drift
         fb += ba;
     }
     return fb;