added docstring and minor changes

Author: TristanWolfram

vortex-filtering/include/vortex_filtering/filters/README.md

@@ -273,7 +273,7 @@ The perceived measurements. This parameter consists of an Eigen vector. It shoul
 This will be a short description of what is happening inside the step function. For a more clear and specific explanation of the steps of the PDAF please look into the recommended textbooks.
 * The step function will first use the dynamic model to calculate the next state (only considering the given model). Then the sensor model is used to convert the predicted state into the measurement space. That means to get the value, we would measure with our sensor if the perceived state is like the predicted state (of the dynamic model). Concerning our rocket example: We predict the drive temperature with the dynamic model and use the sensor model to convert the drive temperature to the temperature we would measure from the outside.
 Both of those steps are done in one line of code by using the EKF explained earlier.
-* The second step is the gating of the measurements. This is to exclude measurements that are too far away. SO, we can assume that they are definitely clutter, and we won't use them. The **mahalanobis_threshold** parameter is used to define the threshold used for gating. This value will scale the covariance of the predicted state. All measurements inside this will be considered. Additionally,**min_gate_threshold** and **max_gate_threshold** are used here. Since we scale the over-time-changing covariance, we implemented a min and max threshold. If the scaled covariance is too large or too small, we still take measurements in a fixed area into account.
+* The second step is the gating of the measurements. This is to exclude measurements that are too far away. So, we can assume that they are definitely clutter, and we won't use them. The **mahalanobis_threshold** parameter is used to define the threshold used for gating. This value will scale the covariance of the predicted state. All measurements inside this will be considered. Additionally, **min_gate_threshold** and **max_gate_threshold** are used here. Since we scale the over-time-changing covariance, we implemented a min and max threshold. If the scaled covariance is too large or too small, we still take measurements in a fixed area into account.
 * The next step is the update states step. All measurements inside the gate will be compared to the predicted state estimate (by the dynamic model). This results in a Gaussian distribution for all of these measurements.
 * In the last step, the weighted average of estimated Gaussian distributions will be calculated. This weighted average will be the final output of the PDAF and is considered to be the current state estimate. Therefore, it will be the previous state estimate in the next iteration.
 

vortex-filtering/include/vortex_filtering/filters/ipda.hpp

@@ -1,3 +1,13 @@
+/**
+ * @file ipda.hpp
+ * @author Tristan Wolfram
+ * @brief File for the IPDA filter
+ * @version 1.0
+ * @date 2024-05-07
+ *
+ * @copyright Copyright (c) 2024
+ *
+ */
 #pragma once
 #include <vector>
 #include <Eigen/Dense>
@@ -7,6 +17,11 @@
 
 namespace vortex::filter
 {
+/**
+ * @brief The IPDA filter class
+ * @tparam DynModT The dynamic model type
+ * @tparam SensModT The sensor model type
+ */
 template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT>
 class IPDA
 {
@@ -25,7 +40,7 @@ class IPDA
   using Gauss_x = typename T::Gauss_x;
   using Vec_z = typename T::Vec_z;
   using GaussMix = vortex::prob::GaussianMixture<N_DIM_x>;
-  using PDAF = vortex::filter::PDAF<vortex::models::ConstantVelocity, vortex::models::IdentitySensorModel<4, 2>>;
+  using PDAF = vortex::filter::PDAF<DynModT, SensModT>;
 
   IPDA() = delete;
 
@@ -60,6 +75,19 @@ class IPDA
     return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability);
   }
 
+  /**
+   * @brief The IPDAF step function. Gets following parameters and calculates the next state with the probablity of
+   * existence.
+   * @param dyn_model The dynamic model.
+   * @param sen_model The sensor model.
+   * @param timestep The timestep.
+   * @param x_est The estimated state.
+   * @param z_meas The percieved measurements.
+   * @param survive_est The estimated survival probability (current state).
+   * @param config configuration data - see Config struct of PDAF.
+   * @return A tuple containing the final state, the existence probability, the inside (of the gate) measurements, the
+   * outside (of the gate) measurements, the predicted state, the predicted measurements, and the updated state.
+   */
   static std::tuple<Gauss_x, double, std::vector<Vec_z>, std::vector<Vec_z>, Gauss_x, Gauss_z, std::vector<Gauss_x>>
   step(const DynModT& dyn_model, const SensModT& sen_model, double timestep, const Gauss_x& x_est,
        const std::vector<Vec_z>& z_meas, double survive_est, const IPDA::Config& config)

vortex-filtering/include/vortex_filtering/filters/pdaf.hpp

