Update Kalman3D_Wave.h

Author: mgrouch

bbn_wave_freq_m5atomS3/Kalman3D_Wave.h

@@ -387,43 +387,42 @@ class EIGEN_ALIGN_MAX Kalman3D_Wave {
     static void PhiAxis4x1_analytic(T tau, T h, Eigen::Matrix<T,4,4>& Phi_axis);
     static void QdAxis4x1_analytic(T tau, T h, T sigma2, Eigen::Matrix<T,4,4>& Qd_axis);
 
-    static inline void joseph_update_rank3(
-        Eigen::Matrix<T,NX,1>& x,
-        Eigen::Matrix<T,NX,NX>& P,
-        const Eigen::Matrix<T,3,NX>& C,
-        const Eigen::Matrix<T,3,1>& r,
-        const Eigen::Matrix<T,3,3>& R)
-    {
-        // Innovation covariance
-        Eigen::Matrix<T,3,3> S = C * P * C.transpose() + R;
-
-        // Factorization (with fallback regularization)
-        Eigen::LDLT<Eigen::Matrix<T,3,3>> ldlt(S);
-        bool used_reg = false;
-        if (ldlt.info() != Eigen::Success) {
-            Eigen::Matrix<T,3,3> S_reg = S + Eigen::Matrix<T,3,3>::Identity() *
-                std::max(std::numeric_limits<T>::epsilon(), T(1e-6) * R.norm());
-            ldlt.compute(S_reg);
-            if (ldlt.info() != Eigen::Success) return;
-            S = S_reg; // <- make sure Joseph uses the SAME S that produced K
-            used_reg = true; (void)used_reg;
-        }
+template<typename T, bool with_gyro_bias, bool with_accel_bias>
+inline void Kalman3D_Wave<T, with_gyro_bias, with_accel_bias>::joseph_update_rank3(
+    Eigen::Matrix<T,NX,1>& x,
+    Eigen::Matrix<T,NX,NX>& P,
+    const Eigen::Matrix<T,3,NX>& C,
+    const Eigen::Matrix<T,3,1>& r,
+    const Eigen::Matrix<T,3,3>& R)
+{
+    // Innovation covariance
+    Eigen::Matrix<T,3,3> S = C * P * C.transpose() + R;
+
+    // Factorization (with fallback regularization)
+    Eigen::LDLT<Eigen::Matrix<T,3,3>> ldlt(S);
+    if (ldlt.info() != Eigen::Success) {
+        Eigen::Matrix<T,3,3> S_reg = S + Eigen::Matrix<T,3,3>::Identity() *
+            std::max(std::numeric_limits<T>::epsilon(), T(1e-6) * R.norm());
+        ldlt.compute(S_reg);
+        if (ldlt.info() != Eigen::Success) return;
+    }
 
-        // Gain
-        Eigen::Matrix<T,NX,3> K = P * C.transpose() * ldlt.solve(Eigen::Matrix<T,3,3>::Identity());
+    // Gain
+    Eigen::Matrix<T,NX,3> K = P * C.transpose() * ldlt.solve(Eigen::Matrix<T,3,3>::Identity());
 
-        // State
-        x.noalias() += K * r;
+    // State update
+    x.noalias() += K * r;
 
-        // Rank-3 Joseph covariance update:
-        // P' = P - KCP - (KCP)ᵀ + K S Kᵀ   with K = P Cᵀ S⁻¹
-        Eigen::Matrix<T,3,NX> CP = C * P;          // 3×NX
-        Eigen::Matrix<T,NX,NX> KC = K * CP;        // NX×NX (rank ≤ 3)
-        P.noalias() = P - KC - KC.transpose() + K * S * K.transpose();
+    // Correct Joseph covariance update (rank-3)
+    // P' = P - KCP - (KCP)ᵀ + K R Kᵀ
+    Eigen::Matrix<T,3,NX> CP = C * P;          // 3×NX
+    Eigen::Matrix<T,NX,NX> KC = K * CP;        // NX×NX (rank ≤ 3)
+    P.noalias() = P - KC - KC.transpose() + K * R * K.transpose();
 
-        // Symmetrize
-        P = T(0.5) * (P + P.transpose());
-    }
+    // Symmetrize
+    P = T(0.5) * (P + P.transpose());
+}
+  
 };
 
 // Implementation