change values. add comment to explain tau. use trapezoidal integration for velocity, in addition to it being used for position

main/sens_fus.h

@@ -24,38 +24,42 @@ inline float getYawDifference(const Eigen::Vector3f &v_gps,
 struct sens_fus_compl {
   Eigen::Vector3f position = Eigen::Vector3f::Zero();
   Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
+  Eigen::Vector3f last_accel_world = Eigen::Vector3f::Zero();
   float yaw_offset = 0.0f;
 
-  // Time Constants per axis (X, Y, Z)
+  /*
-  // Lower = faster tracking of GPS; Higher = smoother/more IMU trust
+   * Tau is the time that the filter takes to reach 1-e^(-1) of the difference
-  Eigen::Vector3f tau_gps_pos = {0.5f, 0.5f, 0.5f};
+   * to a constant target
-  Eigen::Vector3f tau_gps_vel = {1.0f, 1.0f, INFINITY};
+   * so at t=tau, were 63% of the way there
+   * at t=3*tau, were 95% of the way there
+   */
+  Eigen::Vector3f tau_gps_pos = {4.0f, 4.0f, 10.0f};
+  Eigen::Vector3f tau_gps_vel = {5.0f, 5.0f, INFINITY};
 
   Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f};
   Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
 
-  float tau_yaw = 2.0f; // Yaw remains a scalar
+  float tau_yaw = 10.0f; // Yaw remains a scalar
 
   void predict(float dt, Eigen::Vector3f accel) {
-    // Rotate body-frame accel to world-frame
     Eigen::Vector3f accel_rotated =
         Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) * accel;
 
-    Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt);
+    Eigen::Vector3f next_velocity =
+        this->velocity + (this->last_accel_world + accel_rotated) * 0.5f * dt;
 
-    // Trapezoidal integration for position
     this->position += (this->velocity + next_velocity) * 0.5f * dt;
+
     this->velocity = next_velocity;
+    this->last_accel_world = accel_rotated;
   }
 
   void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
-    // Calculate Alpha vectors using element-wise operations
+    // alpha = dt / (tau + dt)
-    // Formula: alpha = dt / (tau + dt)
     Eigen::Vector3f alpha_pos = dt / (tau_gps_pos.array() + dt);
     Eigen::Vector3f alpha_vel = dt / (tau_gps_vel.array() + dt);
     float alpha_yaw = dt / (tau_yaw + dt);
 
-    // 1. Position Update (Element-wise LPF)
     // res = (1 - alpha) * state + alpha * measurement
     this->position =
         (Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
@@ -70,7 +74,7 @@ struct sens_fus_compl {
           std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
     }
 
-    // 3. Velocity Update (Element-wise LPF)
+    // res = (1 - alpha) * state + alpha * measurement
     this->velocity =
         (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
         alpha_vel.array() * gps_vel.array();
@@ -78,7 +82,6 @@ struct sens_fus_compl {
 
   void measure_baro(float dt, Eigen::Vector3f baro_pos,
                     Eigen::Vector3f baro_vel) {
-    // Calculate Alpha vectors using element-wise operations
     // Formula: alpha = dt / (tau + dt)
     Eigen::Vector3f alpha_pos = dt / (tau_baro_pos.array() + dt);
     Eigen::Vector3f alpha_vel = dt / (tau_baro_vel.array() + dt);