Update Kalman3D_Wave.h

Author: mgrouch

bbn_wave_freq_m5atomS3/Kalman3D_Wave.h

@@ -394,21 +394,34 @@ class EIGEN_ALIGN_MAX Kalman3D_Wave {
         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;
         }
 
+        // Gain
         Eigen::Matrix<T,NX,3> K = P * C.transpose() * ldlt.solve(Eigen::Matrix<T,3,3>::Identity());
+
+        // State
         x.noalias() += K * r;
 
-        Eigen::Matrix<T,3,NX> CP = C * P;
-        Eigen::Matrix<T,NX,NX> KC = K * CP;
-        P.noalias() = P - KC - KC.transpose() + K * R * K.transpose();
+        // 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();
+
+        // Symmetrize
         P = T(0.5) * (P + P.transpose());
     }
 };