Feature/pdaf

vortex-filtering/CMakeLists.txt

@@ -23,7 +23,8 @@ find_package(ament_cmake    REQUIRED)
 find_package(rclcpp         REQUIRED)
 find_package(std_msgs       REQUIRED)
 find_package(geometry_msgs  REQUIRED)
-find_package(Eigen3         REQUIRED)
+find_package(Eigen3 3.4.0   REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
 find_package(OpenMP         REQUIRED) # For parallel processing with Eigen
 
 find_package(Gnuplot REQUIRED)
@@ -81,6 +82,7 @@ install(TARGETS ${PROJECT_NAME}_lib
 
 # === Export libraries ===
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(eigen3_cmake_module)
 ament_export_dependencies(${lib_deps}) # Export the dependencies so the package using this package can find them
 
 

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

@@ -6,9 +6,11 @@ All classes and functions are under the namespace `vortex::filters`.
 Here is a great intro to Kalman filters in general: [Kalman and Bayesian Filters in Python](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/00-Preface.ipynb)
 
 ## Overview
-- [`ekf.hpp`](ekf.hpp) contains the EKF filter
-- [`ukf.hpp`](ukf.hpp) contains the UKF filter
+- [`ekf.hpp`](ekf.hpp) contains the EKF
+- [`ukf.hpp`](ukf.hpp) contains the UKF
 - [`imm_filter.hpp`](imm_filter.hpp) contains the IMM filter
+- [`pdaf.hpp`](pdaf.hpp) contains the pdaf
+- [`ipda.hpp`](ipda.hpp) contains the ipda filter
 
 ### EKF
 This class represents an [Extended Kalman Filter](https://en.wikipedia.org/wiki/Extended_Kalman_filter). It is a template class with parameters `DynamicModelT` and `SensorModelT` for the dynamic model and sensor model respectively. It works with models derived from `vortex::models::DynamicModelLTV` and `vortex::models::SensorModelLTV`. All methods are static, so there is no need to create an instance of this class.
@@ -226,4 +228,91 @@ Vec2d z_meas{48, 65};
 using ImmFilter = vortex::filters::IMMFilter<SensModT, IMM>;
 auto [weights_upd, x_est_upds, x_est_preds, z_est_preds] = 
     IMM::step(imm_model, sensor_model, dt, x_est_prevs, z_meas, model_weights, states_min_max);
-```
+```
+
+### PDAF
+
+This class represents the **P**robalistic **D**ata **A**ssociation **F**ilter. It is implemented according to the textbook "Fundamentals of Sensor Fusion" (Chapter 7) by *Edmund Brekke*. The class works as a "toolbox" to provide the usage of the PDAF. Other classes can use the given functions to implement the PDAF for their particular tasks.
+
+However, the PDAF is mainly used by us for target tracking. Additionally, PDAF is **single**-target tracking. That means that all other measurements (besides the tracked target) are considered to be clutter. If multiple targets should be tracked, multiple single-target trackers can be initialized. Notable here is that the PDAF itself doesn't have to be initialized. In fact, it can't be initialized. It just provides static functions. Therefore, you can use the namespace and, with it, the PDAF functions.
+
+The main usage of the PDAF is the function **step()**. It will predict the next state of a given target. The step function will need several parameters to work:
+* Dynamic model,
+* sensor model,
+* current time step,
+* x estimation (the last known state of the target),
+* z measurements,
+* and config (which is a struct that holds:)
+    * mahalanobis threshold
+    * minimum gate threshold
+    * maximum gate threshold
+    * probability of detection
+    * clutter intensity
+
+#### Dynamic/Sensor Models
+
+If you want to use the PDAF for your measurements, you have to define how your measurements behave. That is what the models are for. The **dynamic** model (or transition model) describes how states change over time. A common example of this would be velocity. The **sensor** model is a bit less intuitive and difficult to explain. Let's consider the following example:
+We want to measure the temperature inside a rocket drive. We know in theory how the temperature should change over time. That would be our dynamic model. We basically want to compare this model with the actual measurements and find a good estimated final value for the current state of the temperature in the drive. It's a bit of a design choice if you trust the model or the actual measurements. Both could be accurate or wrong. The main problem is that we can't put sensors directly into the drive (they would melt and be destroyed instantly). Therefore, we put a sensor outside the rocket and measure the temperature there. Here, we have to know how the temperature we measure corresponds to the actual temperature inside the drive. In other words, the temperature that we measure is not the actual value we need. But we know how to convert the measurement to the value we search for. This is the sensor model. If you want to get a better explanation of those models, I suggest using the Textbook "Artificial Intelligence: A Modern Approach, 4th Edition" by *Stuart Russell* and *Peter Norvig*. The book explains those models in Chapter 14 - *Probabilistic Reasoning Over Time*. All in all, you have to define those models and pass them to the function. Under the name space **vortex::models**, you can find simple predefined models to use:
+
+```c++
+// example how to define models using vortex::models
+using DynMod = vortex::models::ConstantVelocity;
+using SensorMod = vortex::models::IdentitySensorModel<4, 2>;
+// example how to use PDAF in pratice
+using PDAF = vortex::filter::PDAF<DynMod, SensorMod>;
+auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(parameters...)
+```
+
+#### Previous State Estimate
+The step function needs to know what the previous state was. Based on this state, the dynamic model will be used. The model will give us a predicted new state. This state will be compared to the actual measurements. The previous state must be a Gaussian distribution. 
+
+#### Measurements
+The perceived measurements. This parameter consists of an Eigen vector. It should hold all perceived measurements. (The dimension must be the same as defined in the models.)
+
+#### Basic Principle
+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 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.
+
+It is highly recommended to look into "Fundamentals of Sensor Fusion" (Chapter 7) by *Edmund Brekke*. You will find there a visualization of the explained process, which makes it easier to understand. Also, the implemented tests for the PDAF will plot the given outcome. This will also help to understand what is happening.
+In order for the test to work with visualization you have to uncomment following statement in the CMakeLists.txt of *vortex-filtering*.
+```
+if(BUILD_TESTING)
+  # add_compile_definitions(GNUPLOT_ENABLE=1) <- uncomment this 
+  add_subdirectory(test)
+endif()
+```
+
+#### Other Parameters
+
+* Current time step: The current iteration.
+* Probability of detection: This is the probability that we will detect the target. Defining this, for example, with 0.8 means that there is a 20% chance that no measurement is assumed to be the target. (In this case, we will only take the dynamic model into consideration)
+* Clutter intensity: Here you can define how much clutter you expect within the region.
+
+#### Returns
+
+The step()-function will return return the new estimated state and all outputs of the substeps. Returning those in addition, enables us to visualize the outputs of the PDAF.
+
+### IPDA
+
+IPDA stand for **I**ntegrated **P**robabilistic **D**ata **A**ssociation. It uses extra steps in addition to the regular PDAF. The PDAF is extended to also include a probability value which describes the target's existence. The main reason for this is to evaluate not only the state of the target but also the probability of existence dependent on the previous states.
+IPDA works in terms of usage the same way the PDAF works. The class provides a static function (also called *step*). You can use this function by referencing the namespace.
+
+```c++
+using ConstantVelocity = vortex::models::ConstantVelocity;
+using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>;
+using IPDA = vortex::filter::IPDA<ConstantVelocity, IdentitySensorModel>;
+
+auto [x_final, existence_pred, inside, outside, x_pred, z_pred, x_updated] = IPDA::step(parameters...);
+```
+
+#### Parameters
+The parameters of the step() function are mainly the same as those of the PDAF. However, two parameters are added:
+* Survival estimate: This is the survival estimate of the previous state estimate (corresponding to x_estimate). It describes the target existence, which we also want to estimate for the current state.
+* Probability of survival: This is a general hyperparameter that defines how likely a target is to survive. This parameter shouldn't be confused with the probability of detection. If no measurement is considered to correspond to the target, we consider the dynamic model. The track still *survives*. If a target *dies*, it can't come back, and the track will be deleted.
+
+#### Returns
+In addition to all the return parameters of the PDAF the IPDA will return the estimated existence prediction for the current state (corresponding to x_final). 

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

@@ -0,0 +1,75 @@
+#pragma once
+#include <vector>
+#include <Eigen/Dense>
+#include <vortex_filtering/filters/pdaf.hpp>
+#include <vortex_filtering/models/dynamic_model_interfaces.hpp>
+#include <vortex_filtering/models/sensor_model_interfaces.hpp>
+
+namespace vortex::filter
+{
+template <concepts::DynamicModelLTVWithDefinedSizes DynModT, concepts::SensorModelLTVWithDefinedSizes SensModT>
+class IPDA
+{
+public:
+  static constexpr int N_DIM_x = DynModT::N_DIM_x;
+  static constexpr int N_DIM_z = SensModT::N_DIM_z;
+  static constexpr int N_DIM_u = DynModT::N_DIM_u;
+  static constexpr int N_DIM_v = DynModT::N_DIM_v;
+  static constexpr int N_DIM_w = SensModT::N_DIM_w;
+
+  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 GaussMix = vortex::prob::GaussianMixture<N_DIM_x>;
+  using PDAF = vortex::filter::PDAF<vortex::models::ConstantVelocity, vortex::models::IdentitySensorModel<4, 2>>;
+
+  IPDA() = delete;
+
+  struct Config : public PDAF::Config
+  {
+    double prob_of_survival = 1.0;
+  };
+
+  /**
+   * @brief Calculates the existence probability of an object.
+   * @param measurements The measurements to iterate over.
+   * @param probability_of_survival How likely the object is to survive (Ps).
+   * @param last_detection_probability_ The last detection probability.
+   * @param probability_of_detection How likely the object is to be detected (Pd).
+   * @param clutter_intensity How likely it is to have a false positive.
+   * @param z_pred The predicted measurement.
+   * @return The existence probability.
+   */
+  static double get_existence_probability(const std::vector<Vec_z>& measurements, double probability_of_survival,
+                                          double last_detection_probability_, double probability_of_detection,
+                                          double clutter_intensity, Gauss_z& z_pred)
+  {
+    double predicted_existence_probability = probability_of_survival * last_detection_probability_;  // Finds Ps
+
+    double sum = 0.0;
+    for (const Vec_z& measurement : measurements)
+    {
+      sum += z_pred.pdf(measurement);
+    }
+    double l_k = 1 - probability_of_detection + probability_of_detection / clutter_intensity * sum;
+
+    return (l_k * predicted_existence_probability) / (1 - (1 - l_k) * predicted_existence_probability);
+  }
+
+  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)
+  {
+    auto [x_final, inside, outside, x_pred, z_pred, x_updated] =
+        PDAF::step(dyn_model, sen_model, timestep, x_est, z_meas, static_cast<PDAF::Config>(config));
+
+    double existence_probability = get_existence_probability(
+        inside, config.prob_of_survival, survive_est, config.prob_of_detection, config.clutter_intensity, z_pred);
+    return { x_final, existence_probability, inside, outside, x_pred, z_pred, x_updated };
+  }
+};
+}  // namespace vortex::filter

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

@@ -0,0 +1,111 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+#include <vortex_filtering/vortex_filtering.hpp>
+
+namespace vortex::filter {
+
+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;
+  static constexpr int N_DIM_u = DynModT::N_DIM_u;
+  static constexpr int N_DIM_v = DynModT::N_DIM_v;
+  static constexpr int N_DIM_w = SensModT::N_DIM_w;
+
+  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 MeasurementsZd = std::vector<Vec_z>;
+  using StatesXd       = std::vector<Gauss_x>;
+  using GaussMixZd     = vortex::prob::GaussianMixture<N_DIM_x>;
+
+  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;
+  };
+
+  PDAF() = delete;
+
+  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)
+  {
+    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) {
+      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};
+  }
+
+  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) {
+      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) {
+        inside_meas.push_back(measurement);
+      }
+      else {
+        outside_meas.push_back(measurement);
+      }
+    }
+
+    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)
+  {
+    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);
+
+    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)
+  {
+    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;
+
+    // measurements associating with the target
+    for (size_t k = 1; k < z_meas.size() + 1; k++) {
+      weights(k) = (prob_of_detection * z_pred.pdf(z_meas.at(k - 1)));
+    }
+
+    // normalize weights
+    weights /= weights.sum();
+
+    return weights;
+  }
+};
+
+} // namespace vortex::filter

vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp

@@ -32,7 +32,7 @@ template <size_t n_dims> class MultiVarGauss {
   // Default constructor
   MultiVarGauss() = default;
 
-  /** Calculate the probability density function of x given the Gaussian
+  /** Calculate the probability density of x given the Gaussian
    * @param x
    * @return double
    */

vortex-filtering/package.xml

@@ -8,17 +8,23 @@
   <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+  <build_depend>eigen</build_depend>
 
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
-  <depend>eigen</depend>
+
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>gnuplot</test_depend>
   <test_depend>gnuplot-iostream</test_depend>
+
+  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
+  <build_export_depend>eigen</build_export_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

vortex-filtering/test/CMakeLists.txt

@@ -8,7 +8,9 @@ ament_add_gtest(${PROJECT_NAME}_test
   dynamic_model_test.cpp
   ekf_test.cpp
   imm_test.cpp
+  ipda_test.cpp
   numerical_integration_test.cpp
+  pdaf_test.cpp
   probability_test.cpp
   sensor_model_test.cpp
   types_test.cpp