bugfix

Fixed a bug where the existence probability would just be set to the predicted probability when performing step function. PDAF::get_weights now correctly handles the case when no measurements are associated with the target
vortexntnu · Oct 28, 2024 · fbb2cd2 · fbb2cd2
1 parent 661b85f
commit fbb2cd2
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 2 deletions.
diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp
@@ -141,7 +141,7 @@ template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
     Arr_zXd z_meas_inside = PDAF::get_inside_measurements(z_measurements, gated_measurements);
 
     double existence_probability_upd = existence_prob_pred;
-    if (z_measurements.cols() == 0 && !config.ipda.update_existence_probability_on_no_detection) {
+    if (!(z_measurements.cols() == 0 && config.ipda.update_existence_probability_on_no_detection)) {
       existence_probability_upd = existence_prob_update(z_meas_inside, z_pred, existence_prob_pred, config);
     }
     // clang-format off

diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp
@@ -166,7 +166,11 @@ template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
     Eigen::VectorXd weights(z_measurements.cols() + 1);
 
     // in case no measurement assosiates with the target
-    weights(0) = lambda * (1 - P_d);
+    if(lambda == 0.0 && z_measurements.cols() == 0) {
+      weights(0) = 1.0;
+    } else {
+      weights(0) = lambda * (1 - P_d);
+    }
 
     // measurements associating with the target
     for (size_t a_k = 1; const Vec_z &z_k : z_measurements.colwise()) {