From c2c89809062c37a89d602b1b77810883be0ad061 Mon Sep 17 00:00:00 2001
From: Mikhail Grushinskiy <mgrouch@users.noreply.github.com>
Date: Tue, 24 Sep 2024 11:05:06 -0400
Subject: [PATCH] Update KalmanQMEKF.h

---
 bbn_wave_freq_m5atomS3/KalmanQMEKF.h | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/bbn_wave_freq_m5atomS3/KalmanQMEKF.h b/bbn_wave_freq_m5atomS3/KalmanQMEKF.h
index 395f156..37113ab 100644
--- a/bbn_wave_freq_m5atomS3/KalmanQMEKF.h
+++ b/bbn_wave_freq_m5atomS3/KalmanQMEKF.h
@@ -90,12 +90,12 @@ class QuaternionMEKF {
     Vector3 accelerometer_measurement_func() const;
     Vector3 magnetometer_measurement_func() const;
 
-    static constexpr MatrixN initialize_Q(Vector3 sigma_g);
+    static constexpr MatrixN initialize_Q(Vector3 sigma_g, T b0);
 };
 
 template <typename T, bool with_bias>
-QuaternionMEKF<T, with_bias>::QuaternionMEKF(Vector3 const& sigma_a, Vector3 const& sigma_g, Vector3 const& sigma_m, T Pq0, T Pb0)
-  : Q(initialize_Q(sigma_g)),
+QuaternionMEKF<T, with_bias>::QuaternionMEKF(Vector3 const& sigma_a, Vector3 const& sigma_g, Vector3 const& sigma_m, T Pq0, T Pb0, Tb0)
+  : Q(initialize_Q(sigma_g, b0)),
   Racc(sigma_a.array().square().matrix().asDiagonal()),
   Rmag(sigma_m.array().square().matrix().asDiagonal()),
   R((Vector6() << sigma_a, sigma_m).finished().array().square().matrix().asDiagonal()) {
@@ -375,9 +375,9 @@ Matrix<T, 3, 1> QuaternionMEKF<T, with_bias>::magnetometer_measurement_func() co
 }
 
 template<typename T, bool with_bias>
-constexpr typename QuaternionMEKF<T, with_bias>::MatrixN QuaternionMEKF<T, with_bias>::initialize_Q(Vector3 sigma_g) {
+constexpr typename QuaternionMEKF<T, with_bias>::MatrixN QuaternionMEKF<T, with_bias>::initialize_Q(Vector3 sigma_g, T b0) {
   if constexpr (with_bias) {
-    return (Vector6() << sigma_g.array().square().matrix(), 1e-12, 1e-12, 1e-12).finished().asDiagonal();
+    return (Vector6() << sigma_g.array().square().matrix(), b0, b0, b0).finished().asDiagonal();
   }
   else {
     return sigma_g.array().square().matrix().asDiagonal();
@@ -396,4 +396,3 @@ typedef struct QMEKF_vars {
 } Kalman_QMEKF_vars;
 */
 
-