@@ -1,3 +1,13 @@
+/**
+ * @file ipda.hpp
+ * @author Tristan Wolfram
+ * @brief File for the PDAF filter
+ * @version 1.0
+ * @date 2024-05-07
+ *
+ * @copyright Copyright (c) 2024
+ *
+ */
 #pragma once
 
 #include <Eigen/Dense>
@@ -7,9 +17,16 @@
 #include <vector>
 #include <vortex_filtering/vortex_filtering.hpp>
 
-namespace vortex::filter {
-
-template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT> class PDAF {
+namespace vortex::filter
+{
+/**
+ * @brief The PDAF filter class
+ * @tparam DynModT The dynamic model
+ * @tparam SensModT The sensor model
+ */
+template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT>
+class PDAF
+{
 public:
   static constexpr int N_DIM_x = DynModT::N_DIM_x;
   static constexpr int N_DIM_z = SensModT::N_DIM_z;
@@ -19,85 +36,140 @@ template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
 
   using T = Types_xzuvw<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
 
-  using EKF            = vortex::filter::EKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
-  using Gauss_z        = typename T::Gauss_z;
-  using Gauss_x        = typename T::Gauss_x;
-  using Vec_z          = typename T::Vec_z;
+  using EKF = vortex::filter::EKF_t<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;
+  using Gauss_z = typename T::Gauss_z;
+  using Gauss_x = typename T::Gauss_x;
+  using Vec_z = typename T::Vec_z;
   using MeasurementsZd = std::vector<Vec_z>;
-  using StatesXd       = std::vector<Gauss_x>;
-  using GaussMixZd     = vortex::prob::GaussianMixture<N_DIM_x>;
+  using StatesXd = std::vector<Gauss_x>;
+  using GaussMixXd = vortex::prob::GaussianMixture<N_DIM_x>;
 
-  struct Config {
+  struct Config
+  {
     double mahalanobis_threshold = 1.0;
-    double min_gate_threshold    = 0.0;
-    double max_gate_threshold    = HUGE_VAL;
-    double prob_of_detection     = 1.0;
-    double clutter_intensity     = 1.0;
+    double min_gate_threshold = 0.0;
+    double max_gate_threshold = HUGE_VAL;
+    double prob_of_detection = 1.0;
+    double clutter_intensity = 1.0;
   };
 
   PDAF() = delete;
 
+  /**
+   * @brief The step function for the PDAF filter
+   * @param dyn_model The dynamic model
+   * @param sen_model The sensor model
+   * @param timestep The timestep
+   * @param x_est The estimated state (current state)
+   * @param z_meas The measurements
+   * @param config The configuration
+   * @return A tuple containing the (new) estimated state, the measurements that were inside the gate, the measurements
+   * that were outside the gate, the predicted state (from dynamic model), the predicted measurement (from sensor
+   * model), and the updated states (comparisson of prediced state and actual measurements)
+   */
   static std::tuple<Gauss_x, MeasurementsZd, MeasurementsZd, Gauss_x, Gauss_z, StatesXd>
-  step(const DynModT &dyn_model, const SensModT &sen_model, double timestep, const Gauss_x &x_est, const MeasurementsZd &z_meas, const Config &config)
+  step(const DynModT& dyn_model, const SensModT& sen_model, double timestep, const Gauss_x& x_est,
+       const MeasurementsZd& z_meas, const Config& config)
   {
-    auto [x_pred, z_pred]  = EKF::predict(dyn_model, sen_model, timestep, x_est);
-    auto [inside, outside] = apply_gate(z_meas, z_pred, config.mahalanobis_threshold, config.min_gate_threshold, config.max_gate_threshold);
+    auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est);
+    auto [inside, outside] =
+        apply_gate(z_meas, z_pred, config.mahalanobis_threshold, config.min_gate_threshold, config.max_gate_threshold);
 
     StatesXd x_updated;
-    for (const auto &measurement : inside) {
+    for (const auto& measurement : inside)
+    {
       x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, measurement));
     }
 
-    Gauss_x x_final = get_weighted_average(inside, x_updated, z_pred, x_pred, config.prob_of_detection, config.clutter_intensity);
-    return {x_final, inside, outside, x_pred, z_pred, x_updated};
+    Gauss_x x_final =
+        get_weighted_average(inside, x_updated, z_pred, x_pred, config.prob_of_detection, config.clutter_intensity);
+    return { x_final, inside, outside, x_pred, z_pred, x_updated };
   }
 
-  static std::tuple<MeasurementsZd, MeasurementsZd> apply_gate(const MeasurementsZd &z_meas, const Gauss_z &z_pred, double mahalanobis_threshold,
-                                                               double min_gate_threshold = 0.0, double max_gate_threshold = HUGE_VAL)
+  /**
+   * @brief Applies the gate to the measurements
+   * @param z_meas The measurements
+   * @param z_pred The predicted measurement
+   * @param mahalanobis_threshold The threshold for the Mahalanobis distance
+   * @param min_gate_threshold The minimum threshold for the gate
+   * @param max_gate_threshold The maximum threshold for the gate
+   * @return A tuple containing the measurements that were inside the gate and the measurements that were outside the
+   * gate
+   */
+  static std::tuple<MeasurementsZd, MeasurementsZd> apply_gate(const MeasurementsZd& z_meas, const Gauss_z& z_pred,
+                                                               double mahalanobis_threshold,
+                                                               double min_gate_threshold = 0.0,
+                                                               double max_gate_threshold = HUGE_VAL)
   {
     MeasurementsZd inside_meas;
     MeasurementsZd outside_meas;
 
-    for (const auto &measurement : z_meas) {
+    for (const auto& measurement : z_meas)
+    {
       double mahalanobis_distance = z_pred.mahalanobis_distance(measurement);
-      double regular_distance     = (z_pred.mean() - measurement).norm();
-      if ((mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && regular_distance <= max_gate_threshold) {
+      double regular_distance = (z_pred.mean() - measurement).norm();
+      if ((mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) &&
+          regular_distance <= max_gate_threshold)
+      {
         inside_meas.push_back(measurement);
       }
-      else {
+      else
+      {
         outside_meas.push_back(measurement);
       }
     }
 
-    return {inside_meas, outside_meas};
+    return { inside_meas, outside_meas };
   }
 
-  // Getting weighted average of the predicted states
-  static Gauss_x get_weighted_average(const MeasurementsZd &z_meas, const StatesXd &updated_states, const Gauss_z &z_pred, const Gauss_x &x_pred,
-                                      double prob_of_detection, double clutter_intensity)
+  /**
+   * @brief Calculates the weighted average of the measurements
+   * @param z_meas The measurements
+   * @param updated_states The updated states
+   * @param z_pred The predicted measurement
+   * @param x_pred The predicted state
+   * @param prob_of_detection The probability of detection
+   * @param clutter_intensity The clutter intensity
+   * @return The weighted average of the measurements
+   */
+  static Gauss_x get_weighted_average(const MeasurementsZd& z_meas, const StatesXd& updated_states,
+                                      const Gauss_z& z_pred, const Gauss_x& x_pred, double prob_of_detection,
+                                      double clutter_intensity)
   {
     StatesXd states;
     states.push_back(x_pred);
     states.insert(states.end(), updated_states.begin(), updated_states.end());
 
     Eigen::VectorXd weights = get_weights(z_meas, z_pred, prob_of_detection, clutter_intensity);
 
-    GaussMixZd gaussian_mixture(weights, states);
+    GaussMixXd gaussian_mixture(weights, states);
 
     return gaussian_mixture.reduce();
   }
 
-  // Getting association probabilities according to textbook p. 123 "Corollary 7.3.3"
-  static Eigen::VectorXd get_weights(const MeasurementsZd &z_meas, const Gauss_z &z_pred, double prob_of_detection, double clutter_intensity)
+  /**
+   * @brief Gets the weights for the measurements -> see association probabilities in textbook p. 123
+   * "Corollary 7.3.3
+   * @param z_meas The measurements
+   * @param z_pred The predicted measurement
+   * @param prob_of_detection The probability of detection
+   * @param clutter_intensity The clutter intensity
+   * @return The weights for the measurements -> same order as in z_meas
+   * The first element is the weight for no association
+   * length of weights = z_meas.size() + 1
+   */
+  static Eigen::VectorXd get_weights(const MeasurementsZd& z_meas, const Gauss_z& z_pred, double prob_of_detection,
+                                     double clutter_intensity)
   {
     Eigen::VectorXd weights(z_meas.size() + 1);
 
     // in case no measurement assosiates with the target
     double no_association = clutter_intensity * (1 - prob_of_detection);
-    weights(0)            = no_association;
+    weights(0) = no_association;
 
     // measurements associating with the target
-    for (size_t k = 1; k < z_meas.size() + 1; k++) {
+    for (size_t k = 1; k < z_meas.size() + 1; k++)
+    {
       weights(k) = (prob_of_detection * z_pred.pdf(z_meas.at(k - 1)));
     }
 
@@ -108,4 +180,4 @@ template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorMod
   }
 };
 
-} // namespace vortex::filter
+}  // namespace vortex::filter