diff --git a/.github/workflows/run-unit-tests.yml b/.github/workflows/run-unit-tests.yml index 22a63565..6e9d9324 100644 --- 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 index b49d0ea3..e573768d 100644 --- 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 index 28eeec68..4c21c3e4 100644 --- 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(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 index 4f0fc9fe..13d34d3c 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ekf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ekf.hpp @@ -43,30 +43,34 @@ template 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 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 && concepts::SensorModelLTV) + 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, typename T::Gauss_z> + requires(concepts::DynamicModelLTV && + concepts::SensorModelLTV && concepts::MultiVarGaussLike) { - typename T::Gauss_x x_est_pred = dyn_mod.pred_from_est(dt, x_est_prev, u); + using StateT = std::remove_reference_t; + 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) + 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 + requires(concepts::SensorModelLTV && concepts::MultiVarGaussLike && + concepts::MultiVarGaussLike) { 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 - 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 && concepts::SensorModelLTV) + 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, typename T::Gauss_z> + requires(concepts::DynamicModelLTV && + concepts::SensorModelLTV && concepts::MultiVarGaussLike) { 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}; } }; diff --git a/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp b/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp index eec34c04..9dfbd2a3 100644 --- a/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -26,18 +27,22 @@ namespace vortex::filter { -template class ImmFilter { +template class ImmFilter { public: static constexpr size_t N_MODELS = ImmModelT::N_MODELS; static constexpr auto N_DIMS_x = ImmModelT::N_DIMS_x; + static constexpr auto N_DIMS_u = ImmModelT::N_DIMS_u; + static constexpr auto N_DIMS_v = ImmModelT::N_DIMS_v; static constexpr int N_DIM_z = SensModT::N_DIM_z; + static constexpr int N_DIM_w = SensModT::N_DIM_w; - template using T = Types_xz; + template using T = Types_xz; - template using DynModT = typename ImmModelT::template DynModT; + template using DynModT = typename ImmModelT::template DynModT; using Vec_n = Eigen::Vector; + using Arr_nb = Eigen::Array; using Mat_nn = Eigen::Matrix; using Vec_z = typename T<0>::Vec_z; using Gauss_z = typename T<0>::Gauss_z; @@ -60,8 +65,8 @@ template {}); + return [&](std::index_sequence) -> GaussTuple_x { + return {mix_one_component(x_est_prevs, mixing_probs.col(s_k), states_min_max)...}; + }(std::make_index_sequence{}); + } /** @@ -94,7 +101,17 @@ template mode_matched_filter(const ImmModelT &imm_model, const SensModT &sensor_model, double dt, const GaussTuple_x &moment_based_preds, const Vec_z &z_meas) { - return step_kalman_filters(imm_model, sensor_model, dt, moment_based_preds, z_meas, std::make_index_sequence{}); + GaussTuple_x x_est_upds; + GaussTuple_x x_est_preds; + GaussArr_z z_est_preds; + + [&](std::index_sequence) { + ((std::tie(std::get(x_est_upds), std::get(x_est_preds), z_est_preds.at(s_k)) = + kalman_filter_step(imm_model.template get_model(), sensor_model, dt, std::get(moment_based_preds), z_meas)), + ...); + }(std::make_index_sequence{}); + + return {x_est_upds, x_est_preds, z_est_preds}; } /** @@ -111,8 +128,8 @@ template step(const ImmModelT &imm_model, const SensModT &sensor_model, double dt, const GaussTuple_x &x_est_prevs, const Vec_z &z_meas, const Vec_n &weights, - const models::StateMap &states_min_max = {}) + const StateMap &states_min_max = {}) { Mat_nn transition_matrix = imm_model.get_pi_mat_d(dt); Mat_nn mixing_probs = calculate_mixing_probs(transition_matrix, weights); - GaussTuple_x moment_based_preds = mixing(x_est_prevs, mixing_probs, imm_model.get_all_state_names(), states_min_max); + GaussTuple_x moment_based_preds = mixing(x_est_prevs, mixing_probs, states_min_max); auto [x_est_upds, x_est_preds, z_est_preds] = mode_matched_filter(imm_model, sensor_model, dt, moment_based_preds, z_meas); Vec_n weights_upd = update_probabilities(mixing_probs, z_est_preds, z_meas, weights); return {weights_upd, x_est_upds, x_est_preds, z_est_preds}; } -private: - /** - * @brief Calculate the Kalman filter outputs for each mode. - * @tparam Is Indices of models - * @param imm_model The IMM model. - * @param sensor_model The sensor model. - * @param dt double Time step - * @param moment_based_preds Moment-based predictions - * @param z_meas Vec_z Measurement - * @return Tuple of updated states, predicted states, predicted measurements - */ - template - static std::tuple step_kalman_filters(const ImmModelT &imm_model, const SensModT &sensor_model, double dt, - const GaussTuple_x &moment_based_preds, const Vec_z &z_meas, - std::index_sequence) + static std::tuple kalman_filter_predictions(const ImmModelT &imm_model, const SensModT &sensor_model, double dt, + const GaussTuple_x &x_est_prevs) { + GaussTuple_x x_est_preds; + GaussArr_z z_est_preds; + + [&](std::index_sequence) { + ((std::tie(std::get(x_est_preds), z_est_preds.at(s_k)) = + kalman_filter_predict(imm_model.template get_model(), sensor_model, dt, std::get(x_est_prevs))), + ...); + }(std::make_index_sequence{}); - // Calculate mode-matched filter outputs and save them in a tuple of tuples - std::tuple::Gauss_x, typename T::Gauss_x, Gauss_z>...> ekf_outs; - ((std::get(ekf_outs) = step_kalman_filter(imm_model.template get_model(), sensor_model, dt, std::get(moment_based_preds), z_meas)), ...); + return {x_est_preds, z_est_preds}; + } - // Convert tuple of tuples to tuple of tuples + static GaussTuple_x kalman_filter_updates(const ImmModelT &imm_model, const SensModT &sensor_model, double dt, const GaussTuple_x &x_est_preds, + const GaussArr_z &z_est_preds, const Vec_z &z_meas, const Arr_nb &gated_measurements = Arr_nb::Ones()) + { GaussTuple_x x_est_upds; - GaussTuple_x x_est_preds; - GaussArr_z z_est_preds; - ((std::get(x_est_upds) = std::get<0>(std::get(ekf_outs))), ...); - ((std::get(x_est_preds) = std::get<1>(std::get(ekf_outs))), ...); - ((z_est_preds.at(Is) = std::get<2>(std::get(ekf_outs))), ...); + [&](std::index_sequence) { + ((std::get(x_est_upds) = + gated_measurements(s_k) + ? kalman_filter_update(imm_model.template get_model(), sensor_model, dt, std::get(x_est_preds), z_est_preds.at(s_k), z_meas) + : std::get(x_est_preds)), + ...); + }(std::make_index_sequence{}); - return {x_est_upds, x_est_preds, z_est_preds}; + return x_est_upds; } /** * @brief Calculate the Kalman filter outputs for one mode. If the model isn't LTV, use the ukf instead of the ekf. - * @tparam i Index of model + * @tparam s_k Index of model * @param imm_model The IMM model. * @param sensor_model The sensor model. * @param dt double Time step @@ -188,95 +203,94 @@ template - static std::tuple::Gauss_x, typename T::Gauss_x, Gauss_z> step_kalman_filter(const DynModT &dyn_model, const SensModT &sensor_model, - double dt, const T::Gauss_x &x_est_prev, const Vec_z &z_meas) + template + static std::tuple::Gauss_x, typename T::Gauss_x, Gauss_z> + kalman_filter_step(const DynModT &dyn_model, const SensModT &sensor_model, double dt, const T::Gauss_x &x_est_prev, const Vec_z &z_meas) { - static constexpr size_t N_DIM_x = ImmModelT::N_DIM_x(i); - static constexpr size_t N_DIM_z = SensModT::N_DIM_z; - static constexpr size_t N_DIM_u = ImmModelT::N_DIM_u(i); - static constexpr size_t N_DIM_v = ImmModelT::N_DIM_v(i); - static constexpr size_t N_DIM_w = SensModT::N_DIM_w; - - if constexpr (concepts::DynamicModelLTVWithDefinedSizes> && concepts::SensorModelLTVWithDefinedSizes) { - using ImmSensMod = models::ImmSensorModelLTV; - using EKF = filter::EKF_t; + if constexpr (concepts::DynamicModelLTVWithDefinedSizes> && concepts::SensorModelLTVWithDefinedSizes) { + using ImmSensMod = models::ImmSensorModelLTV; + using EKF = filter::EKF_t; ImmSensMod imm_sens_mod{sensor_model}; return EKF::step(dyn_model, imm_sens_mod, dt, x_est_prev, z_meas); } else { - using ImmSensMod = models::ImmSensorModel; - using UKF = filter::UKF_t; + using ImmSensMod = models::ImmSensorModel; + using UKF = filter::UKF_t; ImmSensMod imm_sens_mod{sensor_model}; return UKF::step(dyn_model, imm_sens_mod, dt, x_est_prev, z_meas); } } - /** Helper function to mix the components (modes) of the IMM filter - * @tparam model_indices - * @param x_est_prevs Gaussians from previous time step - * @param mixing_probs Mixing probabilities - * @param state_names Names of the states - * @param states_min_max The minimum and maximum value each state can take - * @return std::tuple...> - */ - template - static std::tuple::Gauss_x...> mix_components(const GaussTuple_x &x_est_prevs, const Mat_nn &mixing_probs, - const ImmModelT::StateNames &state_names, const models::StateMap &states_min_max, - std::integer_sequence) + template + static std::tuple::Gauss_x, Gauss_z> kalman_filter_predict(const DynModT &dyn_model, const SensModT &sensor_model, double dt, + const T::Gauss_x &x_est_prev) + { + if constexpr (concepts::DynamicModelLTVWithDefinedSizes> && concepts::SensorModelLTVWithDefinedSizes) { + using ImmSensMod = models::ImmSensorModelLTV; + using EKF = filter::EKF_t; + ImmSensMod imm_sens_mod{sensor_model}; + return EKF::predict(dyn_model, imm_sens_mod, dt, x_est_prev); + } + else { + using ImmSensMod = models::ImmSensorModel; + using UKF = filter::UKF_t; + ImmSensMod imm_sens_mod{sensor_model}; + return UKF::predict(dyn_model, imm_sens_mod, dt, x_est_prev); + } + } + + template + static T::Gauss_x kalman_filter_update(const DynModT &dyn_model, const SensModT &sensor_model, double dt, const T::Gauss_x &x_est_pred, + const T::Gauss_z &z_est_pred, const Vec_z &z_meas) { - return {mix_one_component(x_est_prevs, mixing_probs.col(model_indices), state_names, states_min_max)...}; + if constexpr (concepts::DynamicModelLTVWithDefinedSizes> && concepts::SensorModelLTVWithDefinedSizes) { + using ImmSensMod = models::ImmSensorModelLTV; + using EKF = filter::EKF_t; + ImmSensMod imm_sens_mod{sensor_model}; + return EKF::update(imm_sens_mod, x_est_pred, z_est_pred, z_meas); + } + else { + using ImmSensMod = models::ImmSensorModel; + using UKF = filter::UKF_t; + ImmSensMod imm_sens_mod{sensor_model}; + return UKF::update(dyn_model, imm_sens_mod, dt, x_est_pred, z_est_pred, z_meas); + } } /** Helper function to mix one component (mode) of the IMM filter. It mixes all the components with the target model. * @tparam target_model_index The model to mix the other models into * @param x_est_prevs Gaussians from previous time step * @param weights Weights (column of the mixing_probs matrix corresponding to the target model index) - * @param state_names Names of the states * @param states_min_max The minimum and maximum value each state can take (optional, but can lead to better performance) * @return Gauss_x The updated model after mixing it with the other models * @note This is the function that actually does the mixing of the models. It is called for each model in the IMM filter. */ template - static T::Gauss_x mix_one_component(const GaussTuple_x &x_est_prevs, const Vec_n &weights, const ImmModelT::StateNames &state_names, - const models::StateMap &states_min_max = {}) + static T::Gauss_x mix_one_component(const GaussTuple_x &x_est_prevs, const Vec_n &weights, const StateMap &states_min_max = {}) { - constexpr size_t N_DIM_x = ImmModelT::N_DIM_x(target_model_index); - using GaussMix_x = prob::GaussianMixture; - auto moment_based_preds = prepare_models(x_est_prevs, state_names, states_min_max, std::make_index_sequence{}); - return GaussMix_x{weights, moment_based_preds}.reduce(); - } + auto moment_based_preds = + [&](std::index_sequence) -> std::array::Gauss_x, N_MODELS> { + return {prepare_mixing_model(x_est_prevs, states_min_max)...}; + }(std::make_index_sequence{}); - /** Helper function to prepare the models for mixing in case of mismatching dimensions or state names - * @tparam target_model_index The model to mix the other models into - * @tparam mixing_model_indices The models to mix into the target model - * @param x_est_prevs Gaussians from previous time step - * @param state_names Names of the states - * @param states_min_max The minimum and maximum value each state can take - * @return std::array, N_MODELS> - */ - template - static std::array::Gauss_x, N_MODELS> prepare_models(const GaussTuple_x &x_est_prevs, const ImmModelT::StateNames &state_names, - const models::StateMap &states_min_max, - std::integer_sequence) - { - return {prepare_mixing_model(x_est_prevs, state_names, states_min_max)...}; + return typename T::GaussMix_x{weights, moment_based_preds}.reduce(); } - /** Fit the size of the mixing_model in case it doesn't have the same dimensions or states as the target model. + /** + * @brief Fit the size and states of the mixing_model in case it doesn't have the same dimensions or states as the target model. + * A map of the minimum and maximum values for each state (`states_min_max`) can be provided to initialize the missing states. + * If the `states_min_max` map is not provided, the missing states are initialized with the mean and covariance from the target model. * @tparam target_model_index The model to mix the other models into * @tparam mixing_model_index The model to mix into the target model * @param x_est_prevs Gaussians from previous time step - * @param state_names Names of the states - * @param states_min_max The minimum and maximum value each state can take (optional, but can lead to better performance) + * @param states_min_max The minimum and maximum value each state can take (optional, but can lead to better filter performance) * @return Gauss_x Modified `mixing_model` to fit the size and types of `target_model` * @note - If the sizes and state names of the mixing model and the target model are the same, the mixing model is returned as is. * @note - If the sizes are different, the mixing model is modified to fit the size of the target model and the possibly missing states * are initialized with the mean and covariance from the target model or with a uniform distribution if `states_min_max` is provided. */ template - static T::Gauss_x prepare_mixing_model(const GaussTuple_x &x_est_prevs, const ImmModelT::StateNames &state_names, - const models::StateMap &states_min_max = {}) + static T::Gauss_x prepare_mixing_model(const GaussTuple_x &x_est_prevs, const StateMap &states_min_max = {}) { if constexpr (target_model_index == mixing_model_index) { return std::get(x_est_prevs); @@ -286,45 +300,48 @@ template ; using Mat_xx = Eigen::Matrix; using Uniform = prob::Uniform<1>; using Vec_x_b = Eigen::Vector; using Mat_xx_b = Eigen::Matrix; - auto target_state_names = std::get(state_names); - auto mixing_state_names = std::get(state_names); - auto matching_states = matching_state_names(target_state_names, mixing_state_names); - - bool all_states_match = std::apply([](auto... b) { return (b && ...); }, matching_states); + auto target_state = std::get(x_est_prevs); + auto mixing_state = std::get(x_est_prevs); Vec_x x = Vec_x::Zero(); Mat_xx P = Mat_xx::Zero(); - x.template head() = std::get(x_est_prevs).mean().template head(); - P.template topLeftCorner() = std::get(x_est_prevs).cov().template topLeftCorner(); + x.template head() = mixing_state.mean().template head(); + P.template topLeftCorner() = mixing_state.cov().template topLeftCorner(); + + constexpr auto target_state_names = std::get(ImmModelT::ALL_STATE_NAMES); + constexpr auto mixing_state_names = std::get(ImmModelT::ALL_STATE_NAMES); + constexpr auto matching_states = models::matching_state_names(target_state_names, mixing_state_names); + + constexpr bool all_states_match = std::apply([](auto... b) { return (b && ...); }, matching_states); - if (all_states_match) { + if constexpr (all_states_match) { return {x, P}; } - Vec_x_b matching_states_vec_b = Eigen::Map(matching_state_names(target_state_names, mixing_state_names).data()); + Vec_x_b matching_states_vec_b = Eigen::Map(models::matching_state_names(target_state_names, mixing_state_names).data()); Vec_x matching_states_vec = matching_states_vec_b.template cast(); Mat_xx_b matching_states_mat_b = matching_states_vec_b * matching_states_vec_b.transpose(); Mat_xx matching_states_mat = matching_states_mat_b.template cast(); + // Set the states and covariances that are not in the target model to 0 x = x.cwiseProduct(matching_states_vec); P = P.cwiseProduct(matching_states_mat); for (size_t i = 0; i < N_DIM_target; i++) { if (matching_states_vec(i)) continue; - ST state_name = target_state_names.at(i); + StateName state_name = target_state_names.at(i); if (!states_min_max.contains(state_name)) { - x(i) = std::get(x_est_prevs).mean()(i); - P(i, i) = std::get(x_est_prevs).cov()(i, i); + x(i) = target_state.mean()(i); + P(i, i) = target_state.cov()(i, i); continue; } double min = states_min_max.at(state_name).min; diff --git a/vortex-filtering/include/vortex_filtering/filters/immipda.hpp b/vortex-filtering/include/vortex_filtering/filters/immipda.hpp new file mode 100644 index 00000000..da5089db --- /dev/null +++ b/vortex-filtering/include/vortex_filtering/filters/immipda.hpp @@ -0,0 +1,190 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vortex::filter { + +namespace config { +struct IMMIPDA { + StateMap states_min_max; +}; +} // namespace config + +template class IMMIPDA { +public: + template + using T = Types_xzuvw; + using ImmFilter_ = ImmFilter; + using Arr_nXd = Eigen::Array; + using Arr_1Xd = Eigen::Array; + using Arr_nXb = Eigen::Array; + + template using PDAF_ = PDAF, SensModT>; + template using IPDA_ = IPDA, SensModT>; + + struct Config { + config::PDAF pdaf; + config::IPDA ipda; + config::IMMIPDA immipda; + }; + + struct State { + ImmModelT::GaussTuple_x x_estimate; + ImmModelT::Vec_n mode_probabilities; + double existence_probability; + }; + + struct Output { + State state; + ImmModelT::GaussTuple_x x_preds; + ImmFilter_::GaussArr_z z_preds; + std::vector x_upds; + Arr_nXb gated_measurements; + }; + + IMMIPDA() = delete; + + /** + * @brief Do one step of IMMIPDA filtering + * + * @param imm_model IMM model to use + * @param sens_mod Sensor model to use + * @param timestep Time step + * @param state_est_prev Previous state estimate (previous x-estimate, mode probabilities, existence probability) + * @param z_measurements Measurements + * @param config Configuration + * @return std::tuple, std::vector, ImmModelT::GaussTuple_x, + * ImmModelT::GaussTuple_z, std::vector> + */ + static Output step(const ImmModelT &imm_model, const SensModT &sens_mod, double timestep, const State &state_est_prev, const Arr_nXd &z_measurements, + const Config &config) + { + const auto &x_est_prevs = state_est_prev.x_estimate; + const auto &mode_prob_est_prevs = state_est_prev.mode_probabilities; + const auto &existence_prob_est = state_est_prev.existence_probability; + + const size_t m_k = z_measurements.cols(); + // Calculate IMM mixing probabilities + auto transition_matrix = imm_model.get_pi_mat_d(timestep); + auto mixing_probs = ImmFilter_::calculate_mixing_probs(transition_matrix, mode_prob_est_prevs); + + // IMM mixing + auto moment_based_preds = ImmFilter_::mixing(x_est_prevs, mixing_probs, config.immipda.states_min_max); + + // IMM Kalman prediction + auto [x_est_preds, z_est_preds] = ImmFilter_::kalman_filter_predictions(imm_model, sens_mod, timestep, moment_based_preds); + + // Gate measurements + Arr_nXb gated_measurements(ImmModelT::N_MODELS, m_k); + [&](std::index_sequence) { + ((gated_measurements.row(s_k) = PDAF_::apply_gate(z_measurements, z_est_preds.at(s_k), {config.pdaf})), ...); + }(std::make_index_sequence{}); + + // Split measurements + const size_t m_k_inside = (gated_measurements.colwise().any()).count(); + Arr_nXd z_meas_inside(ImmModelT::N_MODELS, m_k_inside); + Arr_nXb gated_meas_inside(ImmModelT::N_MODELS, m_k_inside); + + for (size_t a_k = 0; const auto &z_k : z_measurements.colwise()) { + if (gated_measurements.col(a_k).any()) { + z_meas_inside.col(a_k) = z_k; + gated_meas_inside.col(a_k) = gated_measurements.col(a_k); + } + a_k++; + } + + // IMM Kalman update (7.63) + std::vector imm_estimates(m_k_inside + 1); + imm_estimates.at(0) = x_est_preds; + for (size_t a_k : std::views::iota(1u, m_k_inside + 1)) { + auto z_k = z_meas_inside.col(a_k - 1); + auto gated_k = gated_meas_inside.col(a_k - 1); + imm_estimates.at(a_k) = ImmFilter_::kalman_filter_updates(imm_model, sens_mod, timestep, x_est_preds, z_est_preds, z_k, gated_k); + } + + // IMM mode probabilities (6.37-6.38) + Arr_nXd predicted_mode_probabilities(ImmModelT::N_MODELS, m_k_inside + 1); + predicted_mode_probabilities.col(0) = mode_prob_est_prevs; // TODO: Find what the mode probability is when a_k = 0 + for (size_t a_k : std::views::iota(1u, m_k_inside + 1)) { + auto z_k = z_meas_inside.col(a_k - 1); + predicted_mode_probabilities.col(a_k) = ImmFilter_::update_probabilities(mixing_probs, z_est_preds, z_k, mode_prob_est_prevs); + } + + // Calculate the hypothesis-conditional likelihood (7.61) + // TODO: Verify what the hypothesis-conditional likelihood is when a_k = 0 + // (for now assume it is lambda(1-P_D) for all s_k) + Arr_nXd hypothesis_conditional_likelyhood(ImmModelT::N_MODELS, m_k_inside + 1); + for (size_t s_k = 0; const auto &z_pred_s_k : z_est_preds) { + double P_d = config.pdaf.prob_of_detection; + double lambda = config.pdaf.clutter_intensity; + hypothesis_conditional_likelyhood(s_k, 0) = lambda * (1.0 - P_d); + for (size_t a_k = 1; const auto &z_k : z_meas_inside.colwise()) { + hypothesis_conditional_likelyhood(s_k, a_k) = z_pred_s_k.pdf(z_k); + a_k++; + } + s_k++; + } + + // Calculate the hypothesis likelihoods (7.62) + Eigen::ArrayXd hypothesis_likelihoods(m_k_inside); + for (size_t a_k : std::views::iota(0u, m_k_inside)) { + hypothesis_likelihoods(a_k) = predicted_mode_probabilities.col(a_k).matrix().dot(hypothesis_conditional_likelyhood.col(a_k).matrix()); + } + + // Calculate the accociation probabilities (7.56) + Eigen::ArrayXd association_probabilities(m_k_inside + 1); + association_probabilities = PDAF_<0>::association_probabilities(hypothesis_likelihoods, config.pdaf.prob_of_detection, config.pdaf.clutter_intensity); + + // Calculate the posterior mode probabilities (7.52) + Arr_nXd posterior_mode_probabilities(ImmModelT::N_MODELS, m_k_inside + 1); + posterior_mode_probabilities = hypothesis_conditional_likelyhood * predicted_mode_probabilities; + posterior_mode_probabilities /= posterior_mode_probabilities.colwise().sum().replicate(ImmModelT::N_MODELS, 1); + + // Calculate the mode-conditional association probabilities (7.57) + Arr_nXd mode_conditional_association_probabilities = posterior_mode_probabilities.rowwise() * association_probabilities.transpose(); + typename ImmModelT::Vec_n mode_prob_upd = mode_conditional_association_probabilities.rowwise().sum(); + mode_conditional_association_probabilities /= mode_prob_upd.replicate(1, m_k_inside + 1).array(); + + // PDAF_ mixture reduction (7.58) + typename ImmModelT::GaussTuple_x x_est_upds; + + [&](std::index_sequence) { + std::tuple::Gauss_x>...> imm_estimates_s_k; + + for (const auto &imm_estimates_a_k : imm_estimates) { + (std::get(imm_estimates_s_k).push_back(std::get(imm_estimates_a_k)), ...); + } + + ((std::get(x_est_upds) = typename T::GaussMix_x{mode_conditional_association_probabilities.row(s_k), std::get(imm_estimates_s_k)}.reduce()), + ...); + }(std::make_index_sequence{}); + + // Calculate existence probability (7.32-7.33) + double existence_prob_upd = IPDA_<0>::existence_prob_update(hypothesis_likelihoods, existence_prob_est, {config.pdaf, config.ipda}); + + return { + .state = + { + .x_estimate = x_est_upds, + .mode_probabilities = mode_prob_upd, + .existence_probability = existence_prob_upd, + }, + .x_preds = x_est_preds, + .z_preds = z_est_preds, + .x_upds = imm_estimates, + .gated_measurements = gated_measurements, + }; + } +}; + +} // namespace vortex::filter \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index ab500088..736b7dc0 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -1,30 +1,22 @@ -/** - * @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 #include +#include #include #include #include +#include + +namespace vortex::filter { + +namespace config { +struct IPDA { + double prob_of_survival = 0.99; + bool estimate_clutter = true; + bool update_existence_probability_on_no_detection = true; +}; +} // namespace config -namespace vortex::filter -{ -/** - * @brief The IPDA filter class - * @tparam DynModT The dynamic model type - * @tparam SensModT The sensor model type - */ -template -class IPDA -{ +template class IPDA { public: static constexpr int N_DIM_x = DynModT::N_DIM_x; static constexpr int N_DIM_z = SensModT::N_DIM_z; @@ -34,70 +26,136 @@ class IPDA using T = Types_xzuvw; - using EKF = vortex::filter::EKF_t; - - 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; - using PDAF = vortex::filter::PDAF; + using Arr_zXd = Eigen::Array; + using Arr_1Xb = Eigen::Array; + using EKF = vortex::filter::EKF_t; + using PDAF = vortex::filter::PDAF; IPDA() = delete; - struct Config : public PDAF::Config - { - double prob_of_survival = 1.0; + struct Config { + config::PDAF pdaf; + config::IPDA ipda; + }; + + struct State { + T::Gauss_x x_estimate; + double existence_probability; + }; + + struct Output { + State state; + T::Gauss_x x_prediction; + T::Gauss_z z_prediction; + std::vector x_updates; + Arr_1Xb gated_measurements; }; + static double existence_prediction(double existence_prob_est, double prob_of_survival) + { + double r_km1 = existence_prob_est; + double P_s = prob_of_survival; + return P_s * r_km1; // (7.28) + } + /** - * @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. + * @brief Calculates the existence probability given the measurements and the previous existence probability. + * @param z_measurements The measurements to iterate over. * @param z_pred The predicted measurement. + * @param existence_prob_est (r_{k-1}) The previous existence probability. + * @param config The configuration for the IPDA. * @return The existence probability. */ - static double get_existence_probability(const std::vector& measurements, double probability_of_survival, - double last_detection_probability_, double probability_of_detection, - double clutter_intensity, Gauss_z& z_pred) + static double existence_prob_update(const Arr_zXd &z_measurements, T::Gauss_z &z_pred, double existence_prob_pred, Config config) { - double predicted_existence_probability = probability_of_survival * last_detection_probability_; // Finds Ps + double r_kgkm1 = existence_prob_pred; + double P_d = config.pdaf.prob_of_detection; + double lambda = config.pdaf.clutter_intensity; - double sum = 0.0; - for (const Vec_z& measurement : measurements) - { - sum += z_pred.pdf(measurement); + // predicted measurement probability + double z_pred_prob = 0.0; + for (const typename T::Vec_z &z_k : z_measurements.colwise()) { + z_pred_prob += z_pred.pdf(z_k); } - 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); + // posterior existence probability r_k + double L_k = 1 - P_d + P_d / lambda * z_pred_prob; // (7.33) + double r_k = (L_k * r_kgkm1) / (1 - (1 - L_k) * r_kgkm1); // (7.32) + return r_k; } /** - * @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 existence_prob 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. + * @brief Calculates the existence probability given the likelyhood of the measurements and the previous existence probability. + * @param z_likelyhoods (l_a_k) The likelyhood of the measurements + * @param existence_prob_est (r_{k-1}) The previous existence probability. + * @param config The configuration for the IPDA. + * @return The existence probability (r_k). */ - static std::tuple, std::vector, Gauss_x, Gauss_z, std::vector> - step(const DynModT& dyn_model, const SensModT& sen_model, double timestep, const Gauss_x& x_est, - const std::vector& z_meas, double existence_prob, const IPDA::Config& config) + static double existence_prob_update(const Eigen::ArrayXd z_likelyhoods, double existence_prob_pred, 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(config)); + double r_kgkm1 = existence_prob_pred; // r_k given k minus 1 + double P_d = config.pdaf.prob_of_detection; + double lambda = config.pdaf.clutter_intensity; + + // posterior existence probability r_k + double L_k = 1 - P_d + P_d / lambda * z_likelyhoods.sum(); // (7.33) + double r_k = (L_k * r_kgkm1) / (1 - (1 - L_k) * r_kgkm1); // (7.32) + return r_k; + } - double existence_probability = get_existence_probability( - inside, config.prob_of_survival, existence_prob, config.prob_of_detection, config.clutter_intensity, z_pred); - return { x_final, existence_probability, inside, outside, x_pred, z_pred, x_updated }; + /** + * @brief Estimates the clutter intensity using (7.31) + * @param z_pred The predicted measurement. + * @param predicted_existence_probability (r_{k|k-1}) The predicted existence probability. + * @param num_measurements (m_k) The number of z_measurements. + * @param config The configuration for the IPDA. + * @return The clutter intensity. + */ + static double estimate_clutter_intensity(const T::Gauss_z &z_pred, double predicted_existence_probability, double num_measurements, Config config) + { + size_t m_k = num_measurements; + double P_d = config.pdaf.prob_of_detection; + double r_k = predicted_existence_probability; + double V_k = utils::Ellipsoid(z_pred, config.pdaf.mahalanobis_threshold).volume(); // gate area + + if (m_k == 0) { + return 0.0; + } + return 1 / V_k * (m_k - r_k * P_d); // (7.31) + } + + static Output step(const DynModT &dyn_mod, const SensModT &sens_mod, double timestep, const State &state_est_prev, const Arr_zXd &z_measurements, + Config &config) + { + double existence_prob_pred = existence_prediction(state_est_prev.existence_probability, config.ipda.prob_of_survival); + + if (config.ipda.estimate_clutter) { + typename T::Gauss_z z_pred; + std::tie(std::ignore, z_pred) = EKF::predict(dyn_mod, sens_mod, timestep, state_est_prev.x_estimate); + config.pdaf.clutter_intensity = estimate_clutter_intensity(z_pred, existence_prob_pred, z_measurements.cols(), config); + } + + auto [x_post, x_pred, z_pred, x_upd, gated_measurements] = + PDAF::step(dyn_mod, sens_mod, timestep, state_est_prev.x_estimate, z_measurements, {config.pdaf}); + + 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)) { + existence_probability_upd = existence_prob_update(z_meas_inside, z_pred, existence_prob_pred, config); + } + // clang-format off + return { + .state = { + .x_estimate = x_post, + .existence_probability = existence_probability_upd, + }, + .x_prediction = x_pred, + .z_prediction = z_pred, + .x_updates = x_upd, + .gated_measurements = gated_measurements + }; + // clang-format on } }; -} // namespace vortex::filter +} // namespace vortex::filter \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp index 861ce899..c3d1a943 100644 --- a/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/pdaf.hpp @@ -1,32 +1,27 @@ -/** - * @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 #include +#include #include +#include #include #include #include -namespace vortex::filter -{ -/** - * @brief The PDAF filter class - * @tparam DynModT The dynamic model - * @tparam SensModT The sensor model - */ -template -class PDAF -{ +namespace vortex::filter { + +namespace config { +struct PDAF { + double mahalanobis_threshold = 1.0; + double min_gate_threshold = 0.0; + double max_gate_threshold = std::numeric_limits::max(); + double prob_of_detection = 1.0; + double clutter_intensity = 1.0; +}; +} // namespace config + +template class PDAF { public: static constexpr int N_DIM_x = DynModT::N_DIM_x; static constexpr int N_DIM_z = SensModT::N_DIM_z; @@ -36,141 +31,150 @@ class PDAF using T = Types_xzuvw; - using EKF = vortex::filter::EKF_t; - 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; - using StatesXd = std::vector; - using GaussMixXd = vortex::prob::GaussianMixture; + using Gauss_z = typename T::Gauss_z; + using Gauss_x = typename T::Gauss_x; + using Vec_z = typename T::Vec_z; + using GaussMix_x = typename T::GaussMix_x; + using GaussMix_z = typename T::GaussMix_z; + using Arr_zXd = Eigen::Array; + using Arr_1Xb = Eigen::Array; + using Gauss_xX = std::vector; + using EKF = vortex::filter::EKF_t; + + struct Config { + config::PDAF pdaf; + }; - 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; + struct Output { + Gauss_x x_; + Gauss_x x_prediction; + Gauss_z z_prediction; + Gauss_xX x_updates; + Arr_1Xb gated_measurements; }; PDAF() = delete; /** - * @brief The step function for the PDAF filter + * @brief Perform one step of the Probabilistic Data Association 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) + * @param timestep Time step in seconds + * @param x_est The estimated state + * @param z_measurements Array of measurements + * @param config Configuration for the PDAF + * @return `Output` The result of the PDAF step and some intermediate results */ - static std::tuple - step(const DynModT& dyn_model, const SensModT& sen_model, double timestep, const Gauss_x& x_est, - const MeasurementsZd& z_meas, const Config& config) + static Output step(const DynModT &dyn_model, const SensModT &sen_model, double timestep, const Gauss_x &x_est, const Arr_zXd &z_measurements, + 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)); + auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); + auto gated_measurements = apply_gate(z_measurements, z_pred, config); + auto inside_meas = get_inside_measurements(z_measurements, gated_measurements); + + Gauss_xX x_updated; + for (const auto &z_k : inside_meas.colwise()) { + x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, z_k)); } - 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_meas, x_updated, z_pred, x_pred, config.pdaf.prob_of_detection, config.pdaf.clutter_intensity); + return {x_final, x_pred, z_pred, x_updated, gated_measurements}; } /** - * @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 + * @brief Apply gate to the measurements + * + * @param z_measurements Array of measurements + * @param z_pred Predicted measurement + * @param config Configuration for the PDAF + * @return `Arr_1Xb` Indeces of the measurements that are inside the gate */ - static std::tuple 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) + static Arr_1Xb apply_gate(const Arr_zXd &z_measurements, const Gauss_z &z_pred, Config config) { - 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); - } + double mahalanobis_threshold = config.pdaf.mahalanobis_threshold; + double min_gate_threshold = config.pdaf.min_gate_threshold; + double max_gate_threshold = config.pdaf.max_gate_threshold; + + Arr_1Xb gated_measurements(1, z_measurements.cols()); + + for (size_t a_k = 0; const Vec_z &z_k : z_measurements.colwise()) { + double mahalanobis_distance = z_pred.mahalanobis_distance(z_k); + double regular_distance = (z_pred.mean() - z_k).norm(); + gated_measurements(a_k++) = + (mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && regular_distance <= max_gate_threshold; } + return gated_measurements; + } - return { inside_meas, outside_meas }; + /** + * @brief Get the measurements that are inside the gate + * + * @param z_measurements Array of measurements + * @param gated_measurements Indeces of the measurements that are inside the gate + * @return `Arr_zXd` The measurements that are inside the gate + */ + static Arr_zXd get_inside_measurements(const Arr_zXd &z_measurements, const Arr_1Xb &gated_measurements) + { + Arr_zXd inside_meas(N_DIM_z, gated_measurements.count()); + for (size_t i = 0, j = 0; bool gated : gated_measurements) { + if (gated) { + inside_meas.col(j++) = z_measurements.col(i); + } + i++; + } + return inside_meas; } /** - * @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 + * @brief Get the weighted average of the states + * + * @param z_measurements Array of measurements + * @param updated_states Array of updated states + * @param z_pred Predicted measurement + * @param x_pred Predicted state + * @param prob_of_detection Probability of detection + * @param clutter_intensity Clutter intensity + * @return `Gauss_x` The weighted average of the 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) + static Gauss_x get_weighted_average(const Arr_zXd &z_measurements, const Gauss_xX &updated_states, const Gauss_z &z_pred, const Gauss_x &x_pred, + double prob_of_detection, double clutter_intensity) { - StatesXd states; + Gauss_xX 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); + Eigen::VectorXd weights = get_weights(z_measurements, z_pred, prob_of_detection, clutter_intensity); - GaussMixXd gaussian_mixture(weights, states); - - return gaussian_mixture.reduce(); + return GaussMix_x{weights, states}.reduce(); } /** - * @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 + * @brief Get the weights for the measurements + * + * @param z_measurements Array of measurements + * @param z_pred Predicted measurement + * @param prob_of_detection Probability of detection + * @param clutter_intensity Clutter intensity + * @return `Eigen::VectorXd` The weights for the measurements */ - static Eigen::VectorXd get_weights(const MeasurementsZd& z_meas, const Gauss_z& z_pred, double prob_of_detection, - double clutter_intensity) + static Eigen::VectorXd get_weights(const Arr_zXd &z_measurements, const Gauss_z &z_pred, double prob_of_detection, double clutter_intensity) { - Eigen::VectorXd weights(z_meas.size() + 1); + double lambda = clutter_intensity; + double P_d = prob_of_detection; + + Eigen::VectorXd weights(z_measurements.cols() + 1); // in case no measurement assosiates with the target - double no_association = clutter_intensity * (1 - prob_of_detection); - weights(0) = no_association; + 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 k = 1; k < z_meas.size() + 1; k++) - { - weights(k) = (prob_of_detection * z_pred.pdf(z_meas.at(k - 1))); + for (size_t a_k = 1; const Vec_z &z_k : z_measurements.colwise()) { + weights(a_k++) = P_d * z_pred.pdf(z_k); } // normalize weights @@ -178,6 +182,32 @@ class PDAF return weights; } + + /** + * @brief Get association probabilities according to Corollary 7.3.3 + * + * @param z_likelyhoods Array of likelyhoods + * @param prob_of_detection Probability of detection + * @param clutter_intensity Clutter intensity + * @return `Eigen::VectorXd` The weights for the measurements + */ + static Eigen::ArrayXd association_probabilities(const Eigen::ArrayXd &z_likelyhoods, double prob_of_detection, double clutter_intensity) + { + size_t m_k = z_likelyhoods.size(); + double lambda = clutter_intensity; + double P_d = prob_of_detection; + + Eigen::ArrayXd weights(m_k + 1); + + // Accociation probabilities (Corrolary 7.3.3) + weights(0) = lambda * (1 - P_d); + weights.tail(m_k) = P_d * z_likelyhoods; + + // normalize weights + weights /= weights.sum(); + + return weights; + } }; -} // namespace vortex::filter \ No newline at end of file +} // namespace vortex::filter \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/models/dynamic_model_interfaces.hpp b/vortex-filtering/include/vortex_filtering/models/dynamic_model_interfaces.hpp index c7f57c84..08856cba 100644 --- a/vortex-filtering/include/vortex_filtering/models/dynamic_model_interfaces.hpp +++ b/vortex-filtering/include/vortex_filtering/models/dynamic_model_interfaces.hpp @@ -14,6 +14,7 @@ #include #include #include +#include #include namespace vortex::models { @@ -162,14 +163,14 @@ template cl * @param x_est T::Vec_x estimate * @return T::Vec_x */ - T::Gauss_x pred_from_est(double dt, const T::Gauss_x &x_est, const T::Vec_u &u = T::Vec_u::Zero()) const + auto pred_from_est(double dt, const auto &x_est, const T::Vec_u &u = T::Vec_u::Zero()) const -> std::remove_reference_t + requires(concepts::MultiVarGaussLike) { typename T::Mat_xx P = x_est.cov(); typename T::Mat_xx A_d = this->A_d(dt, x_est.mean()); typename T::Mat_xx GQGT_d = this->GQGT_d(dt, x_est.mean()); - typename T::Gauss_x x_est_pred(f_d(dt, x_est.mean(), u), A_d * P * A_d.transpose() + GQGT_d); - return x_est_pred; + return {f_d(dt, x_est.mean(), u), A_d * P * A_d.transpose() + GQGT_d}; } /** Get the predicted state distribution given a state @@ -179,7 +180,7 @@ template cl */ T::Gauss_x pred_from_state(double dt, const T::Vec_x &x, const T::Vec_u &u = T::Vec_u::Zero()) const { - typename T::Gauss_x x_est_pred(f_d(dt, x, u), GQGT_d(dt, x)); + typename T::Gauss_x x_est_pred = {f_d(dt, x, u), GQGT_d(dt, x)}; return x_est_pred; } diff --git a/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp b/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp index edbfaa6f..d5217ce4 100644 --- a/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp +++ b/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp @@ -2,6 +2,7 @@ #include #include #include +#include namespace vortex { namespace models { @@ -45,8 +46,7 @@ template class IdentityDynamicModel : public interface::Dynamic }; /** (Nearly) Constant Position Model - * State x = [pos], where pos is a `n_spatial_dim`-dimensional vector - * @tparam n_spatial_dim Number of spatial dimensions + * State x = [position, position] */ class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> { using Parent = interface::DynamicModelLTV<2, UNUSED, 2>; @@ -56,10 +56,10 @@ class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> { static constexpr int N_DIM_u = Parent::N_DIM_u; static constexpr int N_DIM_v = Parent::N_DIM_v; - using T = vortex::Types_xuv; + using T = Types_xuv; - using ST = StateType; - static constexpr std::array StateNames{ST::position, ST::position}; + using S = StateName; + using StateT = State; /** Constant Position Model in 2D * x = [x, y] @@ -103,8 +103,7 @@ class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> { }; /** (Nearly) Constant Velocity Model. - * State x = [pos, vel], where pos and vel are `n_spatial_dim`-dimensional vectors - * @tparam n_spatial_dim Number of spatial dimensions + * State x = [position, position, velocity, velocity] */ class ConstantVelocity : public interface::DynamicModelLTV<4, UNUSED, 2> { using Parent = interface::DynamicModelLTV<4, UNUSED, 2>; @@ -119,12 +118,13 @@ class ConstantVelocity : public interface::DynamicModelLTV<4, UNUSED, 2> { using T = vortex::Types_xuv; + using S = StateName; + using StateT = State; + using Vec_s = Eigen::Matrix; using Mat_ss = Eigen::Matrix; - using ST = StateType; - static constexpr std::array StateNames{ST::position, ST::position, ST::velocity, ST::velocity}; /** * @brief Constant Velocity Model in 2D @@ -184,8 +184,7 @@ class ConstantVelocity : public interface::DynamicModelLTV<4, UNUSED, 2> { }; /** (Nearly) Constant Acceleration Model. - * State vector x = [pos, vel, acc], where pos, vel and acc are `n_spatial_dim`-dimensional vectors - * @tparam n_spatial_dim Number of spatial dimensions + * State vector x = [position, position, velocity, velocity, acceleration, acceleration] */ class ConstantAcceleration : public interface::DynamicModelLTV<3 * 2, UNUSED, 2 * 2> { public: @@ -194,11 +193,13 @@ class ConstantAcceleration : public interface::DynamicModelLTV<3 * 2, UNUSED, 2 using T = vortex::Types_xv; + using S = StateName; + using StateT = State; + using Vec_s = Eigen::Matrix; using Mat_ss = Eigen::Matrix; - using ST = StateType; - static constexpr std::array StateNames{ST::position, ST::position, ST::velocity, ST::velocity, ST::acceleration, ST::acceleration}; + /** Constant Acceleration Model * @param std_vel Standard deviation of velocity * @param std_acc Standard deviation of acceleration @@ -261,15 +262,16 @@ class ConstantAcceleration : public interface::DynamicModelLTV<3 * 2, UNUSED, 2 }; /** Coordinated Turn Model in 2D. - * x = [x_pos, y_pos, x_vel, y_vel, turn_rate] + * x = [position, position, velocity, velocity, turn_rate] */ class CoordinatedTurn : public interface::DynamicModelCTLTV<5, UNUSED, 3> { public: static constexpr int N_STATES = 5; using T = vortex::Types_xv; - using ST = StateType; - static constexpr std::array StateNames{ST::position, ST::position, ST::velocity, ST::velocity, ST::turn_rate}; + using S = StateName; + using StateT = State; + /** (Nearly) Coordinated Turn Model in 2D. (Nearly constant speed, nearly constant turn rate) * State = [x, y, x_dot, y_dot, omega] * @param std_vel Standard deviation of velocity diff --git a/vortex-filtering/include/vortex_filtering/models/imm_model.hpp b/vortex-filtering/include/vortex_filtering/models/imm_model.hpp index 94c6b3b7..22d1aea1 100644 --- a/vortex-filtering/include/vortex_filtering/models/imm_model.hpp +++ b/vortex-filtering/include/vortex_filtering/models/imm_model.hpp @@ -31,13 +31,7 @@ template constexpr std::array matching return matches; } -// structs for the type of state -enum class StateType { none, position, velocity, acceleration, turn_rate }; -struct StateMinMax { - double min; - double max; -}; -using StateMap = std::map; + /** * @brief Container class for interacting multiple models. @@ -53,31 +47,20 @@ template class Imm static constexpr bool MIN_DIM_x = std::min(N_DIMS_x); static constexpr size_t N_MODELS = sizeof...(DynModels); + using StateNames = std::tuple...>; + + static constexpr StateNames ALL_STATE_NAMES = {{DynModels::StateT::STATE_NAMES}...}; + using DynModTuple = std::tuple; using GaussTuple_x = std::tuple::Gauss_x...>; - using StateNames = std::tuple...>; using Vec_n = Eigen::Vector; using Mat_nn = Eigen::Matrix; template using DynModT = typename std::tuple_element::type; - template using T = Types_xuv; + template using State = DynModT::State; - /** - * @brief Construct a new ImmModel object - * @tparam DynModels Dynamic models to use. - * @param jump_matrix Markov jump chain matrix for the transition probabilities. - * I.e. the probability of switching from model i to model j is `jump_matrix(i,j)`. Diagonal should be 0. - * @param hold_times Expected holding time in seconds for each state. Parameter is the mean of an exponential distribution. - * @param models_and_state_names Tuple of dynamic models and an std::array of their state names. The state names is of the vortex::models::StateType enum. - * @note - The jump matrix specifies the probability of switching to a model WHEN a switch occurs. - * @note - The holding times specifies HOW LONG a state is expected to be held between switches. - * @note - In order to change the properties of a model, you must get the model using `get_model()` - */ - ImmModel(Mat_nn jump_matrix, Vec_n hold_times, std::tuple>... models_and_state_names) - : ImmModel(jump_matrix, hold_times, std::get<0>(models_and_state_names)..., {std::get<1>(models_and_state_names)...}) - { - } + template using T = Types_xuv; /** * @brief Construct a new ImmModel object @@ -91,11 +74,10 @@ template class Imm * @note - The holding times specifies HOW LONG a state is expected to be held between switches. * @note - In order to change the properties of a model, you must get the model using `get_model()` */ - ImmModel(Mat_nn jump_matrix, Vec_n hold_times, DynModels... models, StateNames state_names) + ImmModel(Mat_nn jump_matrix, Vec_n hold_times, DynModels... models) : models_(models...) , jump_matrix_(jump_matrix) , hold_times_(hold_times) - , state_names_(state_names) { if (!jump_matrix_.diagonal().isZero()) { throw std::invalid_argument("Jump matrix diagonal should be zero"); @@ -194,26 +176,22 @@ template class Imm */ template T::Mat_vv Q_d(double dt, const T::Vec_x &x) const { return get_model().Q_d(dt, x); } + static constexpr int N_DIM_x(size_t model_index) { return N_DIMS_x.at(model_index); } static constexpr int N_DIM_u(size_t model_index) { return N_DIMS_u.at(model_index); } static constexpr int N_DIM_v(size_t model_index) { return N_DIMS_v.at(model_index); } - StateNames get_all_state_names() const { return state_names_; } - - template std::array get_state_names() { return std::get(state_names_); } - - template StateType get_state_name(size_t i) { return get_state_names().at(i); } - private: DynModTuple models_; Mat_nn jump_matrix_; Vec_n hold_times_; - StateNames state_names_; }; /** * @brief Class for resizing the state vector of a sensor model to fit with multiple dynamic models. * + * @tparam n_dim_a The dimension of the desired state vector. + * @tparam SensModT The sensor model to use. */ template class ImmSensorModel { public: @@ -224,6 +202,11 @@ template ; + /** + * @brief Construct a new Imm Sensor Model object + * + * @param sensor_model The sensor model to use. Must have a copy constructor. + */ ImmSensorModel(SensModT sensor_model) : sensor_model_(sensor_model) { @@ -238,6 +221,12 @@ template class ImmSensorModelLTV { public: static constexpr int N_DIM_x = SensModT::N_DIM_x; @@ -247,6 +236,11 @@ template ; + /** + * @brief Construct a new Imm Sensor Model LTV object + * + * @param sensor_model The sensor model to use. Must have a copy constructor. + */ ImmSensorModelLTV(SensModT sensor_model) : sensor_model_(sensor_model) { @@ -270,7 +264,7 @@ template (); typename T::Mat_xx cov = x_est.cov().template topLeftCorner(); - return sensor_model_.pred_from_est({mean, cov}); + return sensor_model_.pred_from_est(typename T::Gauss_x{mean, cov}); } T::Gauss_z pred_from_state(const T::Vec_a &x) const { return sensor_model_.pred_from_state(x.template head()); } diff --git a/vortex-filtering/include/vortex_filtering/models/sensor_model_interfaces.hpp b/vortex-filtering/include/vortex_filtering/models/sensor_model_interfaces.hpp index 0f33cce1..17d9227e 100644 --- a/vortex-filtering/include/vortex_filtering/models/sensor_model_interfaces.hpp +++ b/vortex-filtering/include/vortex_filtering/models/sensor_model_interfaces.hpp @@ -14,6 +14,8 @@ #include #include #include +#include +#include namespace vortex::models { namespace interface { @@ -131,7 +133,8 @@ template class Sensor * @param x_est TVec_x estimate * @return prob::MultiVarGauss */ - T::Gauss_z pred_from_est(const T::Gauss_x &x_est) const + T::Gauss_z pred_from_est(const auto &x_est) const + requires(vortex::concepts::MultiVarGaussLike) { typename T::Mat_xx P = x_est.cov(); typename T::Mat_zx C = this->C(x_est.mean()); diff --git a/vortex-filtering/include/vortex_filtering/models/state.hpp b/vortex-filtering/include/vortex_filtering/models/state.hpp new file mode 100644 index 00000000..2c0457dd --- /dev/null +++ b/vortex-filtering/include/vortex_filtering/models/state.hpp @@ -0,0 +1,168 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vortex { + +enum class StateName : size_t { none, position, velocity, acceleration, jerk, turn_rate }; + +// structs for the type of state +struct StateMinMax { + double min; + double max; +}; + +template + requires std::is_integral_v || std::is_enum_v +using StateMapT = std::map; + +using StateMap = StateMapT; + +template + requires std::is_integral_v || std::is_enum_v +struct StateLocation { + StateName name; + size_t start_index; + size_t end_index; + constexpr size_t size() const { return end_index - start_index; } + bool operator==(const StateLocation &other) const { return name == other.name && start_index == other.start_index && end_index == other.end_index; } +}; + +template constexpr auto index_of(const R &range, T needle) +{ + auto it = std::ranges::find(range, needle); + if (it == std::ranges::end(range)) + throw std::logic_error("Element not found!"); + return std::ranges::distance(std::ranges::begin(range), it); +} + +template + requires std::is_integral_v || std::is_enum_v +class State : public vortex::prob::MultiVarGauss { +public: + static constexpr size_t N_STATES = sizeof...(Sn); + + static constexpr std::array STATE_NAMES = {Sn...}; + + using T = vortex::Types_n; + using StateLoc = StateLocation; + + State(const T::Vec_n &mean, const T::Mat_nn &cov) + : vortex::prob::MultiVarGauss(mean, cov) + { + } + + explicit State(T::Gauss_n x) + : vortex::prob::MultiVarGauss(x.mean(), x.cov()) + { + } + + // private: + static constexpr size_t UNIQUE_STATES_COUNT = []() { + std::array state_names = STATE_NAMES; + std::sort(state_names.begin(), state_names.end()); + auto last = std::unique(state_names.begin(), state_names.end()); + return std::distance(state_names.begin(), last); + }(); + + // static_assert( + // []() { + // size_t n_unique = 1; + // for (size_t i = 0; i < N_STATES; i++) { + // if (i > 0 && STATE_NAMES[i] == STATE_NAMES[i - 1]) { + // n_unique++; + // } + // } + // if (n_unique != UNIQUE_STATES_COUNT) { + // return false; + // } + // return true; + // }(), + // "Groups of state names must not repeat"); + + static constexpr std::array UNIQUE_STATE_NAMES = []() { + std::array unique_state_names = {}; + size_t map_index = 0; + for (size_t i = 1; i < N_STATES; i++) { + if (STATE_NAMES[i] != STATE_NAMES[i - 1]) { + unique_state_names[map_index++] = STATE_NAMES[i - 1]; + } + } + unique_state_names[map_index] = STATE_NAMES[N_STATES - 1]; + return unique_state_names; + }(); + + static constexpr std::array STATE_MAP = []() { + std::array state_map = {}; + + size_t start_index = 0; + size_t map_index = 0; + + for (size_t i = 1; i < N_STATES; i++) { + if (STATE_NAMES[i] != STATE_NAMES[i - 1]) { + state_map[map_index++] = {STATE_NAMES[i - 1], start_index, i}; + start_index = i; + } + } + state_map[map_index] = {STATE_NAMES[N_STATES - 1], start_index, N_STATES}; + return state_map; + }(); + + static constexpr bool has_state_name(StateName S) { return std::find(UNIQUE_STATE_NAMES.begin(), UNIQUE_STATE_NAMES.end(), S) != UNIQUE_STATE_NAMES.end(); } + +public: + static constexpr StateLoc state_loc(StateName S) { return STATE_MAP[index_of(UNIQUE_STATE_NAMES, S)]; } + + template + requires(has_state_name(S)) + using T_n = vortex::Types_n; + + template + requires(has_state_name(S)) + T_n::Vec_n mean_of() const + { + constexpr StateLoc sm = state_loc(S); + return this->mean().template segment(sm.start_index); + } + + template + requires(has_state_name(S)) + void set_mean_of(const T_n::Vec_n &mean) + { + constexpr StateLoc sm = state_loc(S); + this->mean().template segment(sm.start_index) = mean; + } + + template + requires(has_state_name(S)) + T_n::Mat_nn cov_of() const + { + constexpr StateLoc sm = state_loc(S); + return this->cov().template block(sm.start_index, sm.start_index); + } + + template + requires(has_state_name(S)) + void set_cov_of(const T_n::Mat_nn &cov) + { + constexpr StateLoc sm = state_loc(S); + this->cov().template block(sm.start_index, sm.start_index) = cov; + } + + T::Gauss_n gauss() const { return static_cast(*this); } + + template + requires(has_state_name(S)) + T_n::Gauss_n gauss_of() const + { + return {mean_of(S), cov_of(S)}; + } +}; +} // namespace vortex \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp b/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp index ea093f26..80c2b0c7 100644 --- a/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp @@ -30,10 +30,10 @@ concept is_container = requires(Container c) { { std::size(c) } -> std::convertible_to; - // Accessing an element by index must return a reference to ValueType. + // Accessing an element by index must return a reference to ValueType or a type convertible to ValueType. { c[std::declval()] - } -> std::same_as; + } -> std::convertible_to; }; } // namespace concepts @@ -59,6 +59,9 @@ template class GaussianMixture { : weights_(Eigen::Map(weights.data(), std::distance(std::begin(weights), std::end(weights)))), gaussians_(std::begin(gaussians), std::end(gaussians)) { + if ((size_t)weights_.size() != gaussians_.size()) { + throw std::invalid_argument("The number of weights must match the number of Gaussians"); + } } /** Default Constructor diff --git a/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp b/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp index 961a6c5d..0630f72b 100644 --- a/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp @@ -2,6 +2,8 @@ #include #include #include +#include +#include namespace vortex::prob { @@ -23,10 +25,14 @@ template class MultiVarGauss { MultiVarGauss(const Vec_n &mean, const Mat_nn &cov) : mean_(mean), cov_(cov), cov_inv_(cov_.llt().solve(Mat_nn::Identity())) { if (!cov_.isApprox(cov_.transpose(), 1e-6)) { - throw std::invalid_argument("Covariance matrix is not symmetric"); + std::stringstream ss; + ss << "Covariance matrix is not symmetric:\n" << cov_; + throw std::invalid_argument(ss.str()); } if (cov_.llt().info() != Eigen::Success) { - throw std::invalid_argument("Covariance matrix is not positive definite"); + std::stringstream ss; + ss << "Covariance matrix is not positive definite:\n" << cov_; + throw std::invalid_argument(ss.str()); } } @@ -56,10 +62,12 @@ template class MultiVarGauss { return exponent - 0.5 * std::log(std::pow(2 * std::numbers::pi, size()) * cov().determinant()); } - Vec_n mean() const { return mean_; } - Mat_nn cov() const { return cov_; } - Mat_nn cov_inv() const { return cov_inv_; } + const Vec_n &mean() const { return mean_; } + Vec_n &mean() { return mean_; } + const Mat_nn &cov() const { return cov_; } + Mat_nn &cov() { return cov_; } + Mat_nn cov_inv() const { return cov().llt().solve(Mat_nn::Identity()); } /** Calculate the Mahalanobis distance of x given the Gaussian * @param x * @return double diff --git a/vortex-filtering/include/vortex_filtering/types/general_concepts.hpp b/vortex-filtering/include/vortex_filtering/types/general_concepts.hpp index 9f4c0dd4..5270cedb 100644 --- a/vortex-filtering/include/vortex_filtering/types/general_concepts.hpp +++ b/vortex-filtering/include/vortex_filtering/types/general_concepts.hpp @@ -5,16 +5,12 @@ #include namespace vortex::concepts { - -template -concept mat_convertible_to = requires { - // Check if From is convertible to To. - requires std::convertible_to; - // Check if both From and To are Eigen::Matrix types. - requires std::is_base_of_v, From> && std::is_base_of_v, To>; - // Compile-time check for fixed dimensions compatibility. - requires(From::RowsAtCompileTime == To::RowsAtCompileTime) && (From::ColsAtCompileTime == To::ColsAtCompileTime); -}; +template +concept mat_convertible_to = + std::convertible_to, std::decay_t> && std::is_base_of_v>, std::decay_t> && + std::is_base_of_v>, std::decay_t> && + (std::decay_t::RowsAtCompileTime == std::decay_t::RowsAtCompileTime) && + (std::decay_t::ColsAtCompileTime == std::decay_t::ColsAtCompileTime); } // namespace vortex::concepts \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/types/model_concepts.hpp b/vortex-filtering/include/vortex_filtering/types/model_concepts.hpp index 87580f0f..3e545465 100644 --- a/vortex-filtering/include/vortex_filtering/types/model_concepts.hpp +++ b/vortex-filtering/include/vortex_filtering/types/model_concepts.hpp @@ -2,10 +2,72 @@ #include #include #include +#include namespace vortex::concepts { using std::size_t; +/** + * @brief Concept for MultiVarGauss-like classes. Requires the following functions: + * @brief - `double pdf(Vec_n)` + * @brief - `double logpdf(Vec_n)` + * @brief - `Vec_n mean()` + * @brief - `Mat_nn cov()` + * @brief - `int size()` + * @brief - `Vec_n sample(std::mt19937_64&)` + * @brief - `double mahalanobis_distance(Vec_n)` + * + * @tparam T The MultiVarGauss-like class + * @tparam n_dim Dimension of the state + */ +template +concept MultiVarGaussLike = requires { + { + std::declval().pdf(std::declval::Vec_n>()) + } -> std::convertible_to; + + { + std::declval().logpdf(std::declval::Vec_n>()) + } -> std::convertible_to; + + { + std::declval().mean() + } -> mat_convertible_to::Vec_n>; + + { + std::declval().cov() + } -> mat_convertible_to::Mat_nn>; + + { + std::declval().size() + } -> std::convertible_to; + + { + std::declval().sample(std::declval()) + } -> mat_convertible_to::Vec_n>; + + { + std::declval().mahalanobis_distance(std::declval::Vec_n>()) + } -> std::convertible_to; +}; + +template +concept StateLike = requires { + requires MultiVarGaussLike; + + { + T::N_STATES + } -> std::convertible_to; + + { + T::STATE_NAMES + } -> std::convertible_to>; + + { + std::declval().state_loc(std::declval()) + } -> std::convertible_to; +}; + /** * @brief Concept for dynamic models. Requires the following functions: * @brief - `Vec_x f_d(double, Vec_x, Vec_u, Vec_v)` diff --git a/vortex-filtering/include/vortex_filtering/utils/algorithms/auction_algorithm.hpp b/vortex-filtering/include/vortex_filtering/utils/algorithms/auction_algorithm.hpp new file mode 100644 index 00000000..f55a03a1 --- /dev/null +++ b/vortex-filtering/include/vortex_filtering/utils/algorithms/auction_algorithm.hpp @@ -0,0 +1,9 @@ +#pragma once +#include +#include + +namespace vortex::utils { + +std::pair auction_algorithm(const Eigen::MatrixXd &cost_matrix); + +} // namespace vortex::utils diff --git a/vortex-filtering/include/vortex_filtering/utils/algorithms/murtys_method.hpp b/vortex-filtering/include/vortex_filtering/utils/algorithms/murtys_method.hpp new file mode 100644 index 00000000..953fadc4 --- /dev/null +++ b/vortex-filtering/include/vortex_filtering/utils/algorithms/murtys_method.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace vortex::utils { + +// Concept to ensure that the assignment algorithm is compatible with the cost matrix +template +concept assignment_algorithm = requires(T a, const Eigen::MatrixXd &cost_matrix) { + { + a(cost_matrix) + } -> std::same_as>; +}; + +std::vector> murtys_method(const Eigen::MatrixXd &cost_matrix, int m, auto assignment_solver = auction_algorithm()) + requires(assignment_algorithm) + +} // namespace vortex::utils \ No newline at end of file diff --git a/vortex-filtering/src/utils/algorithms/auction_algorithm.cpp b/vortex-filtering/src/utils/algorithms/auction_algorithm.cpp new file mode 100644 index 00000000..1c84aaf1 --- /dev/null +++ b/vortex-filtering/src/utils/algorithms/auction_algorithm.cpp @@ -0,0 +1,47 @@ +#include +#include +#include + +namespace vortex::utils { + +std::pair auction_algorithm(const Eigen::MatrixXd &cost_matrix) +{ + int num_items = cost_matrix.cols(); + Eigen::VectorXi assignment = Eigen::VectorXi::Constant(num_items, -1); + Eigen::VectorXd prices = Eigen::VectorXd::Zero(num_items); + + std::vector unassigned; + for (int i = 0; i < num_items; ++i) { + unassigned.push_back(i); + } + + double epsilon = 1.0 / (num_items + 1); + + while (!unassigned.empty()) { + int customer = unassigned.back(); + unassigned.pop_back(); + + double max_value = std::numeric_limits::lowest(); + int max_item = -1; + for (int item = 0; item < num_items; ++item) { + double value = cost_matrix(customer, item) - prices[item]; + if (value > max_value) { + max_value = value; + max_item = item; + } + } + + int current_owner = assignment[max_item]; + if (current_owner != -1) { + unassigned.push_back(current_owner); + } + + assignment[max_item] = customer; + prices[max_item] += max_value + epsilon; + } + + double total_cost = prices.sum(); + return {total_cost, assignment}; +} + +} // namespace vortex::utils \ No newline at end of file diff --git a/vortex-filtering/src/utils/algorithms/murtys_method.cpp b/vortex-filtering/src/utils/algorithms/murtys_method.cpp new file mode 100644 index 00000000..6c4aff94 --- /dev/null +++ b/vortex-filtering/src/utils/algorithms/murtys_method.cpp @@ -0,0 +1,32 @@ +#include "murtys_method_impl.hpp" + +#include +#include +#include +#include +#include + +namespace vortex::utils { + +using CostValuePair = std::pair; + +std::vector murtys_method(const Eigen::MatrixXd &cost_matrix, int num_solutions, auto assignment_solver) +{ + std::vector R; // Solution set R + auto comp = [](const CostValuePair &a, const CostValuePair &b) { return a.first < b.first; }; + std::priority_queue, decltype(comp)> Q(comp); + + Q.push(assignment_solver(cost_matrix)); // Add the first problem to Q with its cost + + while (R.size() < num_solutions && !Q.empty()) { + auto [_, Q_max] = Q.top(); // Fetch the next problem to solve + Q.pop(); + R.push_back(Q_max); + // Generate subproblems based on the last solution added to R + auto Q_ = partition(Q_max.first, Q_max.second); + } + + return R; // Return the set of solutions found +} + +} // namespace vortex::utils \ No newline at end of file diff --git a/vortex-filtering/src/utils/algorithms/murtys_method_impl.cpp b/vortex-filtering/src/utils/algorithms/murtys_method_impl.cpp new file mode 100644 index 00000000..ba94fc3b --- /dev/null +++ b/vortex-filtering/src/utils/algorithms/murtys_method_impl.cpp @@ -0,0 +1,36 @@ +#include "murtys_method_impl.hpp" + +#include +#include +#include +#include +#include +#include +#include + +std::vector> partition(const Eigen::MatrixXd &P, const Eigen::VectorXi &S) +{ + std::vector> Q_prime; + + // Each assignment in S is excluded once to create a new subproblem + for (int i = 0; i < S.size(); ++i) { + // Skip invalid assignments + if (S(i) == -1) + continue; + + // Create a copy of the cost matrix and set a high cost for the current assignment to prevent it + Eigen::MatrixXd new_P = P; + new_P(i, S(i)) = std::numeric_limits::max(); + + Q_prime.emplace_back(0, new_P); // The cost (0) will be recalculated when solving the subproblem + } + + return Q_prime; +} + +bool is_valid_solution(const Eigen::VectorXi &solution) +{ + // A solution is valid if it does not contain any forbidden assignments + // In this context, a forbidden assignment could be represented by -1 or any other marker used + return !solution.isConstant(-1); +} diff --git a/vortex-filtering/src/utils/algorithms/murtys_method_impl.hpp b/vortex-filtering/src/utils/algorithms/murtys_method_impl.hpp new file mode 100644 index 00000000..81ec803e --- /dev/null +++ b/vortex-filtering/src/utils/algorithms/murtys_method_impl.hpp @@ -0,0 +1,7 @@ +#include +#include +#include + +std::vector> partition(const Eigen::MatrixXd &P, const Eigen::VectorXi &S); + +bool is_valid_solution(const Eigen::VectorXi &solution); diff --git a/vortex-filtering/src/utils/ellipse.cpp b/vortex-filtering/src/utils/ellipse.cpp index 41d4fc92..78f0cbf5 100644 --- a/vortex-filtering/src/utils/ellipse.cpp +++ b/vortex-filtering/src/utils/ellipse.cpp @@ -1,5 +1,6 @@ #include #include +#include using std::numbers::pi; @@ -35,8 +36,8 @@ double Ellipse::major_axis() const { return 2 * a_; } double Ellipse::minor_axis() const { return 2 * b_; } Eigen::Vector2d Ellipse::axes() const { return Eigen::Vector2d(2 * a_, 2 * b_); } double Ellipse::angle_rad() const { return angle_; } -double Ellipse::angle_deg() const { return angle_ * 180 / pi; } -double Ellipse::area() const { return pi * a_ * b_; } +double Ellipse::angle_deg() const { return angle_ * 180 / std::numbers::pi; } +double Ellipse::area() const { return std::numbers::pi * a() * b(); } } // namespace utils } // namespace vortex diff --git a/vortex-filtering/test/CMakeLists.txt b/vortex-filtering/test/CMakeLists.txt index d9ea28d8..cd61349e 100644 --- a/vortex-filtering/test/CMakeLists.txt +++ b/vortex-filtering/test/CMakeLists.txt @@ -9,11 +9,13 @@ ament_add_gtest(${PROJECT_NAME}_test ekf_test.cpp ellipsoid_test.cpp imm_test.cpp + immipda_test.cpp ipda_test.cpp numerical_integration_test.cpp pdaf_test.cpp probability_test.cpp sensor_model_test.cpp + state_test.cpp types_test.cpp ukf_test.cpp ) diff --git a/vortex-filtering/test/imm_test.cpp b/vortex-filtering/test/imm_test.cpp index a5a4c343..50d54742 100644 --- a/vortex-filtering/test/imm_test.cpp +++ b/vortex-filtering/test/imm_test.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -25,12 +26,8 @@ TEST(ImmModel, initWithStateNames) double std = 1.0; double dt = 1.0; - using ST = vortex::models::StateType; using ImmModelT = vortex::models::ImmModel; - ImmModelT imm_model{jump_mat, - hold_times, - std::tuple{ConstantPosition(std), std::array{ST::position, ST::position}}, - std::tuple{ConstantVelocity(std), std::array{ST::position, ST::position, ST::velocity, ST::velocity}}}; + ImmModelT imm_model{jump_mat, hold_times, ConstantPosition(std), ConstantVelocity(std)}; EXPECT_EQ(typeid(imm_model.get_model<0>()), typeid(ConstantPosition)); EXPECT_EQ(typeid(imm_model.get_model<1>()), typeid(ConstantVelocity)); @@ -54,11 +51,9 @@ TEST(ImmModel, piMatC) Eigen::Vector3d hold_times{1.0 / 6, 1.0 / 12, 1.0 / 18}; double std = 1.0; - using ST = vortex::models::StateType; using ImmModelT = vortex::models::ImmModel; - ImmModelT::StateNames state_names{{ST::position, ST::position}, {ST::position, ST::position}, {ST::position, ST::position}}; - ImmModelT imm_model(jump_mat, hold_times, ConstantPosition(std), ConstantPosition(std), ConstantPosition(std), state_names); + ImmModelT imm_model(jump_mat, hold_times, ConstantPosition(std), ConstantPosition(std), ConstantPosition(std)); // clang-format off Eigen::Matrix3d pi_mat_c{ @@ -86,11 +81,9 @@ TEST(ImmModel, piMatD) Eigen::Vector3d hold_times{1.0 / 6, 1.0 / 12, 1.0 / 18}; double std = 1.0; - using ST = vortex::models::StateType; using ImmModelT = vortex::models::ImmModel; - ImmModelT::StateNames state_names{{ST::position, ST::position}, {ST::position, ST::position}, {ST::position, ST::position}}; - ImmModelT imm_model(jump_mat, hold_times, ConstantPosition(std), ConstantPosition(std), ConstantPosition(std), state_names); + ImmModelT imm_model(jump_mat, hold_times, ConstantPosition(std), ConstantPosition(std), ConstantPosition(std)); Eigen::Matrix3d pi_mat_d_true; // clang-format off @@ -129,7 +122,7 @@ TEST(ImmFilter, init) using vortex::models::ConstantPosition; using SensModT = vortex::models::IdentitySensorModel<2, 1>; using ImmModelT = vortex::models::ImmModel; - using ImmFilterT = vortex::filter::ImmFilter; + using ImmFilterT = vortex::filter::ImmFilter; EXPECT_EQ(ImmFilterT::N_MODELS, 2u); EXPECT_EQ(ImmFilterT::N_DIM_z, SensModT::N_DIM_z); @@ -146,12 +139,12 @@ TEST(ImmFilter, calculateMixingProbs) double std = 1.0; using ImmModelT = vortex::models::ImmModel; - using ST = vortex::models::StateType; - ImmModelT imm_model(jump_mat, hold_times, {{std}, {ST::position, ST::position}}, {{std}, {ST::position, ST::position}}); + + ImmModelT imm_model(jump_mat, hold_times, {std}, {std}); auto sensor_model = std::make_shared>(dt); - using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; + using ImmFilterT = vortex::filter::ImmFilter>; Eigen::Vector2d model_weights = {0.5, 0.5}; @@ -180,12 +173,12 @@ TEST(ImmFilter, mixing_two_of_the_same_model) double pos_std = 1; using ImmModelT = vortex::models::ImmModel; - using ST = vortex::models::StateType; - ImmModelT imm_model(jump_mat, hold_times, {{pos_std}, {ST::position, ST::position}}, {{pos_std}, {ST::position, ST::position}}); + + ImmModelT imm_model(jump_mat, hold_times, {pos_std}, {pos_std}); auto sensor_model = std::make_shared>(dt); - using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; + using ImmFilterT = vortex::filter::ImmFilter>; Eigen::Vector2d model_weights{0.5, 0.5}; @@ -194,7 +187,7 @@ TEST(ImmFilter, mixing_two_of_the_same_model) std::tuple x_est_prevs{x_est_prev_1, x_est_prev_2}; - auto [x_est_1, x_est_2] = ImmFilterT::mixing(x_est_prevs, imm_model.get_pi_mat_d(dt), imm_model.get_all_state_names()); + auto [x_est_1, x_est_2] = ImmFilterT::mixing(x_est_prevs, imm_model.get_pi_mat_d(dt)); // The high uncertainty in the second model should make it's position estimate move more towards the first // model than the first model moves towards the second @@ -216,17 +209,13 @@ TEST(ImmFilter, mixing_two_different_models) Eigen::Vector2d hold_times{1, 1}; using ImmModelT = ImmModel; - using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; + using ImmFilterT = vortex::filter::ImmFilter>; double dt = 1; double std_pos = 0.1; double std_vel = 0.1; - using ST = vortex::models::StateType; - ImmModelT imm_model{jump_mat, - hold_times, - std::tuple{ConstantPosition(std_pos), std::array{ST::position, ST::position}}, - std::tuple{ConstantVelocity(std_vel), std::array{ST::position, ST::position, ST::velocity, ST::velocity}}}; + ImmModelT imm_model{jump_mat, hold_times, {std_pos}, {std_vel}}; auto sensor_model = std::make_shared>(dt); @@ -236,13 +225,13 @@ TEST(ImmFilter, mixing_two_different_models) std::tuple x_est_prevs{x_est_prev_1, x_est_prev_2}; // clang-format off - using ST = vortex::models::StateType; - vortex::models::StateMap states_min_max{ - {ST::velocity, {-100 , 100}} + + vortex::StateMap states_min_max{ + {vortex::StateName::velocity, {-100 , 100}} }; // clang-format on - auto [x_est_1, x_est_2] = ImmFilterT::mixing(x_est_prevs, imm_model.get_pi_mat_d(dt), imm_model.get_all_state_names(), states_min_max); + auto [x_est_1, x_est_2] = ImmFilterT::mixing(x_est_prevs, imm_model.get_pi_mat_d(dt), states_min_max); std::cout << "x_est_prev_1:\n" << x_est_prev_1 << std::endl; std::cout << "x_est_prev_2:\n" << x_est_prev_2 << std::endl; @@ -262,17 +251,13 @@ TEST(ImmFilter, modeMatchedFilter) Eigen::Vector2d hold_times{1, 1}; using ImmModelT = ImmModel; - using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; + using ImmFilterT = vortex::filter::ImmFilter>; double dt = 1; double std_pos = 0.1; double std_vel = 0.1; - using ST = vortex::models::StateType; - ImmModelT imm_model{jump_mat, - hold_times, - std::tuple{ConstantPosition(std_pos), std::array{ST::position, ST::position}}, - std::tuple{ConstantVelocity(std_vel), std::array{ST::position, ST::position, ST::velocity, ST::velocity}}}; + ImmModelT imm_model{jump_mat, hold_times, {std_pos}, {std_vel}}; IdentitySensorModel<2, 2> sensor_model{dt}; @@ -310,13 +295,9 @@ TEST(ImmFilter, updateProbabilities) double std_vel = 1; using ImmModelT = ImmModel; - using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; + using ImmFilterT = vortex::filter::ImmFilter>; - using ST = vortex::models::StateType; - ImmModelT imm_model{jump_mat, - hold_times, - std::tuple{ConstantPosition(std_pos), std::array{ST::position, ST::position}}, - std::tuple{ConstantVelocity(std_vel), std::array{ST::position, ST::position, ST::velocity, ST::velocity}}}; + ImmModelT imm_model{jump_mat, hold_times, {std_pos}, {std_vel}}; auto sensor_model = std::make_shared>(dt); @@ -337,6 +318,7 @@ TEST(ImmFilter, step) using namespace vortex::models; using namespace vortex::filter; using namespace vortex::prob; + using vortex::StateName; double dt = 1; // clang-format off @@ -351,13 +333,9 @@ TEST(ImmFilter, step) double std_turn_rate = 0.1; using ImmModelT = ImmModel; - using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; + using ImmFilterT = vortex::filter::ImmFilter>; - ImmModelT imm_model{jump_mat, - hold_times, - {ConstantPosition(std_pos), ConstantPosition::StateNames}, - {ConstantVelocity(std_vel), ConstantVelocity::StateNames}, - {CoordinatedTurn(std_vel, std_turn_rate), CoordinatedTurn::StateNames}}; + ImmModelT imm_model{jump_mat, hold_times, ConstantPosition(std_pos), ConstantVelocity(std_vel), CoordinatedTurn(std_vel, std_turn_rate)}; IdentitySensorModel<2, 2> sensor_model{dt}; @@ -367,7 +345,7 @@ TEST(ImmFilter, step) Gauss2d::Standard(), {{0, 0, 0.9, 0}, Eigen::Matrix4d::Identity()}, {{0, 0, 0.9, 0, 1}, Eigen::Matrix::Identity()}}; Eigen::Vector2d z_meas = {1, 0}; - StateMap states_min_max{{StateType::velocity, {-10, 10}}, {StateType::turn_rate, {-M_PI, M_PI}}}; + vortex::StateMap states_min_max{{StateName::velocity, {-10, 10}}, {StateName::turn_rate, {-std::numbers::pi, std::numbers::pi}}}; auto [weights_upd, x_est_upds, x_est_preds, z_est_preds] = ImmFilterT::step(imm_model, sensor_model, dt, x_est_prevs, z_meas, model_weights, states_min_max); diff --git a/vortex-filtering/test/immipda_test.cpp b/vortex-filtering/test/immipda_test.cpp new file mode 100644 index 00000000..2c7cf784 --- /dev/null +++ b/vortex-filtering/test/immipda_test.cpp @@ -0,0 +1,71 @@ +#define EIGEN_NO_AUTOMATIC_RESIZING +#include +#include +#include + +class IMMIPDA : public ::testing::Test { +protected: + using DynMod1_ = vortex::models::ConstantPosition; + using DynMod2_ = vortex::models::ConstantVelocity; + using SensMod_ = vortex::models::IdentitySensorModel<2, 2>; + using ImmModel_ = vortex::models::ImmModel; + using IMMIPDA_ = vortex::filter::IMMIPDA; + + using S = vortex::StateName; + + IMMIPDA() + : imm_model_(jump_matrix, hold_times, DynMod1_(0.5), DynMod2_(0.5)) + , sensor_model_(2) + , config_{.pdaf = + { + .mahalanobis_threshold = 1.0, + .min_gate_threshold = 0.0, + .max_gate_threshold = std::numeric_limits::max(), + .prob_of_detection = 0.9, + .clutter_intensity = 1.0, + }, + .ipda = + { + .prob_of_survival = 0.9, + .estimate_clutter = true, + .update_existence_probability_on_no_detection = true, + }, + .immipda = { + .states_min_max = {{S::position, {-100.0, 100.0}}, {S::velocity, {-10.0, 10.0}}}, + }} {}; + + double dt_ = 1; + Eigen::Matrix2d jump_matrix{{0.0, 1.0}, {1.0, 0.0}}; + Eigen::Vector2d hold_times{100.0, 100.0}; + + ImmModel_ imm_model_; + SensMod_ sensor_model_; + IMMIPDA_::Config config_; +}; + +TEST_F(IMMIPDA, step) +{ + using namespace vortex; + + + auto [min_vel, max_vel] = config_.immipda.states_min_max.at(S::velocity); + + Eigen::Matrix4d mode_2_cov = prob::Uniform<4>{{-1.0, -1.0, min_vel, min_vel}, {1.0, 1.0, max_vel, max_vel}}.cov(); + + ImmModel_::GaussTuple_x x0 = {prob::Gauss2d::Standard(), {{0.0, 0.0, 1.0, 0.0}, mode_2_cov}}; + ImmModel_::Vec_n model_weights = {0.5, 0.5}; + + Eigen::Array z0 = { + {1.0, 1.0, 1.0, 20}, + {0.1, -0.1, 0.0, 0}, + }; + + IMMIPDA_::Output out = IMMIPDA_::step(imm_model_, sensor_model_, dt_, {x0, model_weights, 0.5}, z0, config_); + + ASSERT_EQ(out.gated_measurements.colwise().any().count(), 3); + + ASSERT_GT(out.state.mode_probabilities(0), 0.0); + ASSERT_GT(out.state.mode_probabilities(1), 0.0); + ASSERT_LT(out.state.mode_probabilities(0), 1.0); + ASSERT_LT(out.state.mode_probabilities(1), 1.0); +} \ No newline at end of file diff --git a/vortex-filtering/test/ipda_test.cpp b/vortex-filtering/test/ipda_test.cpp index 120d620c..e92ec415 100644 --- a/vortex-filtering/test/ipda_test.cpp +++ b/vortex-filtering/test/ipda_test.cpp @@ -10,45 +10,58 @@ using IPDA = vortex::filter::IPDA meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, - { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; + Eigen::Array z_meas = {{0.0, 1.0, 1.0, 0.0, 1.0, 1.0}, {0.0, 2.0, 2.0, 0.0, 2.0, 2.0}}; ConstantVelocity dyn_model{1.0}; IdentitySensorModel sen_model{1.0}; - auto [x_final, existence_pred, inside, outside, x_pred, z_pred, x_updated] = - IPDA::step(dyn_model, sen_model, 1.0, x_est, meas, last_detection_probability, config); + auto [state_post, x_pred, z_pred, x_updated, gated_measurements] = + IPDA::step(dyn_model, sen_model, 1.0, {x_est, last_detection_probability}, z_meas, config); - std::cout << "Existence probability: " << existence_pred << std::endl; + std::cout << "Existence probability: " << state_post.existence_probability << std::endl; } TEST(IPDA, get_existence_probability_is_calculating) { - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; - double probability_of_survival = 0.9; + IPDA::Config config = { + .pdaf = + { + .prob_of_detection = 0.8, + .clutter_intensity = 1.0, + }, + .ipda = + { + .prob_of_survival = 0.9, + }, + }; + double last_detection_probability = 0.9; - std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, - { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; + Eigen::Array meas = {{0.0, 1.0, 1.0, 0.0, 1.0, 1.0}, {0.0, 2.0, 2.0, 0.0, 2.0, 2.0}}; - vortex::prob::Gauss2d z_pred; - z_pred = vortex::prob::Gauss2d(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity() * 0.1); + vortex::prob::Gauss2d z_pred = {{1.0, 1.0}, Eigen::Matrix2d::Identity() * 0.1}; - double existence_probability = IPDA::get_existence_probability( - meas, probability_of_survival, last_detection_probability, prob_of_detection, clutter_intensity, z_pred); + double existence_probability = IPDA::existence_prob_update(meas, z_pred, last_detection_probability, config); std::cout << "Existence probability: " << existence_probability << std::endl; } \ No newline at end of file diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 1bdd5b08..84296788 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -4,9 +4,9 @@ #include #include -using ConstantVelocity = vortex::models::ConstantVelocity; +using ConstantVelocity = vortex::models::ConstantVelocity; using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>; -using PDAF = vortex::filter::PDAF; +using PDAF = vortex::filter::PDAF; // testing the get_weights function TEST(PDAF, get_weights_is_calculating) @@ -15,7 +15,7 @@ TEST(PDAF, get_weights_is_calculating) double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = { { 0.0, 0.0 }, { 2.0, 1.0 } }; + Eigen::Array2Xd meas = {{0.0, 2.0}, {0.0, 1.0}}; Eigen::VectorXd weights = PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); @@ -30,7 +30,7 @@ TEST(PDAF, if_no_clutter_first_weight_is_zero) double clutter_intensity = 0.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = { { 0.0, 0.0 }, { 2.0, 1.0 } }; + Eigen::Array2Xd meas = {{0.0, 2.0}, {0.0, 1.0}}; Eigen::VectorXd weights = PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); @@ -45,7 +45,7 @@ TEST(PDAF, weights_are_decreasing_with_distance) double clutter_intensity = 1.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - std::vector meas = { { 2.0, 1.0 }, { 3.0, 1.0 }, { 4.0, 1.0 } }; + Eigen::Array2Xd meas = {{2.0, 3.0, 4.0}, {1.0, 1.0, 1.0}}; Eigen::VectorXd weights = PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); @@ -63,7 +63,7 @@ TEST(PDAF, get_weighted_average_is_calculating) vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = { { 0.0, 0.0 }, { 2.0, 1.0 } }; + Eigen::Array2Xd meas = {{0.0, 2.0}, {0.0, 1.0}}; std::vector updated_states = { vortex::prob::Gauss4d(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()), vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), Eigen::Matrix4d::Identity()) @@ -82,7 +82,8 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = { { 1.0, 2.0 } }; + Eigen::Array2Xd meas = {{1.0}, {2.0}}; + std::vector updated_states = { vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity()) }; @@ -91,7 +92,7 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); - EXPECT_LT(weighted_average.mean()(1), meas[0](1)); + EXPECT_LT(weighted_average.mean()(1), meas(1, 0)); EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); std::cout << "weighted average: " << weighted_average.mean() << std::endl; @@ -104,7 +105,8 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = { { 2.0, 1.0 } }; + Eigen::Array2Xd meas = {{2.0}, {1.0}}; + std::vector updated_states = { vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()) }; @@ -113,7 +115,7 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); - EXPECT_LT(weighted_average.mean()(0), meas[0](0)); + EXPECT_LT(weighted_average.mean()(0), meas(0, 0)); EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); std::cout << "weighted average: " << weighted_average.mean() << std::endl; @@ -126,7 +128,8 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = { { 2.0, 2.0 } }; + Eigen::Array2Xd meas = {{2.0}, {2.0}}; + std::vector updated_states = { vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity()) }; @@ -135,12 +138,12 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); - EXPECT_LT(weighted_average.mean()(0), meas[0](0)); + EXPECT_LT(weighted_average.mean()(0), meas(0, 0)); EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); - EXPECT_LT(weighted_average.mean()(1), meas[0](1)); + EXPECT_LT(weighted_average.mean()(1), meas(1, 0)); EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); std::cout << "weighted average: " << weighted_average.mean() << std::endl; @@ -149,38 +152,50 @@ TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) // testing the apply_gate function TEST(PDAF, apply_gate_is_calculating) { - double mahalanobis_threshold = 1.8; + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 1.8, + }}; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, - { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; + // clang-format off + Eigen::Array2Xd meas = {{0.0, 1.0, 1.0, 0.0, 2.0, 2.0}, + {1.0, 0.0, 1.0, 2.0, 0.0, 2.0}}; + // clang-format on - auto [inside, outside] = PDAF::apply_gate(meas, z_pred, mahalanobis_threshold); + auto gated = PDAF::apply_gate(meas, z_pred, config); } TEST(PDAF, apply_gate_is_separating_correctly) { - double mahalanobis_threshold = 3; + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 3, + }}; Eigen::Matrix2d cov; cov << 1.0, 0.0, 0.0, 4.0; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), cov); - std::vector meas = { { 0.0, 4.0 }, { 4.0, 0.0 } }; + // clang-format off + Eigen::Array2Xd meas = {{0.0, 4.0}, + {4.0, 0.0}}; + // clang-format on - auto [inside, outside] = PDAF::apply_gate(meas, z_pred, mahalanobis_threshold); + auto gated = PDAF::apply_gate(meas, z_pred, config); - EXPECT_EQ(inside.size(), 1u); - EXPECT_EQ(outside.size(), 1u); - EXPECT_EQ(inside[0], meas[0]); - EXPECT_EQ(outside[0], meas[1]); + EXPECT_TRUE(gated(0)); + EXPECT_FALSE(gated(1)); #if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; gp << "set style circle radius 0.05\n"; gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + gp.send1d(meas_vec); int object_counter = 0; @@ -188,34 +203,42 @@ TEST(PDAF, apply_gate_is_separating_correctly) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; gp << "replot\n"; - vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, mahalanobis_threshold); + vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " - << prediction.major_axis() << "," << prediction.minor_axis() << " angle " << prediction.angle_deg() - << "fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " << prediction.major_axis() << "," + << prediction.minor_axis() << " angle " << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; -#endif + #endif } TEST(PDAF, apply_gate_is_separating_correctly_2) { - double mahalanobis_threshold = 2.1; + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 2.1, + }}; vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, - { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; - auto [inside, outside] = PDAF::apply_gate(meas, z_pred, mahalanobis_threshold); + // clang-format off + Eigen::Array2Xd meas = {{0.0, 1.0, 1.0, 0.0, 2.0, 2.0}, + {1.0, 0.0, 1.0, 2.0, 0.0, 2.0}}; + // clang-format on - EXPECT_EQ(inside.size(), 5u); - EXPECT_EQ(outside.size(), 1u); + auto gated = PDAF::apply_gate(meas, z_pred, config); + + EXPECT_EQ(gated.count(), 5u); + + #if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } -#if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; gp << "set style circle radius 0.05\n"; gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + gp.send1d(meas_vec); int object_counter = 0; @@ -223,41 +246,50 @@ TEST(PDAF, apply_gate_is_separating_correctly_2) << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; gp << "replot\n"; - vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, mahalanobis_threshold); + vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " - << prediction.major_axis() << "," << prediction.minor_axis() << " angle " << prediction.angle_deg() - << "fs empty border lc rgb 'cyan'\n"; + gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " << prediction.major_axis() << "," + << prediction.minor_axis() << " angle " << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; -#endif + #endif } // testing the predict_next_state function TEST(PDAF, predict_next_state_is_calculating) { - PDAF::Config config; - config.mahalanobis_threshold = 1.12; - config.prob_of_detection = 0.8; - config.clutter_intensity = 1.0; + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 1.12, + .prob_of_detection = 0.8, + .clutter_intensity = 1.0, + }}; + vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, - { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; - ConstantVelocity dyn_model{ 1.0 }; - IdentitySensorModel sen_model{ 1.0 }; + // clang-format off + Eigen::Array2Xd meas = {{0.0, 1.0, 1.0, 0.0, 2.0, 2.0}, + {1.0, 0.0, 1.0, 2.0, 0.0, 2.0}}; + + ConstantVelocity dyn_model{1.0}; + IdentitySensorModel sen_model{1.0}; - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + auto [x_final, x_pred, z_pred, x_updated, gated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; -#if (GNUPLOT_ENABLE) + #if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) + { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; gp << "set style circle radius 0.05\n"; gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + gp.send1d(meas_vec); int object_counter = 0; @@ -266,12 +298,10 @@ TEST(PDAF, predict_next_state_is_calculating) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " - << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " - << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -286,40 +316,51 @@ TEST(PDAF, predict_next_state_is_calculating) gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " - << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() - << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; -#endif + #endif } TEST(PDAF, predict_next_state_2) { - PDAF::Config config; - config.mahalanobis_threshold = 2.0; - config.prob_of_detection = 0.8; - config.clutter_intensity = 1.0; + PDAF::Config config = {.pdaf = + { + .mahalanobis_threshold = 2.0, + .prob_of_detection = 0.8, + .clutter_intensity = 1.0, + } + }; + vortex::prob::Gauss4d x_est(Eigen::Vector4d(1.0, 1.5, -2.0, 0.0), Eigen::Matrix4d::Identity()); - std::vector meas = { { -3.0, -3.0 }, { 0.0, 0.0 }, { -1.2, 1.5 }, - { -2.0, -2.0 }, { 2.0, 0.0 }, { -1.0, 1.0 } }; - ConstantVelocity dyn_model{ 1.0 }; - IdentitySensorModel sen_model{ 0.5 }; + // clang-format off + Eigen::Array2Xd meas = {{-3.0, 0.0, -1.2, -2.0, 2.0, -1.0}, + {-3.0, 0.0, 1.5, -2.0, 0.0, 1.0}}; + // clang-format on - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + ConstantVelocity dyn_model{1.0}; + IdentitySensorModel sen_model{0.5}; + + auto [x_final, x_pred, z_pred, x_updated, gated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; -#if (GNUPLOT_ENABLE) + #if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; gp << "set style circle radius 0.05\n"; gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + gp.send1d(meas_vec); int object_counter = 0; @@ -328,12 +369,10 @@ TEST(PDAF, predict_next_state_2) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " - << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " - << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -348,40 +387,49 @@ TEST(PDAF, predict_next_state_2) gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " - << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() - << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; -#endif + #endif } TEST(PDAF, predict_next_state_3_1) { - PDAF::Config config; - config.mahalanobis_threshold = 4.0; - config.prob_of_detection = 0.9; - config.clutter_intensity = 1.0; + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 4.0, + .prob_of_detection = 0.9, + .clutter_intensity = 1.0, + }}; + vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.5, 0.5, -0.75, -0.75), Eigen::Matrix4d::Identity()); - std::vector meas = { { 0.0, 0.5 }, { 0.2, 0.2 }, { 0.8, 2.3 }, - { 0.5, 0.0 }, { 4.2, 2.7 }, { 1.4, 2.5 } }; - ConstantVelocity dyn_model{ 0.5 }; - IdentitySensorModel sen_model{ 1.0 }; + // clang-format off + Eigen::Array2Xd meas = {{0.0, 0.2, 0.8, 0.5, 4.2, 1.4}, + {0.5, 0.2, 2.3, 0.0, 2.7, 2.5}}; + // clang-format on - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; + + auto [x_final, x_pred, z_pred, x_updated, gated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; -#if (GNUPLOT_ENABLE) + #if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; gp << "set style circle radius 0.05\n"; gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + gp.send1d(meas_vec); int object_counter = 0; @@ -390,12 +438,10 @@ TEST(PDAF, predict_next_state_3_1) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " - << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " - << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -412,41 +458,48 @@ TEST(PDAF, predict_next_state_3_1) gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " - << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() - << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; -#endif + #endif } TEST(PDAF, predict_next_state_3_2) { - PDAF::Config config; - config.mahalanobis_threshold = 4.0; - config.prob_of_detection = 0.9; - config.clutter_intensity = 1.0; + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 4.0, + .prob_of_detection = 0.9, + .clutter_intensity = 1.0, + }}; vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.00173734, 0.0766262, -0.614584, -0.57184), Eigen::Matrix4d::Identity()); - std::vector meas = { { -0.5, 2.0 }, { -0.23, 0.5 }, { -2.0, 3.4 }, - { 0.0, 1.3 }, { 0.14, 0.5 }, { -2.5, 0.89 } }; - ConstantVelocity dyn_model{ 0.5 }; - IdentitySensorModel sen_model{ 1.0 }; + // clang-format off + Eigen::Array2Xd meas = {{-0.5, -0.23, -2.0, 0.0, 0.14, -2.5}, + {2.0 , 0.5, 3.4, 1.3, 0.5 , 0.89}}; + // clang-format on + + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + auto [x_final, x_pred, z_pred, x_updated, gated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; -#if (GNUPLOT_ENABLE) + #if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; gp << "set style circle radius 0.05\n"; gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + gp.send1d(meas_vec); int object_counter = 0; @@ -455,12 +508,10 @@ TEST(PDAF, predict_next_state_3_2) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " - << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " - << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -481,40 +532,48 @@ TEST(PDAF, predict_next_state_3_2) gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 << " nohead lc rgb 'orange-red'\n"; - vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " - << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() - << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; -#endif + #endif } TEST(PDAF, predict_next_state_3_3) { - PDAF::Config config; - config.mahalanobis_threshold = 4.0; - config.prob_of_detection = 0.9; - config.clutter_intensity = 1.0; + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 4.0, + .prob_of_detection = 0.9, + .clutter_intensity = 1.0, + }}; vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.55929, 0.0694888, -0.583476, -0.26382), Eigen::Matrix4d::Identity()); - std::vector meas = { { -1.5, 2.5 }, { -1.2, 2.7 }, { -0.8, 2.3 }, - { -1.7, 1.9 }, { -2.0, 3.0 }, { -1.11, 2.04 } }; - ConstantVelocity dyn_model{ 0.5 }; - IdentitySensorModel sen_model{ 1.0 }; + // clang-format off + Eigen::Array2Xd meas = {{-1.5, -1.2, -0.8, -1.7, -2.0, -1.11}, + { 2.5, 2.7, 2.3, 1.9, 3.0, 2.04}}; + // clang-format on + + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + auto [x_final, x_pred, z_pred, x_updated, gated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; -#if (GNUPLOT_ENABLE) + #if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; gp << "set style circle radius 0.05\n"; gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + gp.send1d(meas_vec); int object_counter = 0; @@ -523,12 +582,10 @@ TEST(PDAF, predict_next_state_3_3) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " - << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " - << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -553,40 +610,48 @@ TEST(PDAF, predict_next_state_3_3) gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; - vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " - << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() - << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; -#endif + #endif } TEST(PDAF, predict_next_state_3_4) { - PDAF::Config config; - config.mahalanobis_threshold = 4.0; - config.prob_of_detection = 0.9; - config.clutter_intensity = 1.0; + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 4.0, + .prob_of_detection = 0.9, + .clutter_intensity = 1.0, + }}; + vortex::prob::Gauss4d x_est(Eigen::Vector4d(-1.20613, 0.610616, -0.618037, 0.175242), Eigen::Matrix4d::Identity()); - std::vector meas = { { -2.0, 2.2 }, { -1.8, 2.3 }, { -2.3, 2.0 }, - { 0.6, 1.5 }, { -2.0, 2.0 }, { -1.4, 2.5 } }; + // clang-format off + Eigen::Array2Xd meas = {{-2.0, -1.8, -2.3, 0.6, -2.0, -1.4}, + { 2.2, 2.3, 2.0, 1.5, 2.0, 2.5}}; + // clang-format on - ConstantVelocity dyn_model{ 0.5 }; - IdentitySensorModel sen_model{ 1.0 }; + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = + auto [x_final, x_pred, z_pred, x_updated, gated] = PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); std::cout << "x_final: " << x_final.mean() << std::endl; -#if (GNUPLOT_ENABLE) + #if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; gp << "set size ratio -1\n"; gp << "set style circle radius 0.05\n"; gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas); + gp.send1d(meas_vec); int object_counter = 0; @@ -595,12 +660,10 @@ TEST(PDAF, predict_next_state_3_4) vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " - << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " << ellipse.angle_deg() - << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," + << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " - << 0.02 << " fs empty border lc rgb 'blue'\n"; + gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; } gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " @@ -629,36 +692,10 @@ TEST(PDAF, predict_next_state_3_4) gp << "set arrow from " << -0.55929 << "," << 0.0694888 << " to " << -1.20613 << "," << 0.610616 << " nohead lc rgb 'orange-red'\n"; - vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " - << gate.major_axis() << "," << gate.minor_axis() << " angle " << gate.angle_deg() - << " fs empty border lc rgb 'cyan'\n"; + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() + << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; gp << "replot\n"; -#endif -} - -TEST(PDAF_2, test_with_other_model) -{ - using TestDynamicModel = vortex::models::ConstantAcceleration; - using TestSensorModel = vortex::models::IdentitySensorModel<6, 2>; - using PDAF = vortex::filter::PDAF; - - PDAF::Config config; - config.mahalanobis_threshold = 1.0; - config.prob_of_detection = 0.8; - config.clutter_intensity = 1.0; - vortex::prob::Gauss6d x_est({ 0.0, 1.0, 3.0, -1.0, 0.0, 0.5 }, Eigen::MatrixXd::Identity(6, 6)); - std::vector meas = { { 0.0, 1.0 }, { 1.0, 0.0 }, { 1.0, 1.0 }, - { 0.0, 2.0 }, { 2.0, 0.0 }, { 2.0, 2.0 } }; - - TestDynamicModel dyn_model{ 1.0, 1.0 }; - TestSensorModel sen_model{ 1.0 }; - - auto [x_final, inside, outside, x_pred, z_pred, x_updated] = - PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); - std::cout << "x_final: " << x_final.mean() << std::endl; - - // it compiles -> other template parameters are working with the PDAF class - ASSERT_TRUE(true); + #endif } \ No newline at end of file diff --git a/vortex-filtering/test/probability_test.cpp b/vortex-filtering/test/probability_test.cpp index af966edd..761a3594 100644 --- a/vortex-filtering/test/probability_test.cpp +++ b/vortex-filtering/test/probability_test.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include @@ -41,18 +42,18 @@ TEST(MultiVarGauss, copyConstructor) TEST(MultiVarGauss, pdf) { vortex::prob::MultiVarGauss<2> gaussian(Eigen::Vector2d::Zero(), Eigen::Matrix2d::Identity()); - - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 0}), 1 / (2 * M_PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 0}), 1 / (2 * std::sqrt(M_E) * M_PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 1}), 1 / (2 * std::sqrt(M_E) * M_PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 1}), 1 / (2 * M_E * M_PI), 1e-15); + constexpr double PI = std::numbers::pi; + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 0}), 1 / (2 * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 0}), 1 / (2 * std::sqrt(M_E) * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 1}), 1 / (2 * std::sqrt(M_E) * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 1}), 1 / (2 * M_E * PI), 1e-15); gaussian = vortex::prob::MultiVarGauss<2>(Eigen::Vector2d{0, 0}, Eigen::Matrix2d{{2, 1}, {1, 2}}); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 0}), 1 / (2 * std::sqrt(3) * M_PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 0}), 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * M_PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 1}), 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * M_PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 1}), 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * M_PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 0}), 1 / (2 * std::sqrt(3) * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 0}), 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 1}), 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 1}), 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * PI), 1e-15); } TEST(MultiVarGauss, invalidCovariance) diff --git a/vortex-filtering/test/state_test.cpp b/vortex-filtering/test/state_test.cpp new file mode 100644 index 00000000..728673d1 --- /dev/null +++ b/vortex-filtering/test/state_test.cpp @@ -0,0 +1,123 @@ +#include "gtest_assertions.hpp" + +#include +#include + +TEST(State, typeChecks) +{ + using namespace vortex; + + using S = StateName; + using StateT = State; + + ASSERT_EQ(StateT::N_STATES, 4); + ASSERT_EQ(StateT::UNIQUE_STATES_COUNT, 3); + ASSERT_EQ(StateT::state_loc(S::position).start_index, 0); + ASSERT_EQ(StateT::state_loc(S::velocity).start_index, 1); + ASSERT_EQ(StateT::state_loc(S::acceleration).start_index, 3); + + ASSERT_EQ(StateT::state_loc(S::position).end_index, 1); + ASSERT_EQ(StateT::state_loc(S::velocity).end_index, 3); + ASSERT_EQ(StateT::state_loc(S::acceleration).end_index, 4); + + ASSERT_TRUE(StateT::has_state_name(S::position)); + ASSERT_TRUE(StateT::has_state_name(S::velocity)); + ASSERT_TRUE(StateT::has_state_name(S::acceleration)); + ASSERT_FALSE(StateT::has_state_name(S::jerk)); +} + +TEST(State, init) +{ + using namespace vortex; + + using S = StateName; + using StateT = State; + + auto x = prob::Gauss4d::Standard(); + StateT state(x); + + ASSERT_EQ(state.mean(), x.mean()); + ASSERT_EQ(state.cov(), x.cov()); + ASSERT_EQ(state.gauss(), x); +} + +TEST(State, getMean) +{ + using namespace vortex; + + using S = StateName; + using StateT = State; + + auto x = prob::Gauss4d::Standard(); + StateT state(x); + + ASSERT_TRUE(isApproxEqual(state.mean_of(), x.mean().template head<1>(), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.mean_of(), x.mean().template segment<2>(1), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.mean_of(), x.mean().template tail<1>(), 1e-6)); +} + +TEST(State, getCov) +{ + using namespace vortex; + + using S = StateName; + using StateT = State; + + auto x = prob::Gauss4d::Standard(); + StateT state(x); + + ASSERT_TRUE(isApproxEqual(state.cov_of(), x.cov().template block<1, 1>(0, 0), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.cov_of(), x.cov().template block<2, 2>(1, 1), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.cov_of(), x.cov().template block<1, 1>(3, 3), 1e-6)); +} + +TEST(State, setMean) +{ + using namespace vortex; + + using S = StateName; + using StateT = State; + + auto x = prob::Gauss4d::Standard(); + StateT state(x); + + StateT::T::Vec_n mean = StateT::T::Vec_n::Random(); + StateT::T::Mat_nn cov = StateT::T::Mat_nn::Random(); + + cov = 0.5 * (cov + cov.transpose()).eval(); + cov += StateT::T::Mat_nn::Identity() * StateT::N_STATES; + + StateT::T::Gauss_n x_new = {mean, cov}; + state.set_mean_of(x_new.mean().template head<1>()); + state.set_mean_of(x_new.mean().template segment<2>(1)); + state.set_mean_of(x_new.mean().template tail<1>()); + + ASSERT_TRUE(isApproxEqual(state.mean(), x_new.mean(), 1e-6)); +} + +TEST(State, setCov) +{ + using namespace vortex; + + using S = StateName; + using StateT = State; + + auto x = prob::Gauss4d::Standard(); + StateT state(x); + + StateT::T::Vec_n mean = StateT::T::Vec_n::Random(); + StateT::T::Mat_nn cov = StateT::T::Mat_nn::Random(); + cov = 0.5 * (cov + cov.transpose()).eval(); + cov += StateT::T::Mat_nn::Identity() * StateT::N_STATES; + + StateT::T::Gauss_n x_new = {mean, cov}; + state.set_cov_of(x_new.cov().template block<1, 1>(0, 0)); + state.set_cov_of(x_new.cov().template block<2, 2>(1, 1)); + state.set_cov_of(x_new.cov().template block<1, 1>(3, 3)); + + ASSERT_TRUE(isApproxEqual(state.cov_of(), x_new.cov().template block<1, 1>(0, 0), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.cov_of(), x_new.cov().template block<2, 2>(1, 1), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.cov_of(), x_new.cov().template block<1, 1>(3, 3), 1e-6)); + + ASSERT_FALSE(isApproxEqual(state.cov(), x_new.cov(), 1e-6)); +} \ No newline at end of file diff --git a/vortex-filtering/test/types_test.cpp b/vortex-filtering/test/types_test.cpp index e470484c..695689d0 100644 --- a/vortex-filtering/test/types_test.cpp +++ b/vortex-filtering/test/types_test.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include TEST(Types, x_2_z_1) @@ -29,6 +30,22 @@ TEST(Concepts, MatConvertibleTo) ASSERT_TRUE(true); } +TEST(Concepts, MultiVarGaussLike) +{ + using vortex::prob::Gauss2d, vortex::prob::Gauss3d; + + static_assert(vortex::concepts::MultiVarGaussLike); + static_assert(vortex::concepts::MultiVarGaussLike); + + static_assert(!vortex::concepts::MultiVarGaussLike); + + using S = vortex::StateName; + using StateT = vortex::State; + static_assert(vortex::concepts::MultiVarGaussLike); + + ASSERT_TRUE(true); +} + TEST(Concepts, Models) { constexpr size_t N_DIM_x = 5;