fix bias evolution in zero-velocity not using squared noise terms

config/rpng_sim/estimator_config.yaml

@@ -28,11 +28,11 @@ feat_rep_aruco: "GLOBAL_3D"
 # zero velocity update parameters we can use
 # we support either IMU-based or disparity detection.
 try_zupt: false
-zupt_chi2_multipler: 0 # set to 0 for only disp-based
+zupt_chi2_multipler: 1 # set to 0 for only disp-based
 zupt_max_velocity: 0.1
-zupt_noise_multiplier: 50
-zupt_max_disparity: 0.5 # set to 0 for only imu-based
-zupt_only_at_beginning: true
+zupt_noise_multiplier: 1
+zupt_max_disparity: 0 # set to 0 for only imu-based
+zupt_only_at_beginning: false
 
 # ==================================================================
 # ==================================================================

ov_core/src/feat/FeatureHelper.h

@@ -121,7 +121,7 @@ class FeatureHelper {
    * @param oldest_time Only compute disparity for ones newer (-1 to disable)
    */
   static void compute_disparity(std::shared_ptr<ov_core::FeatureDatabase> db, double &disp_mean, double &disp_var, int &total_feats,
-                                double newest_time = 1, double oldest_time = 1) {
+                                double newest_time = -1, double oldest_time = -1) {
 
     // Compute the disparity
     std::vector<double> disparities;

ov_msckf/launch/serial.launch

@@ -10,7 +10,7 @@
     <arg name="use_stereo"  default="true" />
     <arg name="bag_start"   default="0" /> <!-- v1-2: 0, mh1: 40, mh2: 35, mh3: 17.5, mh4-5: 15 -->
     <arg name="dataset"     default="V1_02_medium" /> <!-- V1_01_easy, V1_02_medium, V2_02_medium, dataset-room1_512_16 -->
-    <arg name="bag"         default="/media/patrick/RPNG FLASH 3/$(arg config)/$(arg dataset).bag" />
+    <arg name="bag"         default="/media/patrick/RPNG_DATA_6/$(arg config)/$(arg dataset).bag" />
     <!-- <arg name="bag"         default="/home/patrick/datasets/$(arg config)/$(arg dataset).bag" /> -->
     <!-- <arg name="bag"         default="/datasets/$(arg config)/$(arg dataset).bag" /> -->
 

ov_msckf/src/core/VioManager.cpp

@@ -189,6 +189,13 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector
     // Replace with the simulated tracker
     trackSIM = std::make_shared<TrackSIM>(state->_cam_intrinsics_cameras, state->_options.max_aruco_features);
     trackFEATS = trackSIM;
+    // Need to also replace it in init and zv-upt since it points to the trackFEATS db pointer
+    initializer = std::make_shared<ov_init::InertialInitializer>(params.init_options, trackFEATS->get_feature_database());
+    if (params.try_zupt) {
+      updaterZUPT = std::make_shared<UpdaterZeroVelocity>(params.zupt_options, params.imu_noises, trackFEATS->get_feature_database(),
+                                                          propagator, params.gravity_mag, params.zupt_max_velocity,
+                                                          params.zupt_noise_multiplier, params.zupt_max_disparity);
+    }
     PRINT_WARNING(RED "[SIM]: casting our tracker to a TrackSIM object!\n" RESET);
   }
 
@@ -208,6 +215,7 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector
       did_zupt_update = updaterZUPT->try_update(state, timestamp);
     }
     if (did_zupt_update) {
+      trackDATABASE->cleanup_measurements(timestamp);
       return;
     }
   }
@@ -281,6 +289,7 @@ void VioManager::track_image_and_update(const ov_core::CameraData &message_const
       did_zupt_update = updaterZUPT->try_update(state, message.timestamp);
     }
     if (did_zupt_update) {
+      trackDATABASE->cleanup_measurements(message.timestamp);
       return;
     }
   }
@@ -558,7 +567,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message)
   // First do anchor change if we are about to lose an anchor pose
   updaterSLAM->change_anchors(state);
 
