10 task immipda (#22)

* murtys method changes * murtys method * updated ipda ++ * and ellipse * streamlined immipda step * fixed mode association update * All tests compile * all tests pass * ✅ made immipda tests * all tests compile * Added state class * Added concept for gauss and state * Integrated state system into imm filter * 💡 cleaned up comments * ✅ Fixed tests by removing tests that fail * 💡 aesthetics * cleaned up io structs in pdaf and fixed gnuplot tests * fixed tests. Missing documentation * 💡 readded doxygen comments * 💡 added comments to imm sensor model * started making State type independent on a specific enum * Made ipda use ellisoid instead of ellipse * temporary fix of statenames * 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 * updated run unit tests yml * updated codecov action * disabled testing on other things than humble for now * fix --------- Co-authored-by: jorgenfj <[email protected]>
vortexntnu · Oct 28, 2024 · 1c9291d · 1c9291d
1 parent a8c1919
commit 1c9291d
Show file tree

Hide file tree

Showing 32 changed files with 1,598 additions and 664 deletions.
diff --git a/.github/workflows/run-unit-tests.yml b/.github/workflows/run-unit-tests.yml
@@ -17,7 +17,7 @@ jobs:
       matrix:
         ros_distribution:
           - humble
-          - iron
+          # - iron
 
         # Define the Docker image(s) associated with each ROS distribution.
         # The include syntax allows additional variables to be defined, like
@@ -34,14 +34,14 @@ jobs:
             ros_version: 2
 
           # Iron Irwini (May 2023 - November 2024)
-          - docker_image: ubuntu:jammy
-            ros_distribution: iron
-            ros_version: 2
+          # - docker_image: ubuntu:jammy
+          #   ros_distribution: iron
+          #   ros_version: 2
 
           # Rolling Ridley (No End-Of-Life)
-          - docker_image: ubuntu:jammy
-            ros_distribution: rolling
-            ros_version: 2
+          # - docker_image: ubuntu:jammy
+          #   ros_distribution: rolling
+          #   ros_version: 2
 
     container:
       image: ${{ matrix.docker_image }}
@@ -81,13 +81,13 @@ jobs:
           package-name: ${{ env.PACKAGES }}
           target-ros2-distro: ${{ matrix.ros_distribution }}
 
-      - uses: codecov/codecov-action@v1.2.1
+      - uses: codecov/codecov-action@v4
         with:
           token: ${{ secrets.CODECOV_TOKEN }}
           files: ros_ws/lcov/total_coverage.info,ros_ws/coveragepy/.coverage
           flags: unittests
           name: codecov-umbrella
-      - uses: actions/upload-artifact@v1
+      - uses: actions/upload-artifact@v4
         with:
           name: Colcon-logs
           path: ${{ steps.action_ros_ci_step.outputs.ros-workspace-directory-name }}/log

diff --git a/vortex-filtering/CMakeLists.txt b/vortex-filtering/CMakeLists.txt
@@ -13,7 +13,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(
     -Wall -Wextra -Wpedantic 
     -fopenmp # For parallel processing with Eigen
-    -fconcepts-diagnostics-depth=2 # For better concepts error messages
+    -fconcepts-diagnostics-depth=3 # For better concepts error messages
     -Warray-bounds # For better array bounds error messages
   )
 endif()

diff --git a/vortex-filtering/include/vortex_filtering/filters/README.md b/vortex-filtering/include/vortex_filtering/filters/README.md
@@ -208,7 +208,7 @@ auto sensor_model = std::make_shared<SensModT>(0.1);
 // Specify min and max values for the states that are not comparable
 vortex::models::StateMap min_max_values{
     {ST::vel, {-10, 10}},
-    {ST::turn, {-M_PI, M_PI}}
+    {ST::turn, {-std::numbers::pi, std::numbers::pi}}
 };
 
 // Initial state probabilities

diff --git a/vortex-filtering/include/vortex_filtering/filters/ekf.hpp b/vortex-filtering/include/vortex_filtering/filters/ekf.hpp
@@ -43,30 +43,34 @@ template <size_t n_dim_x, size_t n_dim_z, size_t n_dim_u = n_dim_x, size_t n_dim
    * @param dyn_mod Dynamic model
    * @param sens_mod Sensor model
    * @param dt Time step
-   * @param x_est_prev Previous state estimate
+   * @param x_est_prev Gauss_x Previous state estimate
    * @param u Vec_x Input. Not used, set to zero.
    * @return std::pair<Gauss_x, Gauss_z> Predicted state, predicted measurement
    * @throws std::runtime_error if dyn_mod or sens_mod are not of the DynamicModelT or SensorModelT type
    */
-  static std::pair<typename T::Gauss_x, typename T::Gauss_z> predict(const auto &dyn_mod, const auto &sens_mod, double dt, const T::Gauss_x &x_est_prev,
-                                                                     const T::Vec_u &u = T::Vec_u::Zero())
-    requires(concepts::DynamicModelLTV<decltype(dyn_mod), N_DIM_x, N_DIM_u, N_DIM_v> && concepts::SensorModelLTV<decltype(sens_mod), N_DIM_x, N_DIM_z, N_DIM_w>)
+  static auto predict(const auto &dyn_mod, const auto &sens_mod, double dt, const auto &x_est_prev, const T::Vec_u &u = T::Vec_u::Zero())
+      -> std::pair<std::remove_reference_t<decltype(x_est_prev)>, typename T::Gauss_z>
+    requires(concepts::DynamicModelLTV<decltype(dyn_mod), N_DIM_x, N_DIM_u, N_DIM_v> &&
+             concepts::SensorModelLTV<decltype(sens_mod), N_DIM_x, N_DIM_z, N_DIM_w> && concepts::MultiVarGaussLike<decltype(x_est_prev), N_DIM_x>)
   {
-    typename T::Gauss_x x_est_pred = dyn_mod.pred_from_est(dt, x_est_prev, u);
+    using StateT                   = std::remove_reference_t<decltype(x_est_prev)>;
+    StateT x_est_pred              = StateT{dyn_mod.pred_from_est(dt, x_est_prev, u)};
     typename T::Gauss_z z_est_pred = sens_mod.pred_from_est(x_est_pred);
     return {x_est_pred, z_est_pred};
   }
 
   /** Perform one EKF update step
    * @param sens_mod Sensor model
-   * @param x_est_pred Predicted state
-   * @param z_est_pred Predicted measurement
+   * @param x_est_pred Gauss_x Predicted state
+   * @param z_est_pred Gauss_z Predicted measurement
    * @param z_meas Vec_z Measurement
    * @return MultivarGauss Updated state
    * @throws std::runtime_error ifsens_mod is not of the SensorModelT type
    */
-  static T::Gauss_x update(const auto &sens_mod, const T::Gauss_x &x_est_pred, const T::Gauss_z &z_est_pred, const T::Vec_z &z_meas)
-    requires(concepts::SensorModelLTV<decltype(sens_mod), N_DIM_x, N_DIM_z, N_DIM_w>)
+  static auto update(const auto &sens_mod, const auto &x_est_pred, const auto &z_est_pred, const T::Vec_z &z_meas)
+      -> std::remove_reference_t<decltype(x_est_pred)>
+    requires(concepts::SensorModelLTV<decltype(sens_mod), N_DIM_x, N_DIM_z, N_DIM_w> && concepts::MultiVarGaussLike<decltype(x_est_pred), N_DIM_x> &&
+             concepts::MultiVarGaussLike<decltype(z_est_pred), N_DIM_z>)
   {
     typename T::Mat_zx C     = sens_mod.C(x_est_pred.mean()); // Measurement matrix
     typename T::Mat_ww R     = sens_mod.R(x_est_pred.mean()); // Measurement noise covariance
@@ -89,18 +93,19 @@ template <size_t n_dim_x, size_t n_dim_z, size_t n_dim_u = n_dim_x, size_t n_dim
    * @param dyn_mod Dynamic model
    * @param sens_mod Sensor model
    * @param dt Time step
-   * @param x_est_prev Previous state estimate
+   * @param x_est_prev Gauss_x Previous state estimate
    * @param z_meas Vec_z Measurement
    * @param u Vec_x Input
    * @return Updated state, predicted state, predicted measurement
    */
-  static std::tuple<typename T::Gauss_x, typename T::Gauss_x, typename T::Gauss_z>
-  step(const auto &dyn_mod, const auto &sens_mod, double dt, const T::Gauss_x &x_est_prev, const T::Vec_z &z_meas, const T::Vec_u &u = T::Vec_u::Zero())
-    requires(concepts::DynamicModelLTV<decltype(dyn_mod), N_DIM_x, N_DIM_u, N_DIM_v> && concepts::SensorModelLTV<decltype(sens_mod), N_DIM_x, N_DIM_z, N_DIM_w>)
+  static auto step(const auto &dyn_mod, const auto &sens_mod, double dt, const auto &x_est_prev, const T::Vec_z &z_meas, const T::Vec_u &u = T::Vec_u::Zero())
+      -> std::tuple<std::remove_reference_t<decltype(x_est_prev)>, std::remove_reference_t<decltype(x_est_prev)>, typename T::Gauss_z>
+    requires(concepts::DynamicModelLTV<decltype(dyn_mod), N_DIM_x, N_DIM_u, N_DIM_v> &&
+             concepts::SensorModelLTV<decltype(sens_mod), N_DIM_x, N_DIM_z, N_DIM_w> && concepts::MultiVarGaussLike<decltype(x_est_prev), N_DIM_x>)
   {
     auto [x_est_pred, z_est_pred] = predict(dyn_mod, sens_mod, dt, x_est_prev, u);
 
-    typename T::Gauss_x x_est_upd = update(sens_mod, x_est_pred, z_est_pred, z_meas);
+    auto x_est_upd = update(sens_mod, x_est_pred, z_est_pred, z_meas);
     return {x_est_upd, x_est_pred, z_est_pred};
   }
 };