-  // Cleanup any features older then the marginalization time
+  // Cleanup any features older than the marginalization time
   if ((int)state->_clones_IMU.size() > state->_options.max_clone_size) {
     trackFEATS->get_feature_database()->cleanup_measurements(state->margtimestep());
     trackDATABASE->cleanup_measurements(state->margtimestep());

ov_msckf/src/update/UpdaterZeroVelocity.cpp

@@ -176,16 +176,18 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr<State> state, double timest
   R *= _zupt_noise_multiplier;
 
   // Next propagate the biases forward in time
-  // NOTE: G*Qd*G^t = dt*Qd*dt = dt*Qc
+  // NOTE: G*Qd*G^t = dt*Qd*dt = dt*(1/dt*Qc)*dt = dt*Qc
   Eigen::MatrixXd Q_bias = Eigen::MatrixXd::Identity(6, 6);
-  Q_bias.block(0, 0, 3, 3) *= dt_summed * _noises.sigma_wb;
-  Q_bias.block(3, 3, 3, 3) *= dt_summed * _noises.sigma_ab;
+  Q_bias.block(0, 0, 3, 3) *= dt_summed * _noises.sigma_wb_2;
+  Q_bias.block(3, 3, 3, 3) *= dt_summed * _noises.sigma_ab_2;
 
   // Chi2 distance check
-  // NOTE: we also append the propagation we "would do before the update" if this was to be accepted
+  // NOTE: we also append the propagation we "would do before the update" if this was to be accepted (just the bias evolution)
   // NOTE: we don't propagate first since if we fail the chi2 then we just want to return and do normal logic
   Eigen::MatrixXd P_marg = StateHelper::get_marginal_covariance(state, Hx_order);
-  P_marg.block(3, 3, 6, 6) += Q_bias;
+  if (model_time_varying_bias) {
+    P_marg.block(3, 3, 6, 6) += Q_bias;
+  }
   Eigen::MatrixXd S = H * P_marg * H.transpose() + R;
   double chi2 = res.dot(S.llt().solve(res));
 
@@ -207,25 +209,24 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr<State> state, double timest
     double time0_cam = state->_timestamp;
     double time1_cam = timestamp;
     int num_features = 0;
-    double average_disparity = 0.0;
-    double variance_disparity = 0.0;
-    FeatureHelper::compute_disparity(_db, time0_cam, time1_cam, average_disparity, variance_disparity, num_features);
+    double disp_avg = 0.0;
+    double disp_var = 0.0;
+    FeatureHelper::compute_disparity(_db, time0_cam, time1_cam, disp_avg, disp_var, num_features);
 
     // Check if this disparity is enough to be classified as moving
-    disparity_passed = (average_disparity < _zupt_max_disparity && num_features > 20);
+    disparity_passed = (disp_avg < _zupt_max_disparity && num_features > 20);
     if (disparity_passed) {
-      PRINT_INFO(CYAN "[ZUPT]: passed disparity (%.3f < %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity,
-                 (int)num_features);
+      PRINT_INFO(CYAN "[ZUPT]: passed disparity (%.3f < %.3f, %d features)\n" RESET, disp_avg, _zupt_max_disparity, (int)num_features);
     } else {
-      PRINT_DEBUG(YELLOW "[ZUPT]: failed disparity (%.3f > %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity,
-                  (int)num_features);
+      PRINT_DEBUG(YELLOW "[ZUPT]: failed disparity (%.3f > %.3f, %d features)\n" RESET, disp_avg, _zupt_max_disparity, (int)num_features);
     }
   }
 
   // Check if we are currently zero velocity
   // We need to pass the chi2 and not be above our velocity threshold
   if (!disparity_passed && (chi2 > _options.chi2_multipler * chi2_check || state->_imu->vel().norm() > _zupt_max_velocity)) {
     last_zupt_state_timestamp = 0.0;
+    last_zupt_count = 0;
     PRINT_DEBUG(YELLOW "[ZUPT]: rejected |v_IinG| = %.3f (chi2 %.3f > %.3f)\n" RESET, state->_imu->vel().norm(), chi2,
                 _options.chi2_multipler * chi2_check);
     return false;
@@ -236,8 +237,9 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr<State> state, double timest
   // Do our update, only do this update if we have previously detected
   // If we have succeeded, then we should remove the current timestamp feature tracks
   // This is because we will not clone at this timestep and instead do our zero velocity update
-  // We want to keep the tracks from the previous timestep, thus only delete measurements from the current timestep
-  if (last_zupt_state_timestamp > 0.0) {
+  // NOTE: We want to keep the tracks from the second time we have called the zv-upt since this won't have a clone
+  // NOTE: All future times after the second call to this function will also *not* have a clone, so we can remove those
+  if (last_zupt_count >= 2) {
     _db->cleanup_measurements_exact(last_zupt_state_timestamp);
   }
 
@@ -309,5 +311,6 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr<State> state, double timest
 
   // Finally return
   last_zupt_state_timestamp = timestamp;
+  last_zupt_count++;
   return true;
 }

ov_msckf/src/update/UpdaterZeroVelocity.h

@@ -142,6 +142,9 @@ class UpdaterZeroVelocity {
 
   /// Last timestamp we did zero velocity update with
   double last_zupt_state_timestamp = 0.0;
+
+  /// Number of times we have called update
+  int last_zupt_count = 0;
 };
 
 } // namespace ov_msckf