diff --git a/.clang-format b/.clang-format index 129df136..82e1136c 100644 --- a/.clang-format +++ b/.clang-format @@ -23,7 +23,7 @@ AlwaysBreakAfterDefinitionReturnType: None AlwaysBreakAfterReturnType: None AlwaysBreakBeforeMultilineStrings: false AlwaysBreakTemplateDeclarations: MultiLine -BinPackArguments: true +BinPackArguments: false BinPackParameters: true BraceWrapping: AfterCaseLabel: false @@ -47,8 +47,8 @@ BreakBeforeBraces: Stroustrup BreakBeforeInheritanceComma: false BreakInheritanceList: BeforeColon BreakBeforeTernaryOperators: true -BreakConstructorInitializersBeforeComma: false -BreakConstructorInitializers: BeforeColon +BreakConstructorInitializersBeforeComma: true +BreakConstructorInitializers: BeforeComma BreakAfterJavaFieldAnnotations: false BreakStringLiterals: true ColumnLimit: 160 @@ -67,7 +67,7 @@ ForEachMacros: - foreach - Q_FOREACH - BOOST_FOREACH -IncludeBlocks: Preserve +IncludeBlocks: Regroup IncludeCategories: - Regex: '^"(llvm|llvm-c|clang|clang-c)/' Priority: 2 diff --git a/.github/workflows/run-unit-tests.yml b/.github/workflows/run-unit-tests.yml index 5144e7dc..22a63565 100644 --- a/.github/workflows/run-unit-tests.yml +++ b/.github/workflows/run-unit-tests.yml @@ -52,6 +52,15 @@ jobs: uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distribution }} + - name: Use gcc 13 and g++ 13 + run: | + sudo apt-get update + sudo apt-get install -y software-properties-common + sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test + sudo apt-get update + sudo apt-get install -y gcc-13 g++-13 + sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-13 100 --slave /usr/bin/g++ g++ /usr/bin/g++-13 + sudo update-alternatives --config gcc - name: Build and test ROS 2 packages uses: ros-tooling/action-ros-ci@v0.3 id: action_ros_ci_step diff --git a/.gitignore b/.gitignore index 600d2d33..2abcb93d 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,4 @@ -.vscode \ No newline at end of file +.vscode +build/* +install/* +log/* \ No newline at end of file diff --git a/README.md b/README.md index 15e8d3c5..0af7be9e 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,9 @@ # Vortex VKF ## Introduction -This is a C++ library for state estimation. +This is a C++ library for state estimation. VKF stands for Visual Kalman Filter which is a stupid and misleading name ## Packages -### vortex-filtering -Currently, it has a pure C++ implementation of the Unscented Kalman Filter (UKF) and the Extended Kalman Filter (EKF) as well as model -[More info](vortex-filtering/README.md) + +- [vortex-filtering](vortex-filtering/README.md) + + diff --git a/docs/vkf_class_diagram.md b/docs/vkf_class_diagram.md deleted file mode 100644 index 60943286..00000000 --- a/docs/vkf_class_diagram.md +++ /dev/null @@ -1,71 +0,0 @@ - - -```mermaid -classDiagram - - DynamicModel <|-- DynamicModelLTV - SensorModel <|-- SensorModelLTV - DynamicModelLTV <|-- ConstantVelocity - DynamicModelLTV <|-- ConstantAcceleration - DynamicModelLTV <|-- CoordinatedTurn - - EKF -- DynamicModelLTV - EKF -- SensorModelLTV - - UKF -- DynamicModel - UKF -- SensorModel - - - - class EKF{ - +predict() - +update() - +step() - } - - class UKF{ - +predict() - +update() - +step() - -get_sigma_points() - -propagate_sigma_points_f() - -propagate_sigma_points_h() - -estimate_gaussian() - } - - class DynamicModel{ - +virtual f_d() Vec_x - +virtual Q_d() Mat_vv - +sample_f_d() Vec_x - } - - class SensorModel{ - +virtual h() Vec_z - +virtual R() Mat_ww - +sample_h() Vec_z - } - - class DynamicModelLTV { - +overide f_d() Vec_x - +virtual A_d() Mat_xx - +virtual Q_d() Mat_vv - +vurtual G_d() Mat_xv - +pred_from_est() Gauss_x - +pred_from_state() Gauss_x - } - - class SensorModelLTV { - +override h(x) Vec_z - +virtual R(x) Mat_ww - +virtual C(x) Mat_zx - +virtual H(x) Mat_zw - +pred_from_est(x_est) Gauss_z - +pred_from_state(x) Gauss_z - } - - class ConstantVelocity - class CoordinatedTurn - class ConstantAcceleration - -``` - \ No newline at end of file diff --git a/vortex-filtering/CMakeLists.txt b/vortex-filtering/CMakeLists.txt index 6bd36d76..c4211fd3 100644 --- a/vortex-filtering/CMakeLists.txt +++ b/vortex-filtering/CMakeLists.txt @@ -13,6 +13,8 @@ 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 + -Warray-bounds # For better array bounds error messages ) endif() @@ -32,7 +34,11 @@ find_package(Boost REQUIRED filesystem ) # for gnuplot -# = Specify dependencies + +# === Include directories === +include_directories(include) + +# = Specify the dependencies set(lib_deps Eigen3 OpenMP @@ -84,8 +90,8 @@ install( DESTINATION include ) -# === Build tests === if(BUILD_TESTING) + # add_compile_definitions(GNUPLOT_ENABLE=1) add_subdirectory(test) endif() diff --git a/vortex-filtering/README.md b/vortex-filtering/README.md index 8ed106a4..c1fa9304 100644 --- a/vortex-filtering/README.md +++ b/vortex-filtering/README.md @@ -6,4 +6,72 @@ Contains the models used in the filters. The models are implemented as classes t Contains the filters. The filters are implemented as classes that inherit from the `KalmanFilterBase` class. The filters are implemented in the `filters` namespace. [More info](include/vortex_filtering/filters/README.md) ## Class Diagram -![Class Diagram](../docs/vkf_class_diagram.md) \ No newline at end of file + +```mermaid +classDiagram + + DynamicModel <|-- DynamicModelLTV + SensorModel <|-- SensorModelLTV + DynamicModelLTV <|-- ConstantVelocity + DynamicModelLTV <|-- ConstantAcceleration + DynamicModelLTV <|-- CoordinatedTurn + + EKF -- DynamicModelLTV + EKF -- SensorModelLTV + + UKF -- DynamicModel + UKF -- SensorModel + + + + class EKF{ + +predict() + +update() + +step() + } + + class UKF{ + +predict() + +update() + +step() + -get_sigma_points() + -propagate_sigma_points_f() + -propagate_sigma_points_h() + -estimate_gaussian() + } + + class DynamicModel{ + +virtual f_d() Vec_x + +virtual Q_d() Mat_vv + +sample_f_d() Vec_x + } + + class SensorModel{ + +virtual h() Vec_z + +virtual R() Mat_ww + +sample_h() Vec_z + } + + class DynamicModelLTV { + +overide f_d() Vec_x + +virtual A_d() Mat_xx + +virtual Q_d() Mat_vv + +vurtual G_d() Mat_xv + +pred_from_est() Gauss_x + +pred_from_state() Gauss_x + } + + class SensorModelLTV { + +override h(x) Vec_z + +virtual R(x) Mat_ww + +virtual C(x) Mat_zx + +virtual H(x) Mat_zw + +pred_from_est(x_est) Gauss_z + +pred_from_state(x) Gauss_z + } + + class ConstantVelocity + class CoordinatedTurn + class ConstantAcceleration + +``` \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/filters/README.md b/vortex-filtering/include/vortex_filtering/filters/README.md index 6d07a7a6..b26afe81 100644 --- a/vortex-filtering/include/vortex_filtering/filters/README.md +++ b/vortex-filtering/include/vortex_filtering/filters/README.md @@ -1,38 +1,229 @@ # Filters -This folder contains various filters +This folder contains various filters for state estimation and target tracking. The filters are based on the Kalman filter and its extensions, and are designed to work with the models in the `vortex::models` namespace. All classes and functions are under the namespace `vortex::filters`. -## EKF -This class represents an [Extended Kalman Filter](https://en.wikipedia.org/wiki/Extended_Kalman_filter). It is a template class with parameters `DynamicModelT` and `SensorModelT` for the dynamic model and sensor model respectively. The models have to be derived from `vortex::models::DynamicModelLTV` and `vortex::models::SensorModelLTV`. All methods are static, so there is no need to create an instance of this class. +Here is a great intro to Kalman filters in general: [Kalman and Bayesian Filters in Python](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/00-Preface.ipynb) -### Usage +## Overview +- [`ekf.hpp`](ekf.hpp) contains the EKF filter +- [`ukf.hpp`](ukf.hpp) contains the UKF filter +- [`imm_filter.hpp`](imm_filter.hpp) contains the IMM filter + +### EKF +This class represents an [Extended Kalman Filter](https://en.wikipedia.org/wiki/Extended_Kalman_filter). It is a template class with parameters `DynamicModelT` and `SensorModelT` for the dynamic model and sensor model respectively. It works with models derived from `vortex::models::DynamicModelLTV` and `vortex::models::SensorModelLTV`. All methods are static, so there is no need to create an instance of this class. + +#### Usage + + +#### Example ```cpp -// Make typedef for the EKF using the dynamic and sensor models -using EKF = vortex::filters::EKF; +#include + +// Create aliases for the models and EKF (optional) +using DynModT = vortex::models::ConstantVelocity; +using SensModT = vortex::models::RangeBearingSensor; + +using EKF = vortex::filters::EKF; + // Create the dynamic model and sensor model -auto dynamic_model = std::make_shared(...); -auto sensor_model = std::make_shared(...); +double std_vel = 1.0; -// Initial values -Gauss_x x_est_prev = ...; -Vec_z z_meas = ...; +double std_range = 0.1; +double std_bearing = 0.1; + +auto dynamic_model = std::make_shared(std_vel); +auto sensor_model = std::make_shared(std_range, std_bearing); + +// Get the sizes of the state and measurement vectors +constexpr int N_DIM_x = DynModT::N_DIM_x; +constexpr int N_DIM_z = SensModT::N_DIM_z; + +// Get all types used in the models +using T = vortex::Types_xz; + +// Initial estimate +typename T::Gauss_x x_est_prev{50, 60, 0, 0}; + +// Measurement +typename T::Vec_z z_meas{48, 65}; // Estimate the next state auto [x_est_upd, x_est_pred, z_est_pred] = EKF::step(dynamic_model, sensor_model, dt, x_est_prev, z_meas); ``` -## UKF +### UKF [UKF explained](https://towardsdatascience.com/the-unscented-kalman-filter-anything-ekf-can-do-i-can-do-it-better-ce7c773cf88d) The UKF can take any model derived from `vortex::models::DynamicModel` and `vortex::models::SensorModel`. All methods are static, so there is no need to create an instance of this class. -### Usage +#### Usage +The EKF and UKF share mostly the same interface and so it can be used for everything the EKF can. The main purpose of it is that it works with nonlinear models. + +The UKF parameters $\alpha$, $\beta$ and $\kappa$ are set to 1.0, 2.0 and 0.0 by default. These can be changed by passing them as template arguments after the models. I don't know a reason for why you would want to change these, but the option is there anyways. + +#### Example ```cpp -// Same as EKF +// Same as the EKF but with a U instead of an E ``` -## IMM Filter -This class represents an [Interacting Multiple Model Filter](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/14-Adaptive-Filtering.ipynb). It is a template class with parameters `SensModelT` and `ImmModelT` for the sensor model and IMM model respectively. All methods are static, so there is no need to create an instance of this class. \ No newline at end of file +### IMM Filter +This class represents an [Interacting Multiple Model Filter](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/14-Adaptive-Filtering.ipynb). It is a template class with parameters `SensModT` and `ImmModT` for the sensor model and IMM model respectively. All methods are static, so there is no need to create an instance of this class. + +The IMM filter supports both linear and nonlinear models, using EKF for linear and UKF for nonlinear models derived from `vortex::models::DynamicModel` and `vortex::models::SensorModel`. + +#### Theory +The IMM filter is a filter that can switch between different models based on the probability of each model being the correct one. It does this by running multiple models in parallel and mixing the estimates from each model together. + +The workings of the IMM filter can be summarized in the following four steps: +1. Calculate Mixing Probabilities +2. Mixing +3. Mode Matching +4. Update Mixing Probabilities + +which can be visualized in the following flowchart and will be explained in more detail below. + +```mermaid +flowchart TD + +subgraph mixing_probabilities[Calculate Mixing Probabilities] + prev_weights[Previous Weights]--Transition model-->new_weights[New Mixing Weights] +end +mixing_probabilities --> Mixing +subgraph Mixing + A_1["`Model A_1`"] + B_1[Model B] + C_1[Model C] + A_2[Model A] + B_2[Model B] + C_2[Model C] + A_1 --Mixing Weights----> A_2 & B_2 & C_2 + B_1 ----> A_2 & B_2 & C_2 + C_1 ----> A_2 & B_2 & C_2 +end + +A_2 --> A_3[Model A] +B_2 --> B_3[Model B] +C_2 --> C_3[Model C] + +subgraph mode_matching[Mode Matching] + A_3 --Kalman Filter--> A_4[Model A] + B_3 --Kalman Filter--> B_4[Model B] + C_3 --Kalman Filter--> C_4[Model C] +end + +A_4 --> A_5[Model A] +B_4 --> B_5[Model B] +C_4 --> C_5[Model C] + +subgraph update_mixing_probs[Update Mixing Probabilities] + A_5 & B_5 & C_5--Measurement Model--> updated_weights[Updated Weights] +end +update_mixing_probs --> Repeat +``` + +##### 1 - Calculate Mixing Probabilities +The mixing probabilities are the probabilities of each model being the correct one. These are calculated based on the previous mixing probabilities and the transition model. The transition model is a matrix that specifies the probability of switching from one model to another and is specified when creating the [IMM model](../models/README.md#imm-model). + +##### 2 - The Mixing Step +The mixing step is the most important part of the IMM filter. It is the step where the estimates from the different models are combined and mixed together. Essentially, the IMM filter calculates the next state for each model based on a weighted average of the estimates from the other models. The weights are determined by the probability of each model being the correct one. + +__The Mixing of Non-Comparable States__ + +In order to estimate the states when mixing the models together, the IMM filter needs to know which states are comparable to each other. For example if the states of the models are + +$$ +\begin{align*} +\text{Model A:} \quad \mathbb{x} &= \begin{bmatrix} p_x & p_y \end{bmatrix}^\top \\ +\text{Model B:} \quad \mathbb{x} &= \begin{bmatrix} p_x & p_y & v_x & v_y \end{bmatrix}^\top \\ +\text{Model C:} \quad \mathbb{x} &= \begin{bmatrix} p_x & p_y & v_x & v_y & \theta \end{bmatrix}^\top \\ +\text{Model D:} \quad \mathbb{x} &= \begin{bmatrix} p_x & p_y & v_x & v_y & a_x & a_y \end{bmatrix}^\top \\ +\end{align*} +$$ + +then the IMM filter needs to know that the states $p_x$ and $p_y$ are comparable between all models, but the states $\theta$ and $a_x$ aren't. This is done by specifying the names of the states in the [IMM model](../models/README.md#imm-model). The similar states are mixed as normal as specified in Edmund Brekkes sensor fusion book, but the states that are not comparable are mixed using the method outlined in [this paper](https://www.researchgate.net/publication/289707032_Systematic_approach_to_IMM_mixing_for_unequal_dimension_states). + +The method in the paper works as long as the minimum and maximum value a state can take is fed to the mixing function. Essentially a uniform distribution is created for the states that are not comparable and the mixing is done using the mean and variance of this distribution as state estimates for the missing states. + +For example when mixing the states of model A into model B, the states $v_x$ and $v_y$ are missing. The mixing function then creates a uniform distribution for these states from the minimum and maximum values of the $v_x$ and $v_y$ states. The mean and variance of this distribution is then used as the state estimates for the missing states before the mixing is done. If the min and max isn't provided however, the mixing function will copy the state estimates from the other model as is. + +> This feature is the main reason the implementation is so much more complex than for the EKF and UKF. + +##### 3 - Mode Matching +The mode matching step is where the Kalman filter is run for each model. This is done in the same way as for the EKF and UKF, but for each model separately. + +##### 4 - Update Mixing Probabilities +The mixing probabilities are updated based on the measurements and the estimates from the mode matching step. This is done using the measurement model and the estimates from the mode matching step as well as the previous mixing probabilities. + +#### Usage +The IMM filter is used in the same way as the EKF and UKF. The main difference is that the IMM filter needs initial state probabilities as well as an initial state estimate. And instead of returning a single state estimate, it returns a tuple of state estimates and weights for each model. + +#### Example +```cpp +#include + +// Create aliases for the models and IMM filter (optional) +using CP = vortex::models::ConstantPosition; +using CV = vortex::models::ConstantVelocity; +using CT = vortex::models::CoordinatedTurn; + +using IMM = vortex::filters::IMMFilter; + +// Specify holding times and switching probabilities +Eigen::Vector3d hold_times{10.0, 10.0, 10.0}; +Eigen::Matrix3d switch_probs{ + {0.0, 0.5, 0.5}, + {0.5, 0.0, 0.5}, + {0.5, 0.5, 0.0} +}; + +double std_pos = 0.1, std_vel = 0.1, std_turn = 0.1; + +// Specify the state names of the models +using ST = vortex::models::StateType; +const std::array cp_names{ST::pos, ST::pos}; +const std::array cv_names{ST::pos, ST::pos, ST::vel, ST::vel}; +const std::array ct_names{ST::pos, ST::pos, ST::vel, ST::vel, ST::turn}; + +/* Note: for the models in this example, you can use the already defined state names instead: + auto cp_names = CP::StateNames; + auto cv_names = CV::StateNames; + auto ct_names = CT::StateNames; +But for custom models, you will have to define the state names yourself. +*/ + +// Create the IMM model and sensor model +IMM imm_model(hold_times, switch_probs, + {CP(std_pos), cp_names}, + {CV(std_vel), cv_names}, + {CT(std_vel, std_turn), ct_names}); + +using SensModT = vortex::models::IdentitySensorModel; +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}} +}; + +// Initial state probabilities +Eigen::Vector3d model_weights{0.3, 0.3, 0.4}; // Must sum to 1 + +// Initial state estimates +std::tuple x_est_prevs = { + {50, 60}, + {50, 60, 0, 0}, + {50, 60, 0, 0, 0.1} +}; + +// Measurement +Vec2d z_meas{48, 65}; + +// Estimate the next state +using ImmFilter = vortex::filters::IMMFilter; +auto [weights_upd, x_est_upds, x_est_preds, z_est_preds] = + IMM::step(imm_model, sensor_model, dt, x_est_prevs, z_meas, model_weights, states_min_max); +``` \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/filters/ekf.hpp b/vortex-filtering/include/vortex_filtering/filters/ekf.hpp index dc53e7e0..4f0fc9fe 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ekf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ekf.hpp @@ -13,57 +13,31 @@ #include #include #include +#include +#include namespace vortex::filter { /** - * @brief Extended Kalman Filter. - * @tparam DynModT Dynamic model type derived from `vortex::models::interface::DynamicModelLTV` - * @tparam SensModT Sensor model type derived from `vortex::models::interface::SensorModelLTV` + * @brief Extended Kalman Filter + * + * @tparam n_dim_x State dimension + * @tparam n_dim_z Measurement dimension + * @tparam n_dim_u Input dimension + * @tparam n_dim_v Process noise dimension + * @tparam n_dim_w Measurement noise dimension */ -template class EKF { +template class EKF_t { public: - static constexpr int N_DIM_x = DynModT::DynModI::N_DIM_x; - static constexpr int N_DIM_z = SensModT::SensModI::N_DIM_z; - static constexpr int N_DIM_u = DynModT::DynModI::N_DIM_u; - static constexpr int N_DIM_v = DynModT::DynModI::N_DIM_v; - static constexpr int N_DIM_w = SensModT::SensModI::N_DIM_w; - - using Vec_x = Eigen::Vector; - using Vec_z = Eigen::Vector; - using Vec_u = Eigen::Vector; - using Vec_v = Eigen::Vector; - using Vec_w = Eigen::Vector; - - using Mat_xx = Eigen::Matrix; - using Mat_xz = Eigen::Matrix; - using Mat_xv = Eigen::Matrix; - using Mat_xw = Eigen::Matrix; - - using Mat_zx = Eigen::Matrix; - using Mat_zz = Eigen::Matrix; - using Mat_zw = Eigen::Matrix; - - using Mat_vx = Eigen::Matrix; - using Mat_vv = Eigen::Matrix; - using Mat_vw = Eigen::Matrix; - - using Mat_wx = Eigen::Matrix; - using Mat_wv = Eigen::Matrix; - using Mat_ww = Eigen::Matrix; + static constexpr int N_DIM_x = (int)n_dim_x; + static constexpr int N_DIM_z = (int)n_dim_z; + static constexpr int N_DIM_u = (int)n_dim_u; + static constexpr int N_DIM_v = (int)n_dim_v; + static constexpr int N_DIM_w = (int)n_dim_w; - using Gauss_x = prob::MultiVarGauss; - using Gauss_z = prob::MultiVarGauss; - using Gauss_v = prob::MultiVarGauss; - using Gauss_w = prob::MultiVarGauss; + using T = Types_xzuvw; - using DynModI = models::interface::DynamicModelLTV; - using SensModI = models::interface::SensorModelLTV; - - using DynModIPtr = typename DynModI::SharedPtr; - using SensModIPtr = typename SensModI::SharedPtr; - - EKF() = delete; + EKF_t() = delete; /** Perform one EKF prediction step * @param dyn_mod Dynamic model @@ -74,11 +48,12 @@ 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 DynModIPtr &dyn_mod, const SensModIPtr &sens_mod, double dt, const Gauss_x &x_est_prev, - const Vec_u &u = Vec_u::Zero()) + 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) { - Gauss_x x_est_pred = dyn_mod->pred_from_est(dt, x_est_prev, u); - Gauss_z z_est_pred = sens_mod->pred_from_est(x_est_pred); + typename T::Gauss_x x_est_pred = 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}; } @@ -90,21 +65,22 @@ template ) { - Mat_zx C = sens_mod->C(x_est_pred.mean()); - Mat_ww R = sens_mod->R(x_est_pred.mean()); - Mat_zw H = sens_mod->H(x_est_pred.mean()); - Mat_xx P = x_est_pred.cov(); - Mat_zz S_inv = z_est_pred.cov_inv(); - Mat_xx I = Mat_xx::Identity(N_DIM_x, N_DIM_x); + 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 + typename T::Mat_zw H = sens_mod.H(x_est_pred.mean()); // Measurement noise cross-covariance + typename T::Mat_xx P = x_est_pred.cov(); // State covariance + typename T::Mat_zz S_inv = z_est_pred.cov_inv(); // Inverse of the predicted measurement covariance + typename T::Mat_xx I = T::Mat_xx::Identity(N_DIM_x, N_DIM_x); - Mat_xz W = P * C.transpose() * S_inv; // Kalman gain - Vec_z innovation = z_meas - z_est_pred.mean(); + typename T::Mat_xz W = P * C.transpose() * S_inv; // Kalman gain + typename T::Vec_z innovation = z_meas - z_est_pred.mean(); - Vec_x state_upd_mean = x_est_pred.mean() + W * innovation; + typename T::Vec_x state_upd_mean = x_est_pred.mean() + W * innovation; // Use the Joseph form of the covariance update to ensure positive definiteness - Mat_xx state_upd_cov = (I - W * C) * P * (I - W * C).transpose() + W * H * R * H.transpose() * W.transpose(); + typename T::Mat_xx state_upd_cov = (I - W * C) * P * (I - W * C).transpose() + W * H * R * H.transpose() * W.transpose(); return {state_upd_mean, state_upd_cov}; } @@ -118,14 +94,23 @@ template step(const DynModIPtr &dyn_mod, const SensModIPtr &sens_mod, double dt, const Gauss_x &x_est_prev, - const Vec_z &z_meas, const Vec_u &u = Vec_u::Zero()) + static std::tuple + 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) { auto [x_est_pred, z_est_pred] = predict(dyn_mod, sens_mod, dt, x_est_prev, u); - Gauss_x x_est_upd = update(sens_mod, x_est_pred, z_est_pred, z_meas); + typename T::Gauss_x x_est_upd = update(sens_mod, x_est_pred, z_est_pred, z_meas); return {x_est_upd, x_est_pred, z_est_pred}; } }; +/** + * @brief Extended Kalman Filter. + * @tparam DynModT Dynamic model type derived from `vortex::models::interface::DynamicModelLTV` + * @tparam SensModT Sensor model type derived from `vortex::models::interface::SensorModelLTV` + */ +template +using EKF = EKF_t; + } // namespace vortex::filter \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp b/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp index 50e38219..eec34c04 100644 --- a/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp @@ -9,65 +9,55 @@ * */ +#include #include #include - +#include #include #include - #include +#include #include #include +#include #include +#include +#include namespace vortex::filter { -template class ImmFilter { +template class ImmFilter { public: - using SensModTPtr = std::shared_ptr; - using SensModI = typename SensModT::SensModI; - static constexpr size_t N_MODELS = ImmModelT::N_MODELS; - static constexpr int N_DIM_x = SensModI::N_DIM_x; - static constexpr int N_DIM_z = SensModI::N_DIM_z; - using Vec_n = typename ImmModelT::Vec_n; - using Mat_nn = typename ImmModelT::Mat_nn; + static constexpr auto N_DIMS_x = ImmModelT::N_DIMS_x; + static constexpr int N_DIM_z = SensModT::N_DIM_z; - using Vec_x = typename SensModI::Vec_x; - using Vec_z = typename SensModI::Vec_z; + template using T = Types_xz; - using Mat_xz = typename SensModI::Mat_xz; - using Mat_zz = typename SensModI::Mat_zz; + template using DynModT = typename ImmModelT::template DynModT; - using Gauss_x = typename SensModI::Gauss_x; - using Gauss_z = typename SensModI::Gauss_z; + using Vec_n = Eigen::Vector; + using Mat_nn = Eigen::Matrix; + using Vec_z = typename T<0>::Vec_z; + using Gauss_z = typename T<0>::Gauss_z; + using GaussTuple_x = typename ImmModelT::GaussTuple_x; + using GaussArr_z = std::array; - using Vec_Gauss_x = std::vector; - using Vec_Gauss_z = std::vector; - - using GaussMix_x = prob::GaussianMixture; - using GaussMix_z = prob::GaussianMixture; - - ImmFilter() - { - // Check if the number of states in each dynamic model is the same as the number of states in the sensor model - static_assert(ImmModelT::SAME_DIMS_x, "The number of states in each dynamic model must be the same as the number of states in the sensor model."); - } + /// No need to instantiate this class. All methods are static. + ImmFilter() = delete; /** * Calculates the mixing probabilities for the IMM filter, following step 1 in (6.4.1) in the book. - * @param imm_model The IMM model. + * @param transition_matrix The discrete time transition matrix for the Markov chain. (pi_mat_d from the imm model) * @param model_weights The weights (mode probabilities) from the previous time step. - * @param dt The time step. * @return The mixing probabilities. Each element is mixing_probs[s_{k-1}, s_k] = mu_{s_{k-1}|s_k} where s is the index of the model. */ - static Mat_nn calculate_mixing_probs(const ImmModelT &imm_model, const Vec_n &model_weights, double dt) + static Mat_nn calculate_mixing_probs(const Mat_nn &transition_matrix, const Vec_n &model_weights) { - Mat_nn pi_mat = imm_model.get_pi_mat_d(dt); // mu_{s_{k-1}|s_k} = pi_{s_{k-1}|s_k} * mu_{s_{k-1}|k-1} - Mat_nn mixing_probs = pi_mat.cwiseProduct(model_weights.replicate(1, N_MODELS)); + Mat_nn mixing_probs = transition_matrix.cwiseProduct(model_weights.replicate(1, N_MODELS)); // Normalize for (int i = 0; i < mixing_probs.cols(); i++) { @@ -80,16 +70,16 @@ template mixing(const std::vector &x_est_prevs, const Mat_nn &mixing_probs) + static GaussTuple_x mixing(const GaussTuple_x &x_est_prevs, const Mat_nn &mixing_probs, const ImmModelT::StateNames &state_names, + const models::StateMap &states_min_max = {}) { - std::vector moment_based_preds; - for (const Vec_n &weights : mixing_probs.rowwise()) { - GaussMix_x mixture(weights, x_est_prevs); - moment_based_preds.push_back(mixture.reduce()); - } - return moment_based_preds; + return mix_components(x_est_prevs, mixing_probs, state_names, states_min_max, std::make_index_sequence{}); } /** @@ -101,56 +91,24 @@ template mode_matched_filter(const ImmModelT &imm_model, const SensModTPtr &sensor_model, double dt, - const std::vector &moment_based_preds, const Vec_z &z_meas) + static std::tuple 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 mode_matched_filter_impl(imm_model, sensor_model, dt, moment_based_preds, z_meas, std::make_index_sequence{}); - } - - /** - * @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 - * @param imm_model The IMM model. - * @param sensor_model The sensor model. - * @param dt double Time step - * @param x_est_prev Moment-based prediction - * @param z_meas Vec_z Measurement - * @return Tuple of updated state, predicted state, predicted measurement - */ - template - static std::tuple step_kalman_filter(const ImmModelT &imm_model, const SensModTPtr &sensor_model, double dt, - const Gauss_x &x_est_prev, const Vec_z &z_meas) - { - using DynModT = typename ImmModelT::template DynModT; - using DynModI = typename DynModT::DynModI; - using DynModIPtr = typename DynModI::SharedPtr; - - DynModIPtr dyn_model = imm_model.template get_model(); - - if constexpr (models::concepts::DynamicModelLTV && models::concepts::SensorModelLTV) { - using EKF = filter::EKF; - return EKF::step(dyn_model, sensor_model, dt, x_est_prev, z_meas); - } - else { - using UKF = filter::UKF; - return UKF::step(dyn_model, sensor_model, dt, x_est_prev, z_meas); - } + return step_kalman_filters(imm_model, sensor_model, dt, moment_based_preds, z_meas, std::make_index_sequence{}); } /** * @brief Update the mode probabilites based on how well the predictions matched the measurements. * Using (6.37) from step 3 and (6.38) from step 4 in (6.4.1) in the book - * @param imm_model The IMM model. - * @param dt double Time step + * @param transition_matrix The discrete time transition matrix for the Markov chain. (pi_mat_d from the imm model) * @param z_preds Mode-match filter outputs * @param z_meas Vec_z Measurement * @param prev_weigths Vec_n Weights * @return `Vec_n` Updated weights */ - static Vec_n update_probabilities(const ImmModelT &imm_model, double dt, const std::vector &z_preds, const Vec_z &z_meas, const Vec_n &prev_weights) + static Vec_n update_probabilities(const Mat_nn &transition_matrix, const GaussArr_z &z_preds, const Vec_z &z_meas, const Vec_n &prev_weights) { - Mat_nn pi_mat = imm_model.get_pi_mat_d(dt); - Vec_n weights_pred = pi_mat.transpose() * prev_weights; + Vec_n weights_pred = transition_matrix.transpose() * prev_weights; Vec_n z_probs; for (size_t i = 0; i < N_MODELS; i++) { @@ -165,45 +123,219 @@ 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 = {}) { - Mat_nn mixing_probs = calculate_mixing_probs(x_est_prev.weights(), dt); - std::vector moment_based_preds = mixing(x_est_prev.gaussians(), mixing_probs); - auto [x_est_upds, x_est_preds, z_est_preds] = mode_matched_filter(moment_based_preds, z_meas, dt); - Vec_n weights_upd = update_probabilities(dt, z_est_preds, z_meas, x_est_prev.weights()); + Mat_nn transition_matrix = imm_model.get_pi_mat_d(dt); - return {weights_upd, x_est_upds}; + 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); + 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 mode_matched_filter_impl(const ImmModelT &imm_model, const SensModTPtr &sensor_model, double dt, - const std::vector &moment_based_preds, const Vec_z &z_meas, - std::index_sequence) + 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) { - // Calculate mode-matched filter outputs and save them in a vector of tuples - std::vector> ekf_outs; - ((ekf_outs.push_back(step_kalman_filter(imm_model, sensor_model, dt, moment_based_preds.at(Is), z_meas))), ...); + // 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)), ...); - // Convert vector of tuples to tuple of vectors - std::vector x_est_upds; - std::vector x_est_preds; - std::vector z_est_preds; + // Convert tuple of tuples to tuple of tuples + GaussTuple_x x_est_upds; + GaussTuple_x x_est_preds; + GaussArr_z z_est_preds; - for (auto [x_est_upd, x_est_pred, z_est_pred] : ekf_outs) { - x_est_upds.push_back(x_est_upd); - x_est_preds.push_back(x_est_pred); - z_est_preds.push_back(z_est_pred); - } + ((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))), ...); return {x_est_upds, x_est_preds, z_est_preds}; } + + /** + * @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 + * @param imm_model The IMM model. + * @param sensor_model The sensor model. + * @param dt double Time step + * @param x_est_prev Moment-based prediction + * @param z_meas Vec_z Measurement + * @return Tuple of updated state, predicted state, predicted measurement + */ + 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) + { + 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; + 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; + 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) + { + return {mix_one_component(x_est_prevs, mixing_probs.col(model_indices), state_names, states_min_max)...}; + } + + /** 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 = {}) + { + 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(); + } + + /** 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)...}; + } + + /** Fit the size of the mixing_model in case it doesn't have the same dimensions or states as 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) + * @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 = {}) + { + if constexpr (target_model_index == mixing_model_index) { + return std::get(x_est_prevs); + } + + constexpr size_t N_DIM_target = ImmModelT::N_DIM_x(target_model_index); + constexpr size_t N_DIM_mixing = ImmModelT::N_DIM_x(mixing_model_index); + constexpr size_t N_DIM_min = std::min(N_DIM_target, N_DIM_mixing); + + using ST = models::StateType; + using Vec_x = Eigen::Vector; + 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); + + 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(); + + if (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 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(); + + 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); + 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); + continue; + } + double min = states_min_max.at(state_name).min; + double max = states_min_max.at(state_name).max; + Uniform initial_estimate{min, max}; + x(i) = initial_estimate.mean(); + P(i, i) = initial_estimate.cov(); + } + + return {x, P}; + } }; } // namespace vortex::filter \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/filters/ukf.hpp b/vortex-filtering/include/vortex_filtering/filters/ukf.hpp index f39b4a00..89a28ac7 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ukf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ukf.hpp @@ -15,60 +15,37 @@ #include #include #include +#include +#include #include namespace vortex { namespace filter { /** Unscented Kalman Filter - * @tparam DynModT Dynamic model type derived from `vortex::models::interface::DynamicModel` - * @tparam SensModT Sensor model type derived from `vortex::models::interface::SensorModel` + * @tparam n_dim_x State dimension + * @tparam n_dim_z Measurement dimension + * @tparam n_dim_u Input dimension + * @tparam n_dim_v Process noise dimension + * @tparam n_dim_w Measurement noise dimension * @tparam alpha Parameter for spread of sigma points (default 1.0) * @tparam beta Parameter for weighting of mean in covariance calculation (default 2.0) * @tparam kappa Parameter for adding additional spread to sigma points (default 0.0) */ -template class UKF { +template +class UKF_t { public: - static constexpr int N_DIM_x = DynModT::DynModI::N_DIM_x; - static constexpr int N_DIM_u = DynModT::DynModI::N_DIM_u; - static constexpr int N_DIM_z = SensModT::SensModI::N_DIM_z; - static constexpr int N_DIM_v = DynModT::DynModI::N_DIM_v; - static constexpr int N_DIM_w = SensModT::SensModI::N_DIM_w; - - using DynModI = models::interface::DynamicModel; - using SensModI = models::interface::SensorModel; - using DynModIPtr = DynModI::SharedPtr; - using SensModIPtr = SensModI::SharedPtr; - - using Vec_x = Eigen::Vector; - using Vec_u = Eigen::Vector; - using Vec_z = Eigen::Vector; - using Vec_v = Eigen::Vector; - using Vec_w = Eigen::Vector; - - using Mat_xx = Eigen::Matrix; - using Mat_xz = Eigen::Matrix; - using Mat_xv = Eigen::Matrix; - using Mat_xw = Eigen::Matrix; - - using Mat_zx = Eigen::Matrix; - using Mat_zz = Eigen::Matrix; - using Mat_zw = Eigen::Matrix; - - using Mat_vx = Eigen::Matrix; - using Mat_vv = Eigen::Matrix; - using Mat_vw = Eigen::Matrix; - - using Mat_wx = Eigen::Matrix; - using Mat_wv = Eigen::Matrix; - using Mat_ww = Eigen::Matrix; - - using Gauss_x = prob::MultiVarGauss; - using Gauss_z = prob::MultiVarGauss; + static constexpr int N_DIM_x = n_dim_x; + static constexpr int N_DIM_z = n_dim_z; + static constexpr int N_DIM_u = n_dim_u; + static constexpr int N_DIM_v = n_dim_v; + static constexpr int N_DIM_w = n_dim_w; static constexpr int N_DIM_a = N_DIM_x + N_DIM_v + N_DIM_w; // Augmented state dimension static constexpr size_t N_SIGMA_POINTS = 2 * N_DIM_a + 1; // Number of sigma points + using T = Types_xzuvw; + using Vec_a = Eigen::Vector; // Augmented state vector using Mat_aa = Eigen::Matrix; // Augmented state covariance matrix using Mat_x2ap1 = Eigen::Matrix; // Matrix for sigma points of x @@ -100,33 +77,38 @@ template && concepts::SensorModel) { - Mat_xx P = x_est.cov(); - Mat_vv Q = dyn_mod->Q_d(dt, x_est.mean()); - Mat_ww R = sens_mod->R(x_est.mean()); + typename T::Mat_xx P = x_est.cov(); + typename T::Mat_vv Q = dyn_mod.Q_d(dt, x_est.mean()); + typename T::Mat_ww R = sens_mod.R(x_est.mean()); // Make augmented covariance matrix - Mat_aa P_a; - // clang-format off - P_a << P , Mat_xv::Zero() , Mat_xw::Zero(), - Mat_vx::Zero(), Q , Mat_vw::Zero(), - Mat_wx::Zero(), Mat_wv::Zero() , R; - // clang-format on + Mat_aa P_a = Mat_aa::Zero(); + /* + P_a = | P 0 0 | + | 0 Q 0 | + | 0 0 R | + */ + P_a.template topLeftCorner() = P; + P_a.template block(N_DIM_x, N_DIM_x) = Q; + P_a.template bottomRightCorner() = R; + Mat_aa sqrt_P_a = P_a.llt().matrixLLT(); // Make augmented state vector - Vec_a x_a; - x_a << x_est.mean(), Vec_v::Zero(), Vec_w::Zero(); + Vec_a x_a = Vec_a::Zero(); + x_a.template head() = x_est.mean(); // Calculate sigma points using the symmetric sigma point set Mat_a2ap1 sigma_points; @@ -142,16 +124,17 @@ template ) { Eigen::Matrix sigma_x_pred; for (size_t i = 0; i < N_SIGMA_POINTS; i++) { - Vec_x x_i = sigma_points.template block(0, i); - Vec_v v_i = sigma_points.template block(N_DIM_x, i); - sigma_x_pred.col(i) = dyn_mod->f_d(dt, x_i, u, v_i); + typename T::Vec_x x_i = sigma_points.template block(0, i); + typename T::Vec_v v_i = sigma_points.template block(N_DIM_x, i); + sigma_x_pred.col(i) = dyn_mod.f_d(dt, x_i, u, v_i); } return sigma_x_pred; } @@ -161,13 +144,14 @@ template ) { Mat_z2ap1 sigma_z_pred; for (size_t i = 0; i < N_SIGMA_POINTS; i++) { - Vec_x x_i = sigma_points.template block(0, i); - Vec_w w_i = sigma_points.template block(N_DIM_x + N_DIM_v, i); - sigma_z_pred.col(i) = sens_mod->h(x_i, w_i); + typename T::Vec_x x_i = sigma_points.template block(0, i); + typename T::Vec_w w_i = sigma_points.template block(N_DIM_x + N_DIM_v, i); + sigma_z_pred.col(i) = sens_mod.h(x_i, w_i); } return sigma_z_pred; } @@ -201,11 +185,12 @@ template Predicted state estimate, predicted measurement estimate + * @param u T::Vec_u Control input (default 0) + * @return std::pair Predicted state estimate, predicted measurement estimate */ - static std::pair predict(const DynModIPtr &dyn_mod, const SensModIPtr &sens_mod, double dt, const Gauss_x &x_est_prev, - const Vec_u &u = Vec_u::Zero()) + 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::DynamicModel && concepts::SensorModel) { Mat_a2ap1 sigma_points = get_sigma_points(dyn_mod, sens_mod, dt, x_est_prev); @@ -214,8 +199,8 @@ template (sigma_x_pred); - Gauss_z z_pred = estimate_gaussian(sigma_z_pred); + typename T::Gauss_x x_pred = estimate_gaussian(sigma_x_pred); + typename T::Gauss_z z_pred = estimate_gaussian(sigma_z_pred); return {x_pred, z_pred}; } @@ -226,13 +211,15 @@ template && concepts::SensorModel) { // Generate sigma points from the predicted state estimate - Mat_a2ap1 sigma_points = get_sigma_points(dyn_mod, sens_mod, 0.0, x_est_pred); + Mat_a2ap1 sigma_points = get_sigma_points(dyn_mod, sens_mod, dt, x_est_pred); // Extract the sigma points for the state Mat_x2ap1 sigma_x_pred = sigma_points.template block(0, 0); @@ -241,19 +228,19 @@ template Updated state estimate, predicted state estimate, predicted measurement estimate + * @param u T::Vec_u Control input + * @return std::tuple Updated state estimate, predicted state estimate, predicted measurement estimate */ - static std::tuple step(const DynModIPtr &dyn_mod, const SensModIPtr &sens_mod, double dt, const Gauss_x &x_est_prev, - const Vec_z &z_meas, const Vec_u &u) + static std::tuple + 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::DynamicModel && concepts::SensorModel) { Mat_a2ap1 sigma_points = get_sigma_points(dyn_mod, sens_mod, dt, x_est_prev); @@ -277,27 +265,38 @@ template (sigma_x_pred); - Gauss_z z_pred = estimate_gaussian(sigma_z_pred); + typename T::Gauss_x x_pred = estimate_gaussian(sigma_x_pred); + typename T::Gauss_z z_pred = estimate_gaussian(sigma_z_pred); // Calculate cross-covariance - Mat_xz P_xz = Mat_xz::Zero(); + typename T::Mat_xz P_xz = T::Mat_xz::Zero(); for (size_t i = 0; i < N_SIGMA_POINTS; i++) { P_xz += W_c.at(i) * (sigma_x_pred.col(i) - x_pred.mean()) * (sigma_z_pred.col(i) - z_pred.mean()).transpose(); } // Calculate Kalman gain - Mat_zz P_zz = z_pred.cov(); - Mat_xz K = P_xz * P_zz.llt().solve(Mat_zz::Identity()); + typename T::Mat_zz P_zz = z_pred.cov(); + typename T::Mat_xz K = P_xz * P_zz.llt().solve(T::Mat_zz::Identity()); // Update state estimate - Vec_x x_upd_mean = x_pred.mean() + K * (z_meas - z_pred.mean()); - Mat_xx x_upd_cov = x_pred.cov() - K * P_zz * K.transpose(); - Gauss_x x_est_upd = {x_upd_mean, x_upd_cov}; + typename T::Vec_x x_upd_mean = x_pred.mean() + K * (z_meas - z_pred.mean()); + typename T::Mat_xx x_upd_cov = x_pred.cov() - K * P_zz * K.transpose(); + typename T::Gauss_x x_est_upd = {x_upd_mean, x_upd_cov}; return {x_est_upd, x_pred, z_pred}; } }; +/** Unscented Kalman Filter + * @tparam DynModT Dynamic model type derived from `vortex::models::interface::DynamicModel` + * @tparam SensModT Sensor model type derived from `vortex::models::interface::SensorModel` + * @tparam alpha Parameter for spread of sigma points (default 1.0) + * @tparam beta Parameter for weighting of mean in covariance calculation (default 2.0) + * @tparam kappa Parameter for adding additional spread to sigma points (default 0.0) + */ +template +using UKF = UKF_t; + } // namespace filter } // namespace vortex \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/models/README.md b/vortex-filtering/include/vortex_filtering/models/README.md index ca18f701..5ce8ecc9 100644 --- a/vortex-filtering/include/vortex_filtering/models/README.md +++ b/vortex-filtering/include/vortex_filtering/models/README.md @@ -5,15 +5,24 @@ They define the dynamics of the system and the sensor models used in the project All classes and functions are under the namespace `vortex::models`. The interfaces that the different models are derived from are definged under the namespace `vortex::models::interface`. -## Dynamice Models +## Overview +- [`dynamic_model_interfaces.hpp`](dynamic_model_interfaces.hpp) contains the interfaces for the dynamic models. +- [`sensor_model_interfaces.hpp`](sensor_model_interfaces.hpp) contains the interfaces for the sensor models. +- [`dynamic_models.hpp`](dynamic_models.hpp) contains some movement models that are commonly used in target tracking. +- [`sensor_models.hpp`](sensor_models.hpp) contains some sensor models that are commonly used. +- [`imm_model.hpp`](imm_model.hpp) contains the IMM model. + + + +## Dynamic Models Models for describing the dynamics of the system. ### Dynamic Model Interfaces - +The dynamic model interfaces are virtual template classes that makes it convenient to define your own dynamic models. #### DynamicModel -Dynamic model interface for other classes to derive from. The [UKF class](../filters/README.md#UKF) works on models derived from this class. +Dynamic model interface for other classes to derive from. The [UKF](../filters/README.md#UKF) works on models derived from this class. ##### Key Features - Static Constants for Dimensions: The base class defines static constants (N_DIM_x, N_DIM_u, N_DIM_v) for dimensions, allowing derived classes to reference these dimensions. @@ -22,50 +31,109 @@ Dynamic model interface for other classes to derive from. The [UKF class](../fil - Sampling Methods: Provides methods for sampling from the discrete time dynamics. #### DynamicModelLTV -Dynamic model interface for other classes to derive from. The [EKF class](../filters/README.md#ekf) works on models derived from this class. +Dynamic model interface for other classes to derive from. The [EKF](../filters/README.md#ekf) (and UKF) works on models derived from this class. This interface inherits from the `DynamicModel` interface and defines the dynamics of the system as a linear time varying system. The virtual method `f_d` from the `DynamicModel` interface is implemented as a linear time varying system on the form + $$ f_d(dt, x_k, u_k, v_k) = x_{k+1} = A_d(dt, x_k) x_k + B_d(dt, x_k) u_k + G_d(dt, x_k)v_k $$ + where $dt$ is the time step, $x_k$ is the state at time $k$, $u_k$ is the input at time $k$ and $v_k$ is the process noise at time $k$. The matrices $A_d$, $B_d$ and $G_d$ are defined as virtual methods and must be implemented by the derived class. ##### Usage -In order to define a *new* dynamic model, the user must first create a new class that inherits from the `DynamicModelLTV` interface. The user must then override the methods `A_d`, `B_d`, `G_d` and `Q_d` for the discrete time dynamics. In addition, the typedef `DynModI` has to be present in the derived class and should point to the `DynamicModelLTV` class like this: +In order to define a *new* dynamic model, the user must first create a new class that inherits from the `DynamicModelLTV` interface. The user must then override the methods `A_d`, `B_d`, `G_d` and `Q_d` for the discrete time dynamics. ```cpp +#include + class MyDynamicModel : public interface::DynamicModelLTV { - // ... - using DynModI = interface::DynamicModelLTV; - // ... - using typename DynModI::Gauss_x; - using typename DynModI::Vec_x; +public: + // Get all types used in the models + using T = vortex::Types_xuv; + + + // Define the matrices A_d, (B_d), (G_d) and Q_d + T::Mat_XX A_d(double dt, const T::Vec& x) const override { + // Implement the A_d matrix + } + + T::Mat_XU B_d(double dt, const T::Vec& x) const override { + // Implement the B_d matrix + } + + // ... and so on }; ``` -The purpose of this typedef is to allow convenient access to types like `Gauss_x`, `Vec_x`, etc., without needing to redefine them in the derived class. It also allows other classes to access the types defined in the `DynamicModelLTV` class. For example, the `EKF` class uses this typedef to access the sizes of the state, input and noise vectors inherent to the dynamic models. - #### DynamicModelCTLTV This class implements the `DynamicModelLTV` interface and defines the dynamics of the system as a continuous-time linear time-varying system. The matrices `A_c`, `B_c` `Q_d` and `G_c` are defined as virtual methods and must be implemented by the derived class. The matrices `A_d`, `B_d`, `Q_d` and `G_d` are then generated using [exact discretization](https://en.wikipedia.org/wiki/Discretization). ### IMM Model -This class can hold multiple `DynamicModel` objects and defines functions to calculate the probability of switching between the models. +__Interacting Multiple Models__ +This class can store multiple `DynamicModel` objects and defines functions to calculate the probability of switching between the models. #### Usage -To instantiate a **Interacting Multiple Models (IMM) object**, you must provide three key parameters: +To instantiate a **Interacting Multiple Models (IMM) object**, you must provide four parameters: + +1. **Hold Times Vector**: This vector should contain the expected time durations that each model should be held before a switch occurs. The length of this vector must equal the number of models `N`. _When the switch occurs_ is modeled by an exponential distribution with parameters given in the hold times vector. + +2. **Switching Probabilities Matrix**: This is an `N x N` matrix where each element at index `(i, j)` represents the probability of transitioning from model `i` to model `j`. Each probability lies between 0 and 1, and the sum of probabilities in each row equals 1. These probabilities define the likelihood of transitioning to a particular model given that a switch occurs. The diagonal should be zero as this represents the probability of _switching to itself_, which doesn't make sense. -1. **Hold Times Vector**: This vector should contain the expected time durations between switches for each model. The length of this vector must equal the number of models, denoted as `N`. +3. **Dynamic Model Objects**: A list of `DynamicModel` objects, one for each model in use. -2. **Switching Probabilities Matrix**: This is an `N x N` matrix where each element at index `(i, j)` represents the probability of transitioning from model `i` to model `j`. It's crucial that each probability lies between 0 and 1, and that the sum of probabilities in each row equals 1. These probabilities define the likelihood of transitioning to a particular model given that a switch occurs, but they do not represent the overall probability of a switch happening. +4. **State Names**: An array of state names (enums) for each state in each model. This is used to compare the states of the different models with each other in the [IMM filter](../filters/README.md#imm-filter) for proper mixing of the states. + +#### Example +```cpp +// Create aliases for the dynamic models (optional) +using CP = vortex::models::ConstantPosition; +using CV = vortex::models::ConstantVelocity; +using CT = vortex::models::CoordinatedTurn; + +// Create alias for the IMM model (optional, but probably a good idea) +using IMM = vortex::models::IMMModel; + +// Specify holding times and switching probabilities +Eigen::Vector3d hold_times{1.0, 2.0, 3.0}; +Eigen::Matrix3d switch_probs{ + {0.0, 0.5, 0.5}, + {0.5, 0.0, 0.5}, + {0.5, 0.5, 0.0} +}; -3. **DynamicModel Objects**: A set of `DynamicModel` objects, one for each model in use. +double std_pos = 0.1, std_vel = 0.1, std_turn = 0.1; + +// Specify the state names of the models +using ST = vortex::models::StateType; +std::array cp_names{ST::pos, ST::pos}; +std::array cv_names{ST::pos, ST::pos, ST::vel, ST::vel}; +std::array ct_names{ST::pos, ST::pos, ST::vel, ST::vel, ST::turn}; + +/* Note: for the models in this example, you can use the already defined state names instead: + auto cp_names = CP::StateNames; + auto cv_names = CV::StateNames; + auto ct_names = CT::StateNames; +But for custom models, you will have to define the state names yourself. +*/ + +// initialize IMM with the hold times, switching probabilities, dynamic models and state names +IMM imm_model(hold_times, switch_probs, + {CP(std_pos), cp_names}, + {CV(std_vel), cv_names}, + {CT(std_vel, std_turn), ct_names}); + +// Enjoy your very own IMM model! :) +``` +#### Theory It's important to note that the actual probability of switching from one model to another is determined through the `hold_times` vector. By treating the system as a **Continuous Time Markov Chain (CTMC)**, as detailed on [Wikipedia](https://en.wikipedia.org/wiki/Continuous-time_Markov_chain), the model calculates the switching probabilities based on the specified hold times and the switching probabilities matrix. -### Predefined Model Implementations +### Dynamic Models +`dynamic_models.hpp` This file contains some movement models that are commonly used in an IMM. - `ConstantVelocity`: Has states for position and velocity. The template parameter `n_spatial_dims` specifies the number of spatial dimensions. So if the model is used in 2D, `n_spatial_dims` should be set to 2 and the model will have 4 states. `x`, `y`, `v_x` and `v_y`. - `ConstantAcceleration`: Has states for position, velocity and acceleration. The template parameter `n_spatial_dims` specifies the number of spatial dimensions. So if the model is used in 2D, `n_spatial_dims` should be set to 2 and the model will have 6 states. `x`, `y`, `v_x`, `v_y`, `a_x` and `a_y`. 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 fdcb1502..c7f57c84 100644 --- a/vortex-filtering/include/vortex_filtering/models/dynamic_model_interfaces.hpp +++ b/vortex-filtering/include/vortex_filtering/models/dynamic_model_interfaces.hpp @@ -12,7 +12,9 @@ #include #include #include +#include #include +#include namespace vortex::models { namespace interface { @@ -29,136 +31,44 @@ namespace interface { */ template class DynamicModel { public: - // Declare all sizes and types so that children of this class can reference them static constexpr int N_DIM_x = (int)n_dim_x; static constexpr int N_DIM_u = (int)n_dim_u; static constexpr int N_DIM_v = (int)n_dim_v; - using DynModI = DynamicModel; - - using Vec_x = Eigen::Vector; - using Vec_u = Eigen::Vector; - using Vec_v = Eigen::Vector; - - using Mat_xx = Eigen::Matrix; - using Mat_xu = Eigen::Matrix; - using Mat_xv = Eigen::Matrix; - - using Mat_ux = Eigen::Matrix; - using Mat_uu = Eigen::Matrix; - using Mat_uv = Eigen::Matrix; - - using Mat_vx = Eigen::Matrix; - using Mat_vv = Eigen::Matrix; - using Mat_vu = Eigen::Matrix; - - using Gauss_x = prob::MultiVarGauss; - using Gauss_v = prob::MultiVarGauss; - - using SharedPtr = std::shared_ptr; + using T = Types_xuv; DynamicModel() = default; virtual ~DynamicModel() = default; /** Discrete time dynamics * @param dt Time step - * @param x Vec_x State - * @param u Vec_u Input - * @param v Vec_v Process noise - * @return Vec_x Next state + * @param x T::Vec_x State + * @param u T::Vec_u Input + * @param v T::Vec_v Process noise + * @return T::Vec_x Next state */ - virtual Vec_x f_d(double dt, const Vec_x &x, const Vec_u &u, const Vec_v &v) const = 0; + virtual T::Vec_x f_d(double dt, const T::Vec_x &x, const T::Vec_u &u, const T::Vec_v &v) const = 0; /** Discrete time process noise covariance matrix * @param dt Time step - * @param x Vec_x State + * @param x T::Vec_x State */ - virtual Mat_vv Q_d(double dt, const Vec_x &x) const = 0; + virtual T::Mat_vv Q_d(double dt, const T::Vec_x &x) const = 0; /** Sample from the discrete time dynamics * @param dt Time step - * @param x Vec_x State - * @param u Vec_u Input + * @param x T::Vec_x State + * @param u T::Vec_u Input * @param gen Random number generator (For deterministic behaviour) - * @return Vec_x Next state + * @return T::Vec_x Next state */ - Vec_x sample_f_d(double dt, const Vec_x &x, const Vec_u &u, std::mt19937 &gen) const + T::Vec_x sample_f_d(double dt, const T::Vec_x &x, const T::Vec_u &u, std::mt19937 &gen) const { - Gauss_v v = {Vec_v::Zero(), Q_d(dt, x)}; + typename T::Gauss_v v = {T::Vec_v::Zero(), Q_d(dt, x)}; return f_d(dt, x, u, v.sample(gen)); } }; -/** - * @brief Continuous Time Dynamic Model Interface. x_dot = f_c(x, u, v) - * @tparam n_dim_x State dimension - * @tparam n_dim_u Input dimension (Default: n_dim_x) - * @tparam n_dim_v Process noise dimension (Default: n_dim_x) - * @note To derive from this class, you must override the following virtual functions: - * @note - f_c - * @note - f_d (optional. Does a RK4 integration of f_c by default) - * @note - Q_d - */ -template class DynamicModelCT : public DynamicModel { -public: - using DynModI = DynamicModel; - static constexpr int N_DIM_x = DynModI::N_DIM_x; - static constexpr int N_DIM_u = DynModI::N_DIM_u; - static constexpr int N_DIM_v = DynModI::N_DIM_v; - - using Vec_x = typename DynModI::Vec_x; - using Vec_u = typename DynModI::Vec_u; - using Vec_v = typename DynModI::Vec_v; - - using Mat_xx = typename DynModI::Mat_xx; - using Mat_xu = typename DynModI::Mat_xu; - using Mat_xv = typename DynModI::Mat_xv; - - using Mat_uu = typename DynModI::Mat_uu; - using Mat_vv = typename DynModI::Mat_vv; - - using Dyn_mod_func = std::function; - - /** Continuous Time Dynamic Model Interface - * @tparam n_dim_x State dimension - * @tparam n_dim_u Input dimension (Default: n_dim_x) - * @tparam n_dim_v Process noise dimension (Default: n_dim_x) - */ - DynamicModelCT() : DynModI() {} - virtual ~DynamicModelCT() = default; - - /** Continuous time dynamics - * @param x Vec_x State - * @param u Vec_u Input - * @param v Vec_v Process noise - * @return Vec_x State_dot - */ - virtual Vec_x f_c(const Vec_x &x, const Vec_u &u, const Vec_v &v) const = 0; - - /** Discrete time process noise covariance matrix - * @param dt Time step - * @param x Vec_x State - */ - virtual Mat_vv Q_d(double dt, const Vec_x &x) const override = 0; - -protected: - // Discrete time stuff - - /** Discrete time dynamics. Uses RK4 integration. Assumes constant input and process noise during the time step. - * Overriding DynamicModel::f_d - * @param dt Time step - * @param x Vec_x State - * @param u Vec_u Input - * @param v Vec_v Process noise - * @return Vec_x Next state - */ - virtual Vec_x f_d(double dt, const Vec_x &x, const Vec_u &u, const Vec_v &v) const override - { - Dyn_mod_func f_c = [this, &u, &v](double, const Vec_x &x) { return this->f_c(x, u, v); }; - return vortex::integrator::RK4::integrate(f_c, dt, x); - } -}; - /** Linear Time Variant Dynamic Model Interface. [x_k+1 = f_d = A_k*x_k + B_k*u_k + G_k*v_k] * @tparam n_dim_x State dimension * @tparam n_dim_u Input dimension (Default: n_dim_x) @@ -172,26 +82,11 @@ template cl */ template class DynamicModelLTV : public DynamicModel { public: - using DynModI = DynamicModel; static constexpr int N_DIM_x = n_dim_x; static constexpr int N_DIM_u = n_dim_u; static constexpr int N_DIM_v = n_dim_v; - using Vec_x = typename DynModI::Vec_x; - using Vec_u = typename DynModI::Vec_u; - using Vec_v = typename DynModI::Vec_v; - - using Mat_xx = typename DynModI::Mat_xx; - using Mat_xu = typename DynModI::Mat_xu; - using Mat_xv = typename DynModI::Mat_xv; - - using Mat_uu = typename DynModI::Mat_uu; - using Mat_vv = typename DynModI::Mat_vv; - - using Gauss_x = prob::MultiVarGauss; - using Gauss_v = prob::MultiVarGauss; - - using SharedPtr = std::shared_ptr; + using T = Types_xuv; /** Linear Time Variant Dynamic Model Interface. [x_k+1 = f_d = A_k*x_k + B_k*u_k + G_k*v_k] * @tparam n_dim_x State dimension @@ -204,7 +99,10 @@ template cl * @note - Q_d * @note - G_d (optional if n_dim_x == n_dim_v) */ - DynamicModelLTV() : DynModI() {} + DynamicModelLTV() + : DynamicModel() + { + } virtual ~DynamicModelLTV() = default; /** Discrete time dynamics @@ -214,87 +112,87 @@ template cl * @param v Vec_v Process noise * @return Vec_x */ - virtual Vec_x f_d(double dt, const Vec_x &x, const Vec_u &u = Vec_u::Zero(), const Vec_v &v = Vec_v::Zero()) const override + virtual T::Vec_x f_d(double dt, const T::Vec_x &x, const T::Vec_u &u = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const override { - Mat_xx A_d = this->A_d(dt, x); - Mat_xu B_d = this->B_d(dt, x); - Mat_xv G_d = this->G_d(dt, x); + typename T::Mat_xx A_d = this->A_d(dt, x); + typename T::Mat_xu B_d = this->B_d(dt, x); + typename T::Mat_xv G_d = this->G_d(dt, x); return A_d * x + B_d * u + G_d * v; } /** System matrix (Jacobian of discrete time dynamics with respect to state) * @param dt Time step - * @param x Vec_x + * @param x T::Vec_x * @return State_jac */ - virtual Mat_xx A_d(double dt, const Vec_x &x) const = 0; + virtual T::Mat_xx A_d(double dt, const T::Vec_x &x) const = 0; /** Input matrix * @param dt Time step - * @param x Vec_x + * @param x T::Vec_x * @return Input_jac */ - virtual Mat_xu B_d(double dt, const Vec_x &x) const + virtual T::Mat_xu B_d(double dt, const T::Vec_x &x) const { (void)dt; // unused (void)x; // unused - return Mat_xu::Zero(); + return T::Mat_xu::Zero(); } /** Discrete time process noise covariance matrix * @param dt Time step - * @param x Vec_x State + * @param x T::Vec_x State */ - virtual Mat_vv Q_d(double dt, const Vec_x &x) const override = 0; + virtual T::Mat_vv Q_d(double dt, const T::Vec_x &x) const override = 0; /** Discrete time process noise transition matrix * @param dt Time step - * @param x Vec_x State + * @param x T::Vec_x State */ - virtual Mat_xv G_d(double, const Vec_x &) const + virtual T::Mat_xv G_d(double, const T::Vec_x &) const { - if (N_DIM_x != N_DIM_v) { + if (this->N_DIM_x != this->N_DIM_v) { throw std::runtime_error("G_d not implemented"); } - return Mat_xv::Identity(); + return T::Mat_xv::Identity(); } /** Get the predicted state distribution given a state estimate * @param dt Time step - * @param x_est Vec_x estimate - * @return Vec_x + * @param x_est T::Vec_x estimate + * @return T::Vec_x */ - Gauss_x pred_from_est(double dt, const Gauss_x &x_est, const Vec_u &u = Vec_u::Zero()) const + T::Gauss_x pred_from_est(double dt, const T::Gauss_x &x_est, const T::Vec_u &u = T::Vec_u::Zero()) const { - Mat_xx P = x_est.cov(); - Mat_xx A_d = this->A_d(dt, x_est.mean()); - Mat_xx GQGT_d = this->GQGT_d(dt, x_est.mean()); + 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()); - Gauss_x x_est_pred(f_d(dt, x_est.mean(), u), A_d * P * A_d.transpose() + GQGT_d); + 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; } /** Get the predicted state distribution given a state * @param dt Time step - * @param x Vec_x - * @return Vec_x + * @param x T::Vec_x + * @return T::Vec_x */ - Gauss_x pred_from_state(double dt, const Vec_x &x, const Vec_u &u = Vec_u::Zero()) const + T::Gauss_x pred_from_state(double dt, const T::Vec_x &x, const T::Vec_u &u = T::Vec_u::Zero()) const { - 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; } protected: /** Process noise covariance matrix. For expressing the process noise in the state space. * @param dt Time step - * @param x Vec_x State + * @param x T::Vec_x State * @return Process_noise_jac */ - virtual Mat_xx GQGT_d(double dt, const Vec_x &x) const + virtual T::Mat_xx GQGT_d(double dt, const T::Vec_x &x) const { - Mat_vv Q_d = this->Q_d(dt, x); - Mat_xv G_d = this->G_d(dt, x); + typename T::Mat_vv Q_d = this->Q_d(dt, x); + typename T::Mat_xv G_d = this->G_d(dt, x); return G_d * Q_d * G_d.transpose(); } @@ -314,171 +212,149 @@ template cl */ template class DynamicModelCTLTV : public DynamicModelLTV { public: - using DynModI = DynamicModelLTV; - static constexpr int N_DIM_x = DynModI::N_DIM_x; - static constexpr int N_DIM_u = DynModI::N_DIM_u; - static constexpr int N_DIM_v = DynModI::N_DIM_v; - - using Vec_x = typename DynModI::Vec_x; - using Vec_u = typename DynModI::Vec_u; - using Vec_v = typename DynModI::Vec_v; - - using Mat_xx = typename DynModI::Mat_xx; - using Mat_xu = typename DynModI::Mat_xu; - using Mat_xv = typename DynModI::Mat_xv; - - using Mat_ux = typename DynModI::Mat_ux; - using Mat_uu = typename DynModI::Mat_uu; - using Mat_vv = typename DynModI::Mat_vv; - using Mat_vx = typename DynModI::Mat_vx; + static constexpr int N_DIM_x = n_dim_x; + static constexpr int N_DIM_u = n_dim_u; + static constexpr int N_DIM_v = n_dim_v; - using SharedPtr = std::shared_ptr; + using T = Types_xuv; /** Continuous Time Linear Time Varying Dynamic Model Interface. [x_dot = A_c*x + B_c*u + G_c*v] * @tparam n_dim_x State dimension * @tparam n_dim_u Input dimension (Default: n_dim_x) * @tparam n_dim_v Process noise dimension (Default: n_dim_x) */ - DynamicModelCTLTV() : DynamicModelLTV() {} + DynamicModelCTLTV() + : DynamicModelLTV() + { + } virtual ~DynamicModelCTLTV() = default; /** Continuous time dynamics - * @param x Vec_x State - * @param u Vec_u Input - * @param v Vec_v Process noise - * @return Vec_x State_dot + * @param x T::Vec_x State + * @param u T::Vec_u Input + * @param v T::Vec_v Process noise + * @return T::Vec_x State_dot */ - Vec_x f_c(const Vec_x &x, const Vec_u &u = Vec_u::Zero(), const Vec_v &v = Vec_v::Zero()) const + T::Vec_x f_c(const T::Vec_x &x, const T::Vec_u &u = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const { - Mat_xx A_c = this->A_c(x); - Mat_xu B_c = this->B_c(x); - Mat_xv G_c = this->G_c(x); + typename T::Mat_xx A_c = this->A_c(x); + typename T::Mat_xu B_c = this->B_c(x); + typename T::Mat_xv G_c = this->G_c(x); return A_c * x + B_c * u + G_c * v; } /** System matrix (Jacobian of continuous time dynamics with respect to state) - * @param x Vec_x State + * @param x T::Vec_x State * @return State_jac */ - virtual Mat_xx A_c(const Vec_x &x) const = 0; + virtual T::Mat_xx A_c(const T::Vec_x &x) const = 0; /** Input matrix - * @param x Vec_x State + * @param x T::Vec_x State * @return Input_jac */ - virtual Mat_xu B_c(const Vec_x &x) const + virtual T::Mat_xu B_c(const T::Vec_x &x) const { (void)x; // unused - return Mat_xu::Zero(); + return T::Mat_xu::Zero(); } /** Process noise transition matrix. For expressing the process noise in the state space. - * @param x Vec_x State + * @param x T::Vec_x State * @return Process_noise_jac */ - virtual Mat_xv G_c(const Vec_x &x) const + virtual T::Mat_xv G_c(const T::Vec_x &x) const { if (N_DIM_x != N_DIM_v) { throw std::runtime_error("G_c not implemented"); } (void)x; // unused - return Mat_xv::Identity(); + return T::Mat_xv::Identity(); } /** Process noise covariance matrix. This has the same dimension as the process noise. - * @param x Vec_x State + * @param x T::Vec_x State */ - virtual Mat_vv Q_c(const Vec_x &x) const = 0; + virtual T::Mat_vv Q_c(const T::Vec_x &x) const = 0; /** System dynamics (Jacobian of discrete time dynamics w.r.t. state). Using exact discretization. * @param dt Time step - * @param x Vec_x + * @param x T::Vec_x * @return State_jac */ - Mat_xx A_d(double dt, const Vec_x &x) const override { return (A_c(x) * dt).exp(); } + T::Mat_xx A_d(double dt, const T::Vec_x &x) const override { return (A_c(x) * dt).exp(); } /** Input matrix (Jacobian of discrete time dynamics w.r.t. input). Using exact discretization. * @param dt Time step - * @param x Vec_x + * @param x T::Vec_x * @return Input_jac */ - Mat_xu B_d(double dt, const Vec_x &x) const override + T::Mat_xu B_d(double dt, const T::Vec_x &x) const override { Eigen::Matrix van_loan; - van_loan << A_c(x), B_c(x), Mat_ux::Zero(), Mat_uu::Zero(); + van_loan << A_c(x), B_c(x), T::Mat_ux::Zero(), T::Mat_uu::Zero(); van_loan *= dt; van_loan = van_loan.exp(); - // Mat_xx A_d = van_loan.template block(0, 0); - Mat_xu B_d = van_loan.template block(0, N_DIM_x); + // T::Mat_xx A_d = van_loan.template block(0, 0); + typename T::Mat_xu B_d = van_loan.template block(0, N_DIM_x); return B_d; } - Mat_xv G_d(double dt, const Vec_x &x) const override + T::Mat_xv G_d(double dt, const T::Vec_x &x) const override { Eigen::Matrix van_loan; - van_loan << A_c(x), G_c(x), Mat_vx::Zero(), Mat_vv::Zero(); + van_loan.template topLeftCorner() = A_c(x); + van_loan.template topRightCorner() = G_c(x); + van_loan.template bottomLeftCorner() = T::Mat_vx::Zero(); + van_loan.template bottomRightCorner() = T::Mat_vv::Zero(); + van_loan *= dt; van_loan = van_loan.exp(); - // Mat_xx A_d = van_loan.template block(0, 0); - Mat_xv G_d = van_loan.template block(0, N_DIM_x); + // T::Mat_xx A_d = van_loan.template block(0, 0); + typename T::Mat_xv G_d = van_loan.template block(0, N_DIM_x); return G_d; } /** Discrete time process noise covariance matrix. This is super scuffed, but it works... (As long as G_d^T*G_d is invertible) * Overriding DynamicModel::Q_d * @param dt Time step - * @param x Vec_x + * @param x T::Vec_x * @return Matrix Process noise covariance */ - Mat_vv Q_d(double dt, const Vec_x &x) const override + T::Mat_vv Q_d(double dt, const T::Vec_x &x) const override { - Mat_xx GQGT_d = this->GQGT_d(dt, x); - Mat_xv G_d = this->G_d(dt, x); + typename T::Mat_xx GQGT_d = this->GQGT_d(dt, x); + typename T::Mat_xv G_d = this->G_d(dt, x); // psuedo inverse of G_d - Mat_vx G_d_pinv = G_d.completeOrthogonalDecomposition().pseudoInverse(); + typename T::Mat_vx G_d_pinv = G_d.completeOrthogonalDecomposition().pseudoInverse(); return G_d_pinv * GQGT_d * G_d_pinv.transpose(); - return Mat_vv::Identity(); } - Mat_xx GQGT_d(double dt, const Vec_x &x) const override + T::Mat_xx GQGT_d(double dt, const T::Vec_x &x) const override { - Mat_xx A_c = this->A_c(x); - Mat_vv Q_c = this->Q_c(x); - Mat_xv G_c = this->G_c(x); + typename T::Mat_xx A_c = this->A_c(x); + typename T::Mat_vv Q_c = this->Q_c(x); + typename T::Mat_xv G_c = this->G_c(x); Eigen::Matrix van_loan; - van_loan << -A_c, G_c * Q_c * G_c.transpose(), Mat_xx::Zero(), A_c.transpose(); + van_loan.template topLeftCorner() = -A_c; + van_loan.template topRightCorner() = G_c * Q_c * G_c.transpose(); + van_loan.template bottomLeftCorner() = T::Mat_xx::Zero(); + van_loan.template bottomRightCorner() = A_c.transpose(); + van_loan *= dt; - van_loan = van_loan.exp(); - Mat_xx A_d = van_loan.template block(N_DIM_x, N_DIM_x).transpose(); - Mat_xx A_d_inv_GQGT_d = van_loan.template block(0, N_DIM_x); // A_d^(-1) * G * Q * G^T - Mat_xx GQGT_d = A_d * A_d_inv_GQGT_d; // G * Q * G^T + van_loan = van_loan.exp(); + + typename T::Mat_xx A_d = van_loan.template block(N_DIM_x, N_DIM_x).transpose(); + typename T::Mat_xx A_d_inv_GQGT_d = van_loan.template block(0, N_DIM_x); // A_d^(-1) * G * Q * G^T + typename T::Mat_xx GQGT_d = A_d * A_d_inv_GQGT_d; // G * Q * G^T return GQGT_d; } }; } // namespace interface -namespace concepts { - -template -concept DynamicModel = requires { - // Has member type called DynModI - typename T::DynModI; - // Derived from DynamicModel - std::is_base_of, T>::value; -}; - -template -concept DynamicModelLTV = requires { - // Has member type called DynModI - typename T::DynModI; - // Derived from DynamicModelLTV - std::is_base_of, T>::value; -}; - -} // namespace concepts } // namespace vortex::models \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp b/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp index ce33116c..edbfaa6f 100644 --- a/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp +++ b/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp @@ -1,80 +1,152 @@ #pragma once #include +#include +#include namespace vortex { namespace models { -constexpr int X = 1; // For when a template parameter is required but not used. +constexpr int UNUSED = 1; // For when a template parameter is required but not used. /** Identity Dynamic Model * @tparam n_dim_x Number of dimensions in state vector */ -template class IdentityDynamicModel : public interface::DynamicModelLTV { +template class IdentityDynamicModel : public interface::DynamicModelLTV { + using Parent = interface::DynamicModelLTV; + public: - using DynModI = interface::DynamicModelLTV; - using Vec_x = typename DynModI::Vec_x; - using Mat_xx = typename DynModI::Mat_xx; - using Mat_xv = typename DynModI::Mat_xv; - using Mat_vv = typename DynModI::Mat_vv; + static constexpr int N_DIM_x = Parent::N_DIM_x; + 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; /** Identity Dynamic Model * @param std Standard deviation of process noise */ - IdentityDynamicModel(double std) : Q_(Mat_xx::Identity() * std * std) {} + IdentityDynamicModel(double std) + : Q_(T::Mat_xx::Identity() * std * std) + { + } /** Identity Dynamic Model * @param Q Process noise covariance */ - IdentityDynamicModel(Mat_vv Q) : Q_(Q) {} + IdentityDynamicModel(T::Mat_vv Q) + : Q_(Q) + { + } - Mat_xx A_d(double dt, const Vec_x &) const override { return Mat_xx::Identity() * dt; } - Mat_vv Q_d(double, const Vec_x &) const override { return Q_; } + T::Mat_xx A_d(double dt, const T::Vec_x /*x*/ &) const override { return T::Mat_xx::Identity() * dt; } + T::Mat_vv Q_d(double /*dt*/, const T::Vec_x /*x*/ &) const override { return Q_; } private: - Mat_vv Q_; + T::Mat_vv Q_; }; /** (Nearly) Constant Position Model * State x = [pos], where pos is a `n_spatial_dim`-dimensional vector * @tparam n_spatial_dim Number of spatial dimensions */ -template using ConstantPosition = IdentityDynamicModel; +class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> { + using Parent = interface::DynamicModelLTV<2, UNUSED, 2>; + +public: + static constexpr int N_DIM_x = Parent::N_DIM_x; + 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 ST = StateType; + static constexpr std::array StateNames{ST::position, ST::position}; + + /** Constant Position Model in 2D + * x = [x, y] + * @param std_pos Standard deviation of position + */ + ConstantPosition(double std_pos) + : std_pos_(std_pos) + { + } + + /** Get the Jacobian of the continuous state transition model with respect to the state. + * @param dt Time step + * @param x State (unused) + * @return T::Mat_xx + * @note Overriding DynamicModelLTV::A_d + */ + T::Mat_xx A_d(double /*dt*/, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity(); } + + /** Get the Jacobian of the continuous state transition model with respect to the process noise. + * @param dt Time step + * @param x State (unused) + * @return T::Mat_xv + * @note Overriding DynamicModelLTV::G_d + */ + T::Mat_xv G_d(double dt, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override + { + T::Mat_xx I = T::Mat_xx::Identity(); + return 0.5 * dt * I; + } -// TODO: Update these models to use discrete time instead of continuous time. + /** Get the continuous time process noise covariance matrix. + * @param dt Time step (unused) + * @param x State (unused) + * @return T::Mat_xx Process noise covariance + * @note Overriding DynamicModelLTV::Q_d + */ + T::Mat_vv Q_d(double /*dt*/ = 0.0, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { return T::Mat_vv::Identity() * std_pos_ * std_pos_; } + +private: + double std_pos_; +}; /** (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 */ -template class ConstantVelocity : public interface::DynamicModelLTV<2 * n_spatial_dim, X, n_spatial_dim> { +class ConstantVelocity : public interface::DynamicModelLTV<4, UNUSED, 2> { + using Parent = interface::DynamicModelLTV<4, UNUSED, 2>; + public: - using DynModI = interface::DynamicModelLTV<2 * n_spatial_dim, X, n_spatial_dim>; - using typename DynModI::Mat_vv; - using typename DynModI::Mat_xv; - using typename DynModI::Mat_xx; - using typename DynModI::Vec_x; + static constexpr int N_DIM_x = Parent::N_DIM_x; + static constexpr int N_DIM_u = Parent::N_DIM_u; + static constexpr int N_DIM_v = Parent::N_DIM_v; + + static constexpr int N_SPATIAL_DIM = 2; + static constexpr int N_STATES = 2 * N_SPATIAL_DIM; + + using T = vortex::Types_xuv; - using Vec_s = Eigen::Matrix; - using Mat_ss = Eigen::Matrix; + + 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 * x = [x, y, x_dot, y_dot] * @param std_vel Standard deviation of velocity */ - ConstantVelocity(double std_vel) : std_vel_(std_vel) {} + ConstantVelocity(double std_vel) + : std_vel_(std_vel) + { + } /** Get the Jacobian of the continuous state transition model with respect to the state. * @param dt Time step * @param x State (unused) - * @return Mat_xx + * @return T::Mat_xx * @note Overriding DynamicModelLTV::A_d */ - Mat_xx A_d(double dt, const Vec_x & = Vec_x::Zero()) const override + T::Mat_xx A_d(double dt, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { Mat_ss I = Mat_ss::Identity(); Mat_ss O = Mat_ss::Zero(); - Mat_xx A; + T::Mat_xx A; // clang-format off A << I, I*dt, O, I; @@ -85,13 +157,13 @@ template class ConstantVelocity : public interface::DynamicM /** Get the Jacobian of the continuous state transition model with respect to the process noise. * @param dt Time step * @param x State (unused) - * @return Mat_xv + * @return T::Mat_xv * @note Overriding DynamicModelLTV::G_d */ - Mat_xv G_d(double dt, const Vec_x & = Vec_x::Zero()) const override + T::Mat_xv G_d(double dt, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { Mat_ss I = Mat_ss::Identity(); - Mat_xv G; + T::Mat_xv G; // clang-format off G << 0.5*dt*dt*I, dt*I; @@ -102,10 +174,10 @@ template class ConstantVelocity : public interface::DynamicM /** Get the continuous time process noise covariance matrix. * @param dt Time step (unused) * @param x State (unused) - * @return Mat_xx Process noise covariance + * @return T::Mat_xx Process noise covariance * @note Overriding DynamicModelLTV::Q_d */ - Mat_vv Q_d(double = 0.0, const Vec_x & = Vec_x::Zero()) const override { return Mat_vv::Identity() * std_vel_ * std_vel_; } + T::Mat_vv Q_d(double /*dt*/ = 0.0, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { return T::Mat_vv::Identity() * std_vel_ * std_vel_; } private: double std_vel_; @@ -115,35 +187,38 @@ template class ConstantVelocity : public interface::DynamicM * 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 */ -template class ConstantAcceleration : public interface::DynamicModelLTV<3 * n_spatial_dim, X, 2 * n_spatial_dim> { +class ConstantAcceleration : public interface::DynamicModelLTV<3 * 2, UNUSED, 2 * 2> { public: - using DynModI = interface::DynamicModelLTV<3 * n_spatial_dim, X, 2 * n_spatial_dim>; - using typename DynModI::Vec_v; - using typename DynModI::Vec_x; + static constexpr int N_SPATIAL_DIM = 2; + static constexpr int N_STATES = 3 * N_SPATIAL_DIM; - using typename DynModI::Mat_vv; - using typename DynModI::Mat_xv; - using typename DynModI::Mat_xx; + using T = vortex::Types_xv; - using Vec_s = Eigen::Matrix; - using Mat_ss = Eigen::Matrix; + 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 */ - ConstantAcceleration(double std_vel, double std_acc) : std_vel_(std_vel), std_acc_(std_acc) {} + ConstantAcceleration(double std_vel, double std_acc) + : std_vel_(std_vel) + , std_acc_(std_acc) + { + } /** Get the Jacobian of the continuous state transition model with respect to the state. * @param x State - * @return Mat_xx + * @return T::Mat_xx * @note Overriding DynamicModelLTV::A_d */ - Mat_xx A_d(double dt, const Vec_x &) const override + T::Mat_xx A_d(double dt, const T::Vec_x /*x*/ &) const override { Mat_ss I = Mat_ss::Identity(); Mat_ss O = Mat_ss::Zero(); - Mat_xx A; + T::Mat_xx A; // clang-format off A << I, I*dt, I*0.5*dt*dt, O, I , I*dt , @@ -152,11 +227,11 @@ template class ConstantAcceleration : public interface::Dyna return A; } - Mat_xv G_d(double dt, const Vec_x & = Vec_x::Zero()) const override + T::Mat_xv G_d(double dt, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { Mat_ss I = Mat_ss::Identity(); Mat_ss O = Mat_ss::Zero(); - Mat_xv G; + T::Mat_xv G; // clang-format off G << I*dt, I*0.5*dt*dt, I , I*dt , @@ -168,12 +243,12 @@ template class ConstantAcceleration : public interface::Dyna /** Get the continuous time process noise covariance matrix. * @param dt Time step (unused) * @param x State - * @return Mat_xx Process noise covariance + * @return T::Mat_xx Process noise covariance * @note Overriding DynamicModelLTV::Q_d */ - Mat_vv Q_d(double = 0.0, const Vec_x & = Vec_x::Zero()) const override + T::Mat_vv Q_d(double /*dt*/ = 0.0, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { - Vec_v D; + T::Vec_v D; double var_vel = std_vel_ * std_vel_; double var_acc = std_acc_ * std_acc_; D << var_vel, var_vel, var_acc, var_acc; @@ -188,71 +263,71 @@ template class ConstantAcceleration : public interface::Dyna /** Coordinated Turn Model in 2D. * x = [x_pos, y_pos, x_vel, y_vel, turn_rate] */ -class CoordinatedTurn : public interface::DynamicModelCTLTV<5, X, 3> { +class CoordinatedTurn : public interface::DynamicModelCTLTV<5, UNUSED, 3> { public: - using DynModI = interface::DynamicModelCTLTV<5, X, 3>; - using typename DynModI::Vec_v; - using typename DynModI::Vec_x; - - using typename DynModI::Mat_vv; - using typename DynModI::Mat_xv; - using typename DynModI::Mat_xx; + 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}; /** (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 * @param std_turn Standard deviation of turn rate */ - CoordinatedTurn(double std_vel, double std_turn) : std_vel_(std_vel), std_turn_(std_turn) {} + CoordinatedTurn(double std_vel, double std_turn) + : std_vel_(std_vel) + , std_turn_(std_turn) + { + } /** Get the Jacobian of the continuous state transition model with respect to the state. * @param x State - * @return Mat_xx + * @return T::Mat_xx * @note Overriding DynamicModelCTLTV::A_c */ - Mat_xx A_c(const Vec_x &x) const override + T::Mat_xx A_c(const T::Vec_x /*x*/ &x) const override { - Mat_xx A; // clang-format off - A << 0, 0, 1 , 0 , 0, - 0, 0, 0 , 1 , 0, - 0, 0, 0 ,-x(4), 0, - 0, 0, x(4), 0 , 0, - 0, 0, 0 , 0 , 0; + return T::Mat_xx{ + {0, 0, 1 , 0 , 0}, + {0, 0, 0 , 1 , 0}, + {0, 0, 0 ,-x(4), 0}, + {0, 0, x(4), 0 , 0}, + {0, 0, 0 , 0 , 0} + }; // clang-format on - return A; } /** Get the continuous time process noise matrix * @param x State (unused) - * return Mat_xv Process noise matrix + * return T::Mat_xv Process noise matrix * @note Overriding DynamicModelCTLTV::G_c */ - Mat_xv G_c(const Vec_x & = Vec_x::Zero()) const override + T::Mat_xv G_c(const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { - Mat_xv G; // clang-format off - G << 0, 0, 0, - 0, 0, 0, - 1, 0, 0, - 0, 1, 0, - 0, 0, 1; + return T::Mat_xv { + {0, 0, 0}, + {0, 0, 0}, + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1} + }; // clang-format on - return G; } /** Get the continuous time process noise covariance matrix. * @param x State - * @return Mat_xx Process noise covariance + * @return T::Mat_xx Process noise covariance * @note Overriding DynamicModelCTLTV::Q_c */ - Mat_vv Q_c(const Vec_x & = Vec_x::Zero()) const override + T::Mat_vv Q_c(const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { - Vec_v D; double var_vel = std_vel_ * std_vel_; double var_turn = std_turn_ * std_turn_; - D << var_vel, var_vel, var_turn; - return D.asDiagonal(); + + return T::Vec_v{var_vel, var_vel, var_turn}.asDiagonal(); } private: diff --git a/vortex-filtering/include/vortex_filtering/models/imm_model.hpp b/vortex-filtering/include/vortex_filtering/models/imm_model.hpp index 6468a5ef..94c6b3b7 100644 --- a/vortex-filtering/include/vortex_filtering/models/imm_model.hpp +++ b/vortex-filtering/include/vortex_filtering/models/imm_model.hpp @@ -10,55 +10,92 @@ */ #pragma once #include +#include +#include +#include #include #include #include +#include +#include +#include namespace vortex::models { -// namespace models { + +template constexpr std::array matching_state_names(const std::array &array1, const std::array &array2) +{ + std::array matches{}; + for (std::size_t i = 0; i < N; ++i) { + matches[i] = i < N && i < M && array1.at(i) == array2.at(i); + } + 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. * @tparam DynModels Dynamic models to use. - * @note The models must have a `DynModI` typedef specifying the base interface (e.g. `using DynModI = ...`). */ -template class ImmModel { +template class ImmModel { public: - using DynModTuple = std::tuple; - using DynModPtrTuple = std::tuple...>; + static constexpr std::array N_DIMS_x = {DynModels::N_DIM_x...}; + static constexpr std::array N_DIMS_u = {DynModels::N_DIM_u...}; + static constexpr std::array N_DIMS_v = {DynModels::N_DIM_v...}; + static constexpr bool SAME_DIMS_x = (DynModels::N_DIM_x == ...); + static constexpr bool MIN_DIM_x = std::min(N_DIMS_x); static constexpr size_t N_MODELS = sizeof...(DynModels); - static constexpr bool SAME_DIMS_x = (DynModels::DynModI::N_DIM_x == ...); + 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; - using Vec_n = Eigen::Vector; - using Mat_nn = Eigen::Matrix; + template using DynModT = typename std::tuple_element::type; - template using DynModI = typename std::tuple_element::type::DynModI; // Get the base interface of the i'th model - template using Vec_x = typename DynModI::Vec_x; - template using Vec_u = typename DynModI::Vec_u; - template using Vec_v = typename DynModI::Vec_v; - template using Mat_xx = typename DynModI::Mat_xx; - template using Mat_vv = typename DynModI::Mat_vv; + template using T = Types_xuv; - template using DynModT = typename std::tuple_element::type; - template using DynModTPtr = typename std::shared_ptr>; + /** + * @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)...}) + { + } /** - * @brief Construct a new Imm Model object - * @tparam DynModels Dynamic models to use. The models must be linear-time-varying and have a `DynModI` typedef - * specifying the base interface as the LTV model interface or it's derived classes - * (e.g. `using DynModI = interface::DynamicModelLTV<...>`). + * @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 for each state. Parameter is the mean of an exponential distribution. - * @param models Models to use. The models must have a DynModI typedef specifying the base interface. + * @param hold_times Expected holding time in seconds for each state. Parameter is the mean of an exponential distribution. + * @param models Tuple of dynamic models + * @param state_names Tuple of std::array of state names for each model * @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, DynModels... models) - : models_(std::make_shared(models)...), jump_matrix_(jump_matrix), hold_times_(hold_times) + ImmModel(Mat_nn jump_matrix, Vec_n hold_times, DynModels... models, StateNames state_names) + : 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"); @@ -79,13 +116,14 @@ template class ImmModel { */ Mat_nn get_pi_mat_c() const { - Mat_nn pi_mat_c = hold_times_.replicate(1, N_MODELS); + Vec_n hold_rates = hold_times_.cwiseInverse(); + Mat_nn pi_mat_c = hold_rates.replicate(1, N_MODELS); // Multiply the jump matrix with the hold times pi_mat_c = pi_mat_c.cwiseProduct(jump_matrix_); // Each row should sum to zero - pi_mat_c -= hold_times_.asDiagonal(); + pi_mat_c -= hold_rates.asDiagonal(); return pi_mat_c; } @@ -94,27 +132,43 @@ template class ImmModel { * @brief Compute the discrete-time transition matrix * @return Matrix Discrete-time transition matrix */ - Mat_nn get_pi_mat_d(double dt) const { return (get_pi_mat_c() * dt).exp(); } + Mat_nn get_pi_mat_d(double dt) const + { + // Cache the pi matrix for the same time step as it is likely to be reused and is expensive to compute + static double prev_dt = dt; + static Mat_nn pi_mat_d = (get_pi_mat_c() * dt).exp(); + if (dt != prev_dt) { + pi_mat_d = (get_pi_mat_c() * dt).exp(); + prev_dt = dt; + } + return pi_mat_d; + } /** * @brief Get the dynamic models - * @return Reference to tuple of shared pointers to dynamic models + * @return Reference to tuple of dynamic models + */ + const DynModTuple &get_models() const { return models_; } + + /** + * @brief Get the dynamic models (non-const) + * @return Reference to tuple of dynamic models */ - const DynModPtrTuple &get_models() const { return models_; } + DynModTuple &get_models() { return models_; } /** * @brief Get specific dynamic model * @tparam i Index of model - * @return ModelT shared pointer to model + * @return DynModT model reference */ - template const DynModTPtr &get_model() const { return std::get(models_); } + template const DynModT &get_model() const { return std::get(models_); } /** * @brief Get specific dynamic model (non-const) * @tparam i Index of model - * @return ModelT shared pointer to model + * @return DynModT model reference */ - template DynModTPtr get_model() { return std::get(models_); } + template DynModT &get_model() { return std::get(models_); } /** * @brief f_d of specific dynamic model @@ -125,9 +179,10 @@ template class ImmModel { * @param v Noise (optional) * @return Vec_x */ - template Vec_x f_d(double dt, const Vec_x &x, const Vec_u &u = Vec_u::Zero(), const Vec_v &v = Vec_v::Zero()) const + template + T::Vec_x f_d(double dt, const T::Vec_x &x, const T::Vec_u &u = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const { - return get_model()->f_d(dt, x, u, v); + return get_model().f_d(dt, x, u, v); } /** @@ -137,25 +192,97 @@ template class ImmModel { * @param x State * @return Mat_vv */ - template Mat_vv Q_d(double dt, const Vec_x &x) const { return get_model()->Q_d(dt, x); } + template T::Mat_vv Q_d(double dt, const T::Vec_x &x) const { return get_model().Q_d(dt, x); } - /** - * @brief Get the number of dimensions for the state vector of each dynamic model - * @return std::array - */ - static constexpr std::array get_n_dim_x() { return {DynModels::DynModI::N_DIM_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. + * + */ +template class ImmSensorModel { +public: + static constexpr int N_DIM_x = SensModT::N_DIM_x; + static constexpr int N_DIM_z = SensModT::N_DIM_z; + static constexpr int N_DIM_w = SensModT::N_DIM_w; + static constexpr int N_DIM_a = (int)n_dim_a; + + using T = Types_xzwa; + + ImmSensorModel(SensModT sensor_model) + : sensor_model_(sensor_model) + { + static_assert(N_DIM_a >= SensModT::SensModI::N_DIM_x, "N_DIM_a must be greater than or equal to the state dimension of the sensor model"); + } + + T::Vec_z h(const T::Vec_a &x, const T::Vec_w &w) const { return sensor_model_.h(x.template head(), w); } + + T::Mat_ww R(const T::Vec_x &x) const { return sensor_model_.R(x.template head()); } + +private: + SensModT sensor_model_; +}; + +template class ImmSensorModelLTV { +public: + static constexpr int N_DIM_x = SensModT::N_DIM_x; + static constexpr int N_DIM_z = SensModT::N_DIM_z; + static constexpr int N_DIM_w = SensModT::N_DIM_w; + static constexpr int N_DIM_a = (int)n_dim_a; + + using T = Types_xzwa; + + ImmSensorModelLTV(SensModT sensor_model) + : sensor_model_(sensor_model) + { + static_assert(N_DIM_a >= N_DIM_x, "N_DIM_a must be greater than or equal to the state dimension of the sensor model"); + } + + T::Vec_z h(const T::Vec_a &x, const T::Vec_w &w) const { return sensor_model_.h(x.template head(), w); } + + T::Mat_za C(const T::Vec_a &x) const + { + typename T::Mat_za C_a = T::Mat_za::Zero(); + C_a.template leftCols() = sensor_model_.C(x.template head()); + return C_a; + } + + T::Mat_zw H(const T::Vec_a &x) const { return sensor_model_.H(x.template head()); } + + T::Mat_ww R(const T::Vec_a &x) const { return sensor_model_.R(x.template head()); } + + T::Gauss_z pred_from_est(const T::Gauss_a &x_est) const + { + typename T::Vec_x mean = x_est.mean().template head(); + typename T::Mat_xx cov = x_est.cov().template topLeftCorner(); + return sensor_model_.pred_from_est({mean, cov}); + } + + T::Gauss_z pred_from_state(const T::Vec_a &x) const { return sensor_model_.pred_from_state(x.template head()); } private: - DynModPtrTuple models_; - const Mat_nn jump_matrix_; - const Vec_n hold_times_; + SensModT sensor_model_; }; namespace concepts { template concept ImmModel = requires { typename T::DynModTuple; - typename T::DynModPtrTuple; }; } // namespace concepts 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 ca85a05b..0f33cce1 100644 --- a/vortex-filtering/include/vortex_filtering/models/sensor_model_interfaces.hpp +++ b/vortex-filtering/include/vortex_filtering/models/sensor_model_interfaces.hpp @@ -13,6 +13,7 @@ #include #include #include +#include namespace vortex::models { namespace interface { @@ -28,31 +29,11 @@ namespace interface { */ template class SensorModel { public: - using SensModI = SensorModel; + static constexpr int N_DIM_x = (int)n_dim_x; + static constexpr int N_DIM_z = (int)n_dim_z; + static constexpr int N_DIM_w = (int)n_dim_w; - static constexpr int N_DIM_x = (int)n_dim_x; // Declare so that children of this class can reference it - static constexpr int N_DIM_z = (int)n_dim_z; // Declare so that children of this class can reference it - static constexpr int N_DIM_w = (int)n_dim_w; // Declare so that children of this class can reference it - - using Vec_x = Eigen::Vector; - using Vec_z = Eigen::Vector; - using Vec_w = Eigen::Vector; - - using Mat_xx = Eigen::Matrix; - using Mat_xz = Eigen::Matrix; - using Mat_xw = Eigen::Matrix; - - using Mat_zx = Eigen::Matrix; - using Mat_zz = Eigen::Matrix; - using Mat_zw = Eigen::Matrix; - - using Mat_ww = Eigen::Matrix; - - using Gauss_x = prob::MultiVarGauss; - using Gauss_z = prob::MultiVarGauss; - using Gauss_w = prob::MultiVarGauss; - - using SharedPtr = std::shared_ptr; + using T = vortex::Types_xzw; SensorModel() = default; virtual ~SensorModel() = default; @@ -62,14 +43,14 @@ template class Sensor * @param x State * @return Vec_z */ - virtual Vec_z h(const Vec_x &x, const Vec_w &w) const = 0; + virtual T::Vec_z h(const T::Vec_x &x, const T::Vec_w &w = T::Vec_w::Zero()) const = 0; /** * @brief Noise covariance matrix * @param x State * @return Mat_zz */ - virtual Mat_ww R(const Vec_x &x) const = 0; + virtual T::Mat_ww R(const T::Vec_x &x) const = 0; /** Sample from the sensor model * @param x State @@ -77,9 +58,9 @@ template class Sensor * @param gen Random number generator (For deterministic behaviour) * @return Vec_z */ - Vec_z sample_h(const Vec_x &x, std::mt19937 &gen) const + T::Vec_z sample_h(const T::Vec_x &x, std::mt19937 &gen) const { - prob::MultiVarGauss w = {Vec_w::Zero(), R(x)}; + typename T::Gauss_w w{T::Vec_w::Zero(), R(x)}; return this->h(x, w.sample(gen)); } }; @@ -95,28 +76,13 @@ template class Sensor * @note - R * @note - H (optional if N_DIM_x == N_DIM_z) */ -template class SensorModelLTV : public SensorModel { +template class SensorModelLTV : public SensorModel { public: - using SensModI = SensorModel; - static constexpr int N_DIM_x = SensModI::N_DIM_x; - static constexpr int N_DIM_z = SensModI::N_DIM_z; - static constexpr int N_DIM_w = SensModI::N_DIM_w; - - using Vec_z = typename SensModI::Vec_z; - using Vec_x = typename SensModI::Vec_x; - using Vec_w = typename SensModI::Vec_w; - - using Mat_xx = typename SensModI::Mat_xx; - using Mat_zx = typename SensModI::Mat_zx; - using Mat_zz = typename SensModI::Mat_zz; - using Mat_zw = typename SensModI::Mat_zw; + static constexpr int N_DIM_x = n_dim_x; + static constexpr int N_DIM_z = n_dim_z; + static constexpr int N_DIM_w = n_dim_w; - using Mat_ww = typename SensModI::Mat_ww; - - using Gauss_x = typename SensModI::Gauss_x; - using Gauss_z = typename SensModI::Gauss_z; - - using SharedPtr = std::shared_ptr; + using T = vortex::Types_xzw; virtual ~SensorModelLTV() = default; /** Sensor Model @@ -125,10 +91,10 @@ template class Sensor * @param w Noise * @return Vec_z */ - virtual Vec_z h(const Vec_x &x, const Vec_w &w = Vec_w::Zero()) const override + virtual T::Vec_z h(const T::Vec_x &x, const T::Vec_w &w = T::Vec_w::Zero()) const override { - Mat_zx C = this->C(x); - Mat_zw H = this->H(x); + typename T::Mat_zx C = this->C(x); + typename T::Mat_zw H = this->H(x); return C * x + H * w; } @@ -137,20 +103,19 @@ template class Sensor * @param x State * @return Mat_zx */ - virtual Mat_zx C(const Vec_x &x) const = 0; + virtual T::Mat_zx C(const T::Vec_x &x) const = 0; /** * @brief Noise matrix * @param x State * @return Mat_zz */ - virtual Mat_zw H(const Vec_x &x = Vec_x::Zero()) const + virtual T::Mat_zw H(const T::Vec_x & /* x */ = T::Vec_x::Zero()) const { if (N_DIM_x != N_DIM_z) { throw std::runtime_error("SensorModelLTV::H not implemented"); } - (void)x; // unused - return Mat_zw::Identity(); + return T::Mat_zw::Identity(); } /** @@ -158,20 +123,20 @@ template class Sensor * @param x State * @return Mat_zz */ - virtual Mat_ww R(const Vec_x &x) const override = 0; + virtual T::Mat_ww R(const T::Vec_x &x) const override = 0; /** * @brief Get the predicted measurement distribution given a state estimate. Updates the covariance * - * @param x_est Vec_x estimate + * @param x_est TVec_x estimate * @return prob::MultiVarGauss */ - Gauss_z pred_from_est(const Gauss_x &x_est) const + T::Gauss_z pred_from_est(const T::Gauss_x &x_est) const { - Mat_xx P = x_est.cov(); - Mat_zx C = this->C(x_est.mean()); - Mat_ww R = this->R(x_est.mean()); - Mat_zw H = this->H(x_est.mean()); + typename T::Mat_xx P = x_est.cov(); + typename T::Mat_zx C = this->C(x_est.mean()); + typename T::Mat_ww R = this->R(x_est.mean()); + typename T::Mat_zw H = this->H(x_est.mean()); return {this->h(x_est.mean()), C * P * C.transpose() + H * R * H.transpose()}; } @@ -181,33 +146,13 @@ template class Sensor * @param x Vec_x * @return prob::MultiVarGauss */ - Gauss_z pred_from_state(const Vec_x &x) const + T::Gauss_z pred_from_state(const T::Vec_x &x) const { - Mat_ww R = this->R(x); - Mat_zw H = this->H(x); + typename T::Mat_ww R = this->R(x); + typename T::Mat_zw H = this->H(x); return {this->h(x), H * R * H.transpose()}; } }; } // namespace interface - -namespace concepts { - -template -concept SensorModel = requires { - // Has member type SensModI - typename T::SensModI; - // Derived from SensorModel - std::derived_from>; -}; - -template -concept SensorModelLTV = requires { - // Has member type SensModI - typename T::SensModI; - // Derived from SensorModelLTV - std::derived_from>; -}; - -} // namespace concepts } // namespace vortex::models \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/models/sensor_models.hpp b/vortex-filtering/include/vortex_filtering/models/sensor_models.hpp index 254633f9..4e5d7e41 100644 --- a/vortex-filtering/include/vortex_filtering/models/sensor_models.hpp +++ b/vortex-filtering/include/vortex_filtering/models/sensor_models.hpp @@ -10,16 +10,14 @@ namespace models { * @tparam n_dim_x Dimension of state * @tparam n_dim_z Dimension of measurement */ -template class IdentitySensorModel : public interface::SensorModelLTV { +template class IdentitySensorModel : public interface::SensorModelLTV { + using Parent = interface::SensorModelLTV; public: - using SensModI = interface::SensorModelLTV; + static constexpr int N_DIM_x = Parent::N_DIM_x; + static constexpr int N_DIM_z = Parent::N_DIM_z; + static constexpr int N_DIM_w = Parent::N_DIM_w; - using typename SensModI::Mat_xx; - using typename SensModI::Mat_zw; - using typename SensModI::Mat_zx; - using typename SensModI::Mat_zz; - using typename SensModI::Vec_x; - using typename SensModI::Vec_z; + using T = vortex::Types_xzw; /** Construct a new Simple Sensor Model object. * The measurement model is simply the n_dim_z first elements of the state vector. @@ -27,37 +25,43 @@ template class IdentitySensorModel : public interface * @tparam n_dim_x Dimension of state * @tparam n_dim_z Dimension of measurement */ - IdentitySensorModel(double std) : R_(Mat_zz::Identity() * std * std) {} + IdentitySensorModel(double std) + : R_(T::Mat_ww::Identity() * std * std) + { + } /** Construct a new Simple Sensor Model object. * The measurement model is simply the n_dim_z first elements of the state vector. * @param R Measurement covariance matrix */ - IdentitySensorModel(Mat_zz R) : R_(R) {} + IdentitySensorModel(T::Mat_zz R) + : R_(R) + { + } /** Get the Jacobian of the measurement model with respect to the state. * @param x State (not used) * @return Mat_zx * @note Overriding SensorModelLTV::C */ - Mat_zx C(const Vec_x & = Vec_x::Zero()) const override { return Mat_zx::Identity(); } + T::Mat_zx C(const T::Vec_x /* x */& = T::Vec_x::Zero()) const override { return T::Mat_zx::Identity(); } /** Get the measurement covariance matrix. * @param x State (not used) * @return Mat_zz * @note Overriding SensorModelLTV::R */ - Mat_zz R(const Vec_x & = Vec_x::Zero()) const override { return R_; } + T::Mat_zz R(const T::Vec_x /* x */ & = T::Vec_x::Zero()) const override { return R_; } /** Get the Jacobian of the measurement model with respect to noise * @param x State (not used) * @return Mat_zw * @note Overriding SensorModelLTV::H */ - Mat_zw H(const Vec_x & = Vec_x::Zero()) const override { return Mat_zw::Identity(); } + T::Mat_zw H(const T::Vec_x /* x */& = T::Vec_x::Zero()) const override { return T::Mat_zw::Identity(); } private: - const Mat_zz R_; // Measurement covariance matrix + const T::Mat_ww R_; // Measurement covariance matrix }; /** Range-Bearing sensor model. @@ -66,14 +70,13 @@ template class IdentitySensorModel : public interface * x = [x_target, y_target] * z = [range, bearing] */ -class RangeBearingSensor : public interface::SensorModelLTV<2, 2> { +class RangeBearingSensor : public interface::SensorModelLTV<2, 2, 2> { public: - using SensModI = interface::SensorModel<2, 2>; - using typename SensModI::Mat_xx; - using typename SensModI::Mat_zx; - using typename SensModI::Mat_zz; - using typename SensModI::Vec_x; - using typename SensModI::Vec_z; + static constexpr int N_DIM_x = 2; + static constexpr int N_DIM_z = 2; + static constexpr int N_DIM_w = 2; + + using T = vortex::Types_xzw; /** Range-Bearing sensor model. * The measurement model is the range and bearing to the target. @@ -92,10 +95,9 @@ class RangeBearingSensor : public interface::SensorModelLTV<2, 2> { * @return Vec_z * @note Overriding SensorModelLTV::h */ - Vec_z h(const Vec_x &x, const Vec_w &w = Vec_w::Zero()) const override + T::Vec_z h(const T::Vec_x &x, const T::Vec_w &w = T::Vec_w::Zero()) const override { - Vec_z z; - z << std::sqrt(x(0) * x(0) + x(1) * x(1)), std::atan2(x(1), x(0)); + typename T::Vec_z z{std::sqrt(x(0) * x(0) + x(1) * x(1)), std::atan2(x(1), x(0))}; z += w; return z; } @@ -105,12 +107,13 @@ class RangeBearingSensor : public interface::SensorModelLTV<2, 2> { * @return Mat_zx * @note Overriding SensorModelLTV::C */ - Mat_zx C(const Vec_x &x) const override + T::Mat_zx C(const T::Vec_x &x) const override { - Mat_zx C; // clang-format off - C << (x(0) / std::sqrt(x(0)*x(0) + x(1)*x(1))), (x(1) / std::sqrt(x(0)*x(0) + x(1)*x(1))), - (-x(1) / (x(0)*x(0) + x(1)*x(1))) , (x(0) / (x(0)*x(0) + x(1)*x(1))); + typename T::Mat_zx C{ + {(x(0) / std::sqrt(x(0) * x(0) + x(1) * x(1))), (x(1) / std::sqrt(x(0) * x(0) + x(1) * x(1)))}, + {(-x(1) / (x(0) * x(0) + x(1) * x(1))) , (x(0) / (x(0) * x(0) + x(1) * x(1)))} + }; // clang-format on return C; } @@ -120,11 +123,10 @@ class RangeBearingSensor : public interface::SensorModelLTV<2, 2> { * @return Mat_zz * @note Overriding SensorModelLTV::R */ - Mat_zz R(const Vec_x & = Vec_x::Zero()) const override + T::Mat_zz R(const T::Vec_x & = T::Vec_x::Zero()) const override { - Vec_z D; - D << std_range_ * std_range_, std_bearing_ * std_bearing_; - Mat_zz R = D.asDiagonal(); + typename T::Vec_z D{std_range_ * std_range_, std_bearing_ * std_bearing_}; + typename T::Mat_zz R = D.asDiagonal(); return R; } diff --git a/vortex-filtering/include/vortex_filtering/numerical_integration/erk_methods.hpp b/vortex-filtering/include/vortex_filtering/numerical_integration/erk_methods.hpp index ae7a23d0..a10f56fc 100644 --- a/vortex-filtering/include/vortex_filtering/numerical_integration/erk_methods.hpp +++ b/vortex-filtering/include/vortex_filtering/numerical_integration/erk_methods.hpp @@ -155,12 +155,12 @@ template class ODE45 { // Use the Dormand Prince (RKDP) method // clang-format off _A << 0 , 0 , 0 , 0 , 0 , 0 , 0, - 1/5.0 , 0 , 0 , 0 , 0 , 0 , 0, - 3/40.0 , 9/40.0 , 0 , 0 , 0 , 0 , 0, - 44/45.0 , -56/15.0 , 32/9.0 , 0 , 0 , 0 , 0, - 19372/6561.0, -25360/2187.0, 64448/6561.0, -212/729.0, 0 , 0 , 0, - 9017/3168.0 , -355/33.0 , 46732/5247.0, 49/176.0 , -5103/18656.0 , 0 , 0, - 35/384.0 , 0 , 500/1113.0 , 125/192.0, -2187/6784.0 , 11/84.0 , 0; + 1/5.0 , 0 , 0 , 0 , 0 , 0 , 0, + 3/40.0 , 9/40.0 , 0 , 0 , 0 , 0 , 0, + 44/45.0 , -56/15.0 , 32/9.0 , 0 , 0 , 0 , 0, + 19372/6561.0, -25360/2187.0, 64448/6561.0, -212/729.0, 0 , 0 , 0, + 9017/3168.0 , -355/33.0 , 46732/5247.0, 49/176.0 , -5103/18656.0 , 0 , 0, + 35/384.0 , 0 , 500/1113.0 , 125/192.0, -2187/6784.0 , 11/84.0 , 0; Eigen::Matrix b_T; b_T <<35/384.0 , 0 , 500/1113.0 , 125/192.0, -2187/6784.0 , 11/84.0 , 0, // Error of order O(dt^5) diff --git a/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp b/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp index 38342d6b..ea093f26 100644 --- a/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp @@ -10,13 +10,34 @@ * */ #pragma once +#include +#include #include #include // std::accumulate +#include #include #include namespace vortex::prob { +namespace concepts { + +template +concept is_container = requires(Container c) { + // The container must have a value_type. + typename Container::value_type; + // The size of the container must be convertible to std::size_t. + { + std::size(c) + } -> std::convertible_to; + // Accessing an element by index must return a reference to ValueType. + { + c[std::declval()] + } -> std::same_as; +}; + +} // namespace concepts + /** * A class for representing a multivariate Gaussian mixture distribution * @tparam n_dims dimentions of the Gaussian @@ -34,15 +55,9 @@ template class GaussianMixture { * @param gaussians Gaussians * @note The weights are automatically normalized, so they don't need to sum to 1. */ - GaussianMixture(Eigen::VectorXd weights, std::vector gaussians) : weights_(std::move(weights)), gaussians_(std::move(gaussians)) {} - - /** Construct a new Gaussian Mixture object from a std::vector of weights - * @param weights Weights of the Gaussians - * @param gaussians Gaussians - * @note The weights are automatically normalized, so they don't need to sum to 1. - */ - GaussianMixture(std::vector weights, std::vector gaussians) - : weights_(Eigen::Map(weights.data(), weights.size())), gaussians_(std::move(gaussians)) + GaussianMixture(concepts::is_container auto const &weights, concepts::is_container auto const &gaussians) + : weights_(Eigen::Map(weights.data(), std::distance(std::begin(weights), std::end(weights)))), + gaussians_(std::begin(gaussians), std::end(gaussians)) { } @@ -55,7 +70,7 @@ template class GaussianMixture { /** Copy Constructor * @param gaussian_mixture */ - GaussianMixture(const GaussianMixture &gaussian_mixture) : weights_(gaussian_mixture.weights_), gaussians_(gaussian_mixture.gaussians_) {} + GaussianMixture(const GaussianMixture &gaussian_mixture) : weights_(gaussian_mixture.weights()), gaussians_(gaussian_mixture.gaussians()) {} /** Calculate the probability density function of x given the Gaussian mixture * @param x @@ -91,7 +106,7 @@ template class GaussianMixture { { // Spread of innovations Mat_nn P_bar = Mat_nn::Zero(); - for (int i = 0; i < num_components(); i++) { + for (size_t i = 0; i < num_components(); i++) { P_bar += get_weight(i) * gaussian(i).mean() * gaussian(i).mean().transpose(); } P_bar /= sum_weights(); @@ -99,7 +114,7 @@ template class GaussianMixture { // Spread of Gaussians Mat_nn P = Mat_nn::Zero(); - for (int i = 0; i < num_components(); i++) { + for (size_t i = 0; i < num_components(); i++) { P += get_weight(i) * gaussian(i).cov(); } P /= sum_weights(); @@ -164,7 +179,12 @@ template class GaussianMixture { /** Get the number of Gaussians in the mixture * @return int */ - int num_components() const { return gaussians_.size(); } + size_t num_components() const { return gaussians_.size(); } + + /** Number of components + * + */ + size_t size() const { return num_components(); } /** Sample from the Gaussian mixture * @param gen Random number generator 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 a0ed4f7a..17adb2cb 100644 --- a/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp @@ -93,6 +93,27 @@ template class MultiVarGauss { */ static MultiVarGauss Standard() { return {Vec_n::Zero(), Mat_nn::Identity()}; } + /** operator== + * @param lhs + * @param rhs + * @return bool true if the means and covariances are equal + */ + friend bool operator==(const MultiVarGauss &lhs, const MultiVarGauss &rhs) { return lhs.mean() == rhs.mean() && lhs.cov() == rhs.cov(); } + + /** operator<< + * @param os + * @param gauss + * @return std::ostream& + */ + friend std::ostream &operator<<(std::ostream &os, const MultiVarGauss &gauss) + { + os << "Mean:\n" + << gauss.mean().transpose() << "\n" + << "Covariance:\n" + << gauss.cov(); + return os; + } + private: Vec_n mean_; Mat_nn cov_; @@ -100,8 +121,11 @@ template class MultiVarGauss { }; template using Gauss = MultiVarGauss; -using Gauss2d = Gauss<2>; -using Gauss3d = Gauss<3>; -using Gauss4d = Gauss<4>; + +using Gauss2d = Gauss<2>; +using Gauss3d = Gauss<3>; +using Gauss4d = Gauss<4>; +using Gauss5d = Gauss<5>; +using Gauss6d = Gauss<6>; } // namespace vortex::prob diff --git a/vortex-filtering/include/vortex_filtering/probability/poisson.hpp b/vortex-filtering/include/vortex_filtering/probability/poisson.hpp index 493addf5..633cb84e 100644 --- a/vortex-filtering/include/vortex_filtering/probability/poisson.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/poisson.hpp @@ -45,7 +45,7 @@ class Poisson { * @param x * @return double factorial */ - static constexpr double factorial(int x) { return (x == 1 || x == 0) ? 1 : factorial(x - 1) * x; } + static constexpr double factorial(size_t x) { return (x == 1 || x == 0) ? 1 : factorial(x - 1) * x; } }; } // namespace vortex::prob diff --git a/vortex-filtering/include/vortex_filtering/probability/uniform.hpp b/vortex-filtering/include/vortex_filtering/probability/uniform.hpp new file mode 100644 index 00000000..adbe5240 --- /dev/null +++ b/vortex-filtering/include/vortex_filtering/probability/uniform.hpp @@ -0,0 +1,91 @@ +/** + * @file uniform.hpp + * @author Eirik Kolås + * @brief + * @version 0.1 + * @date 2024-01-20 + * + * @copyright Copyright (c) 2024 + * + */ +#pragma once +#include +#include + +namespace vortex::prob { + +template class Uniform { +public: + using Vec_n = Eigen::Vector; + using Mat_nn = Eigen::Matrix; + + constexpr Uniform(Vec_n lower, Vec_n upper) : lower_(lower), upper_(upper) {} + + double pr(Vec_n x) const + { + for (size_t i = 0; i < n_dims; i++) { + if (x(i) < lower_(i) || x(i) > upper_(i)) { + return 0; + } + } + return 1.0 / (upper_ - lower_).prod(); + } + + Vec_n sample(std::mt19937 &gen) const + { + Vec_n sample; + for (size_t i = 0; i < n_dims; i++) { + std::uniform_real_distribution dist(lower_(i), upper_(i)); + sample(i) = dist(gen); + } + return sample; + } + + Vec_n mean() const { return (upper_ + lower_) / 2; } + Mat_nn cov() const + { + Vec_n diff = upper_ - lower_; + for (double &d : diff) { + d *= d / 12; + } + return diff.asDiagonal(); + } + + const Vec_n &lower() const { return lower_; } + const Vec_n &upper() const { return upper_; } + +private: + Vec_n lower_; + Vec_n upper_; +}; + +template <> class Uniform<1> { +public: + constexpr Uniform(double lower, double upper) : lower_(lower), upper_(upper) {} + + double pr(double x) const + { + if (x < lower_ || x > upper_) { + return 0; + } + return 1.0 / (upper_ - lower_); + } + + double sample(std::mt19937 &gen) const + { + std::uniform_real_distribution dist(lower_, upper_); + return dist(gen); + } + + double mean() const { return (upper_ + lower_) / 2; } + double cov() const { return (upper_ - lower_) * (upper_ - lower_) / 12; } + + double lower() const { return lower_; } + double upper() const { return upper_; } + +private: + double lower_; + double upper_; +}; + +} // namespace vortex::prob \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/types/general_concepts.hpp b/vortex-filtering/include/vortex_filtering/types/general_concepts.hpp new file mode 100644 index 00000000..9f4c0dd4 --- /dev/null +++ b/vortex-filtering/include/vortex_filtering/types/general_concepts.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include +#include +#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); +}; + +} // 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 new file mode 100644 index 00000000..87580f0f --- /dev/null +++ b/vortex-filtering/include/vortex_filtering/types/model_concepts.hpp @@ -0,0 +1,183 @@ +#pragma once +#include +#include +#include + +namespace vortex::concepts { +using std::size_t; + +/** + * @brief Concept for dynamic models. Requires the following functions: + * @brief - `Vec_x f_d(double, Vec_x, Vec_u, Vec_v)` + * @brief - `Mat_vv Q_d(double, Vec_x)` + * + * @tparam DynMod The dynamic model type + * @tparam n_dim_x Dimension of the state + * @tparam n_dim_u Dimension of the input + * @tparam n_dim_v Dimension of the process noise + */ +template +concept DynamicModel = requires { + { + std::declval().f_d(std::declval(), + std::declval::Vec_x>(), + std::declval::Vec_u>(), + std::declval::Vec_v>()) + } -> mat_convertible_to::Vec_x>; + + { + std::declval().Q_d(std::declval(), std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_vv>; +}; + +/** + * @brief Concept for dynamic models with time-varying parameters. Requires the following functions: + * @brief - `Vec_x f_d(double, Vec_x, Vec_u, Vec_v)` + * @brief - `Mat_vv Q_d(double, Vec_x)` + * @brief - `Mat_xx A_d(double, Vec_x)` + * @brief - `Mat_xu B_d(double, Vec_x)` + * @brief - `Mat_xv G_d(double, Vec_x)` + * @brief - `Gauss_x pred_from_est(double, Gauss_x, Vec_u)` + * @brief - `Gauss_x pred_from_state(double, Vec_x, Vec_u)` + * + * @tparam DynMod The dynamic model type + * @tparam n_dim_x Dimension of the state + * @tparam n_dim_u Dimension of the input + * @tparam n_dim_v Dimension of the process noise + */ +template +concept DynamicModelLTV = requires { + requires DynamicModel; // Assuming DynamicModel is correctly defined as shown before + { + std::declval().A_d(std::declval(), std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_xx>; + + { + std::declval().B_d(std::declval(), std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_xu>; + + { + std::declval().G_d(std::declval(), std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_xv>; + + { + std::declval().pred_from_est( + std::declval(), std::declval::Gauss_x>(), std::declval::Vec_u>()) + } -> std::convertible_to::Gauss_x>; + + { + std::declval().pred_from_state( + std::declval(), std::declval::Vec_x>(), std::declval::Vec_u>()) + } -> std::convertible_to::Gauss_x>; +}; + +/** + * @brief Concept for sensor models. Requires the following functions: + * @brief - `Vec_z h(Vec_x, Vec_w)` + * @brief - `Mat_ww R(Vec_x)` + * + * @tparam SensMod The sensor model type + * @tparam n_dim_x Dimension of the state + * @tparam n_dim_z Dimension of the measurement + * @tparam n_dim_w Dimension of the measurement noise + */ +template +concept SensorModel = requires { + { + std::declval().h(std::declval::Vec_x>(), std::declval::Vec_w>()) + } -> mat_convertible_to::Vec_z>; + { + std::declval().R(std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_ww>; +}; + + +/** + * @brief Concept for sensor models with time-varying parameters. Requires the following functions: + * @brief - `Vec_z h(Vec_x, Vec_w)` + * @brief - `Mat_zz R(Vec_x)` + * @brief - `Mat_zw H(double, Vec_x)` + * @brief - `Mat_zx C(Vec_x)` + * @brief - `Gauss_z pred_from_est(Gauss_x)` + * @brief - `Gauss_z pred_from_state(Vec_x)` + */ +template +concept SensorModelLTV = requires { + requires SensorModel; + { + std::declval().H(std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_zw>; + { + std::declval().C(std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_zx>; + { + std::declval().pred_from_est(std::declval::Gauss_x>()) + } -> std::convertible_to::Gauss_z>; + { + std::declval().pred_from_state(std::declval::Vec_x>()) + } -> std::convertible_to::Gauss_z>; +}; + +/////////////////////////////////// +// +/////////////////////////////////// + +template +concept DynamicModelWithDefinedSizes = requires { + { + DynMod::N_DIM_x + } -> std::convertible_to; + { + DynMod::N_DIM_v + } -> std::convertible_to; + { + DynMod::N_DIM_u + } -> std::convertible_to; + requires DynamicModel; +}; + +template +concept DynamicModelLTVWithDefinedSizes = requires { + requires DynamicModelWithDefinedSizes; + { + DynMod::N_DIM_x + } -> std::convertible_to; + { + DynMod::N_DIM_v + } -> std::convertible_to; + { + DynMod::N_DIM_u + } -> std::convertible_to; + requires DynamicModelLTV; +}; + +template +concept SensorModelWithDefinedSizes = requires { + { + SensMod::N_DIM_x + } -> std::convertible_to; + { + SensMod::N_DIM_z + } -> std::convertible_to; + { + SensMod::N_DIM_w + } -> std::convertible_to; + requires SensorModel; +}; + +template +concept SensorModelLTVWithDefinedSizes = requires { + requires SensorModelWithDefinedSizes; + { + SensMod::N_DIM_x + } -> std::convertible_to; + { + SensMod::N_DIM_z + } -> std::convertible_to; + { + SensMod::N_DIM_w + } -> std::convertible_to; + requires SensorModelLTV; +}; + +} // namespace vortex::concepts \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/types/type_aliases.hpp b/vortex-filtering/include/vortex_filtering/types/type_aliases.hpp new file mode 100644 index 00000000..07aa3170 --- /dev/null +++ b/vortex-filtering/include/vortex_filtering/types/type_aliases.hpp @@ -0,0 +1,128 @@ +#pragma once + +#include +#include +#include + +#define MATRIX_TYPES(t1, s1, t2, s2) \ + using Mat_##t1##t2 = Eigen::Matrix; \ + using Mat_##t2##t1 = Eigen::Matrix; + +#define ONE_TYPE(t1, s1) \ + static constexpr size_t N_DIM_##t1 = s1; \ + \ + using Vec_##t1 = Eigen::Vector; \ + using Mat_##t1##t1 = Eigen::Matrix; \ + using Gauss_##t1 = vortex::prob::Gauss; \ + using GaussMix_##t1 = vortex::prob::GaussMix; + +#define TWO_TYPES(t1, s1, t2, s2) \ + ONE_TYPE(t1, s1) \ + ONE_TYPE(t2, s2) \ + MATRIX_TYPES(t1, s1, t2, s2) + +#define THREE_TYPES(t1, s1, t2, s2, t3, s3) \ + ONE_TYPE(t1, s1) \ + ONE_TYPE(t2, s2) \ + ONE_TYPE(t3, s3) \ + MATRIX_TYPES(t1, s1, t2, s2) \ + MATRIX_TYPES(t1, s1, t3, s3) \ + MATRIX_TYPES(t2, s2, t3, s3) + +#define FOUR_TYPES(t1, s1, t2, s2, t3, s3, t4, s4) \ + ONE_TYPE(t1, s1) \ + ONE_TYPE(t2, s2) \ + ONE_TYPE(t3, s3) \ + ONE_TYPE(t4, s4) \ + MATRIX_TYPES(t1, s1, t2, s2) \ + MATRIX_TYPES(t1, s1, t3, s3) \ + MATRIX_TYPES(t1, s1, t4, s4) \ + MATRIX_TYPES(t2, s2, t3, s3) \ + MATRIX_TYPES(t2, s2, t4, s4) \ + MATRIX_TYPES(t3, s3, t4, s4) + +#define FIVE_TYPES(t1, s1, t2, s2, t3, s3, t4, s4, t5, s5) \ + ONE_TYPE(t1, s1) \ + ONE_TYPE(t2, s2) \ + ONE_TYPE(t3, s3) \ + ONE_TYPE(t4, s4) \ + ONE_TYPE(t5, s5) \ + MATRIX_TYPES(t1, s1, t2, s2) \ + MATRIX_TYPES(t1, s1, t3, s3) \ + MATRIX_TYPES(t1, s1, t4, s4) \ + MATRIX_TYPES(t1, s1, t5, s5) \ + MATRIX_TYPES(t2, s2, t3, s3) \ + MATRIX_TYPES(t2, s2, t4, s4) \ + MATRIX_TYPES(t2, s2, t5, s5) \ + MATRIX_TYPES(t3, s3, t4, s4) \ + MATRIX_TYPES(t3, s3, t5, s5) \ + MATRIX_TYPES(t4, s4, t5, s5) + +#define ONE_TYPE_STRUCT(t1) \ + template struct Types_##t1 { \ + Types_##t1() = delete; \ + ONE_TYPE(t1, n_dim_##t1) \ + }; + +#define TWO_TYPES_STRUCT(t1, t2) \ + template struct Types_##t1##t2 { \ + Types_##t1##t2() = delete; \ + TWO_TYPES(t1, n_dim_##t1, t2, n_dim_##t2) \ + }; + +#define THREE_TYPES_STRUCT(t1, t2, t3) \ + template struct Types_##t1##t2##t3 { \ + Types_##t1##t2##t3() = delete; \ + THREE_TYPES(t1, n_dim_##t1, t2, n_dim_##t2, t3, n_dim_##t3) \ + }; + +#define FOUR_TYPES_STRUCT(t1, t2, t3, t4) \ + template struct Types_##t1##t2##t3##t4 { \ + Types_##t1##t2##t3##t4() = delete; \ + FOUR_TYPES(t1, n_dim_##t1, t2, n_dim_##t2, t3, n_dim_##t3, t4, n_dim_##t4) \ + }; + +#define FIVE_TYPES_STRUCT(t1, t2, t3, t4, t5) \ + template struct Types_##t1##t2##t3##t4##t5 { \ + Types_##t1##t2##t3##t4##t5() = delete; \ + FIVE_TYPES(t1, n_dim_##t1, t2, n_dim_##t2, t3, n_dim_##t3, t4, n_dim_##t4, t5, n_dim_##t5) \ + }; + +namespace vortex { +// Hover over the types in vscode to see the expanded types + +ONE_TYPE_STRUCT(x) +ONE_TYPE_STRUCT(z) +ONE_TYPE_STRUCT(u) +ONE_TYPE_STRUCT(v) +ONE_TYPE_STRUCT(w) + +TWO_TYPES_STRUCT(x, z) // Sensor model without noise +TWO_TYPES_STRUCT(x, v) // Dynamic model without input +TWO_TYPES_STRUCT(x, u) // Dynamic model without noise +TWO_TYPES_STRUCT(x, w) + +TWO_TYPES_STRUCT(z, w) + +THREE_TYPES_STRUCT(x, u, v) // Dynamic model +THREE_TYPES_STRUCT(x, z, w) // Sensor model +THREE_TYPES_STRUCT(x, z, n) // For IMM Filter + +FOUR_TYPES_STRUCT(x, z, w, a) // Sensor model and augmented state a + +FIVE_TYPES_STRUCT(x, z, u, v, w) // Dynamic model and sensor model + +} // namespace vortex + +// Don't want you to use these macros outside of this file :) +#undef ONE_TYPE +#undef TWO_TYPES +#undef THREE_TYPES +#undef FOUR_TYPES +#undef FIVE_TYPES +#undef ONE_TYPE_STRUCT +#undef TWO_TYPES_STRUCT +#undef THREE_TYPES_STRUCT +#undef FOUR_TYPES_STRUCT +#undef FIVE_TYPES_STRUCT +#undef MATRIX_TYPES diff --git a/vortex-filtering/include/vortex_filtering/utils/printers.hpp b/vortex-filtering/include/vortex_filtering/utils/printers.hpp new file mode 100644 index 00000000..e4b85867 --- /dev/null +++ b/vortex-filtering/include/vortex_filtering/utils/printers.hpp @@ -0,0 +1,29 @@ +#pragma once +#include +#include +#include + +namespace detail { +template std::ostream &println_tuple_impl(std::ostream &os, std::tuple tuple, std::index_sequence) +{ + static_assert(sizeof...(Is) == sizeof...(Ts), "Indices must have same number of elements as tuple types!"); + static_assert(sizeof...(Ts) > 0, "Cannot insert empty tuple into stream."); + size_t last = sizeof...(Ts) - 1; // assuming index sequence 0,...,N-1 + + return ((os << std::get(tuple) << (Is != last ? "\r\n" : "")), ...); +} +} // namespace detail + +template std::ostream &operator<<(std::ostream &os, const std::tuple &tuple) +{ + return detail::println_tuple_impl(os, tuple, std::index_sequence_for{}); +} + +template std::ostream &operator<<(std::ostream &os, const std::array &arr) +{ + size_t last = N - 1; + for (std::size_t i = 0; i < N; ++i) { + os << arr[i] << (i != last ? "\r\n" : ""); + } + return os; +} \ No newline at end of file diff --git a/vortex-filtering/include/vortex_filtering/vortex_filtering.hpp b/vortex-filtering/include/vortex_filtering/vortex_filtering.hpp index 5445e23a..864de845 100644 --- a/vortex-filtering/include/vortex_filtering/vortex_filtering.hpp +++ b/vortex-filtering/include/vortex_filtering/vortex_filtering.hpp @@ -24,10 +24,16 @@ // Numerical Integration #include -// Utils -#include -#include - // Probability #include #include + +// Types +#include +#include +#include + +// Utils +#include +#include +#include diff --git a/vortex-filtering/test/CMakeLists.txt b/vortex-filtering/test/CMakeLists.txt index 07a1f630..14d34b09 100644 --- a/vortex-filtering/test/CMakeLists.txt +++ b/vortex-filtering/test/CMakeLists.txt @@ -2,7 +2,6 @@ find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(${PROJECT_NAME}_test gtest_main.cpp gtest_assertions.cpp - ../src/utils/plotting.cpp ../src/utils/ellipse.cpp @@ -12,6 +11,7 @@ ament_add_gtest(${PROJECT_NAME}_test numerical_integration_test.cpp probability_test.cpp sensor_model_test.cpp + types_test.cpp ukf_test.cpp ) target_include_directories(${PROJECT_NAME}_test PUBLIC diff --git a/vortex-filtering/test/dynamic_model_test.cpp b/vortex-filtering/test/dynamic_model_test.cpp index 2a71c498..4ea02e1f 100644 --- a/vortex-filtering/test/dynamic_model_test.cpp +++ b/vortex-filtering/test/dynamic_model_test.cpp @@ -12,8 +12,8 @@ namespace simple_dynamic_model_test { -using Vec_x = typename SimpleDynamicModel::Vec_x; -using Mat_xx = typename SimpleDynamicModel::Mat_xx; +using Vec_x = typename SimpleDynamicModel::T::Vec_x; +using Mat_xx = typename SimpleDynamicModel::T::Mat_xx; TEST(DynamicModel, initSimpleModel) { SimpleDynamicModel model; } @@ -51,7 +51,7 @@ TEST(DynamicModel, sampleSimpleModel) EXPECT_TRUE(isApproxEqual(approx_gauss.mean(), true_gauss.mean(), 0.1)); EXPECT_TRUE(isApproxEqual(true_gauss.cov(), true_gauss.cov(), 0.1)); - #ifdef GNUPLOT_ENABLE + #if (GNUPLOT_ENABLE) // Plot Gnuplot gp; gp << "set xrange [-10:10]\nset yrange [-10:10]\n"; @@ -69,9 +69,9 @@ TEST(DynamicModel, sampleSimpleModel) } // namespace simple_dynamic_model_test namespace cv_model_test { -using CVModel = vortex::models::ConstantVelocity<2>; -using Vec_x = typename CVModel::Vec_x; -using Mat_xx = typename CVModel::Mat_xx; +using CVModel = vortex::models::ConstantVelocity; +using Vec_x = typename CVModel::T::Vec_x; +using Mat_xx = typename CVModel::T::Mat_xx; TEST(DynamicModel, initCVModel) { CVModel model(1.0); } diff --git a/vortex-filtering/test/ekf_test.cpp b/vortex-filtering/test/ekf_test.cpp index 5b04eb41..c6144fce 100644 --- a/vortex-filtering/test/ekf_test.cpp +++ b/vortex-filtering/test/ekf_test.cpp @@ -13,24 +13,23 @@ class EKFTestCVModel : public ::testing::Test { protected: using PosMeasModel = vortex::models::IdentitySensorModel<4, 2>; - using CVModel = vortex::models::ConstantVelocity<2>; - using DynModI = CVModel::DynModI; - using Vec_x = typename CVModel::Vec_x; - using Vec_u = typename CVModel::Vec_u; - using Mat_xx = typename CVModel::Mat_xx; - using Gauss_x = typename CVModel::Gauss_x; - using Gauss_z = typename PosMeasModel::Gauss_z; - using Vec_z = typename PosMeasModel::Vec_z; + using CVModel = vortex::models::ConstantVelocity; + using Vec_x = typename CVModel::T::Vec_x; + using Mat_xx = typename CVModel::T::Mat_xx; + using Gauss_x = typename CVModel::T::Gauss_x; + using Gauss_z = typename PosMeasModel::T::Gauss_z; + using Vec_z = typename PosMeasModel::T::Vec_z; + using Vec_u = typename CVModel::T::Vec_u; using EKF = vortex::filter::EKF; - void SetUp() override + EKFTestCVModel() + : dynamic_model_(1e-2) + , sensor_model_(1e-2) { - dynamic_model_ = std::make_shared(1e-3); - sensor_model_ = std::make_shared(1e-2); } - std::shared_ptr dynamic_model_; - std::shared_ptr sensor_model_; + CVModel dynamic_model_; + PosMeasModel sensor_model_; }; TEST_F(EKFTestCVModel, predict) @@ -71,8 +70,8 @@ TEST_F(EKFTestCVModel, convergence) x_est.push_back(x0); for (int i = 0; i < 100; i++) { // Simulate - Vec_x x_true_i = dynamic_model_->sample_f_d(dt, x_true.back(), Vec_u::Zero(), gen); - Vec_z z_meas_i = sensor_model_->sample_h(x_true_i, gen); + Vec_x x_true_i = dynamic_model_.sample_f_d(dt, x_true.back(), Vec_u::Zero(), gen); + Vec_z z_meas_i = sensor_model_.sample_h(x_true_i, gen); x_true.push_back(x_true_i); z_meas.push_back(z_meas_i); @@ -104,7 +103,7 @@ TEST_F(EKFTestCVModel, convergence) } time.pop_back(); - #ifdef GNUPLOT_ENABLE + #if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set terminal qt size 1600,1000\n"; // Modified to make plot larger gp << "set multiplot layout 2,1\n"; diff --git a/vortex-filtering/test/gtest_assertions.cpp b/vortex-filtering/test/gtest_assertions.cpp index 11bc9477..9afd09f1 100644 --- a/vortex-filtering/test/gtest_assertions.cpp +++ b/vortex-filtering/test/gtest_assertions.cpp @@ -1,5 +1,11 @@ #include "gtest_assertions.hpp" +/** Check if two matrices are approximately equal + * @param a matrix a + * @param b matrix b + * @param tol tolerance + * @return testing::AssertionResult + */ testing::AssertionResult isApproxEqual(const Eigen::MatrixXd &a, const Eigen::MatrixXd &b, double tol) { if (a.rows() != b.rows() || a.cols() != b.cols()) { diff --git a/vortex-filtering/test/imm_test.cpp b/vortex-filtering/test/imm_test.cpp index 53d8cddc..a5a4c343 100644 --- a/vortex-filtering/test/imm_test.cpp +++ b/vortex-filtering/test/imm_test.cpp @@ -1,68 +1,71 @@ +#include "gtest_assertions.hpp" + #include #include #include #include - -#include -#include - #include +#include #include - -#include "gtest_assertions.hpp" +#include +#include /////////////////////////////// // IMM Model Tests /////////////////////////////// -TEST(ImmModel, init) +TEST(ImmModel, initWithStateNames) { - using namespace vortex::models; + using vortex::models::ConstantPosition; + using vortex::models::ConstantVelocity; - Eigen::Matrix2d jump_mat; - jump_mat << 0, 1, 1, 0; - Eigen::Vector2d hold_times; - hold_times << 1, 1; - double std = 1.0; - - double dt; + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; - using DynMod2d = IdentityDynamicModel<2>; - using DynMod3d = IdentityDynamicModel<3>; + double std = 1.0; + double dt = 1.0; - ImmModel imm_model(jump_mat, hold_times, DynMod2d(std), DynMod3d(std)); + 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}}}; - EXPECT_EQ(typeid(*imm_model.get_model<0>()), typeid(IdentityDynamicModel<2>)); - EXPECT_EQ(typeid(*imm_model.get_model<1>()), typeid(IdentityDynamicModel<3>)); + EXPECT_EQ(typeid(imm_model.get_model<0>()), typeid(ConstantPosition)); + EXPECT_EQ(typeid(imm_model.get_model<1>()), typeid(ConstantVelocity)); EXPECT_EQ(typeid(imm_model.f_d<0>(dt, Eigen::Vector2d::Zero())), typeid(Eigen::Vector2d)); - EXPECT_EQ(typeid(imm_model.f_d<1>(dt, Eigen::Vector3d::Zero())), typeid(Eigen::Vector3d)); - EXPECT_EQ(typeid(imm_model.Q_d<0>(dt, Eigen::Vector2d::Zero())), typeid(Eigen::Matrix2d)); - EXPECT_EQ(typeid(imm_model.Q_d<1>(dt, Eigen::Vector3d::Zero())), typeid(Eigen::Matrix3d)); + EXPECT_EQ(typeid(imm_model.f_d<1>(dt, Eigen::Vector4d::Zero())), typeid(Eigen::Vector4d)); } TEST(ImmModel, piMatC) { - using namespace vortex::models; + using vortex::models::ConstantPosition; + using vortex::models::ConstantVelocity; - Eigen::Matrix3d jump_mat; // clang-format off - jump_mat << 0, 1.0/2, 1.0/2, - 1.0/3, 0, 2.0/3, - 5.0/6, 1.0/6, 0; + Eigen::Matrix3d jump_mat{ + {0 , 1.0/2, 1.0/2}, + {1.0/3, 0 , 2.0/3}, + {5.0/6, 1.0/6, 0 } + }; // clang-format on - Eigen::Vector3d hold_times; - hold_times << 6, 12, 18; + + Eigen::Vector3d hold_times{1.0 / 6, 1.0 / 12, 1.0 / 18}; double std = 1.0; - using DynMod2d = IdentityDynamicModel<2>; + 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}}; - ImmModel imm_model(jump_mat, hold_times, DynMod2d(std), DynMod2d(std), DynMod2d(std)); + ImmModelT imm_model(jump_mat, hold_times, ConstantPosition(std), ConstantPosition(std), ConstantPosition(std), state_names); - Eigen::Matrix3d pi_mat_c; // clang-format off - pi_mat_c << -6, 3, 3, - 4, -12, 8, - 15, 3, -18; + Eigen::Matrix3d pi_mat_c{ + {-6, 3 , 3 }, + {4 , -12, 8 }, + {15, 3 , -18} + }; // clang-format on EXPECT_EQ(imm_model.get_pi_mat_c(), pi_mat_c); @@ -70,20 +73,24 @@ TEST(ImmModel, piMatC) TEST(ImmModel, piMatD) { - using namespace vortex::models; + using vortex::models::ConstantPosition; + using vortex::models::ConstantVelocity; - Eigen::Matrix3d jump_mat; // clang-format off - jump_mat << 0, 1.0/2, 1.0/2, - 1.0/3, 0, 2.0/3, - 5.0/6, 1.0/6, 0; + Eigen::Matrix3d jump_mat{ + {0 , 1.0/2, 1.0/2}, + {1.0/3, 0 , 2.0/3}, + {5.0/6, 1.0/6, 0 } + }; // clang-format on - Eigen::Vector3d hold_times; - hold_times << 6, 12, 18; + Eigen::Vector3d hold_times{1.0 / 6, 1.0 / 12, 1.0 / 18}; double std = 1.0; - using IMM = ImmModel, IdentityDynamicModel<2>, IdentityDynamicModel<2>>; - IMM imm_model(jump_mat, hold_times, {std}, {std}, {std}); + 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); Eigen::Matrix3d pi_mat_d_true; // clang-format off @@ -104,10 +111,13 @@ TEST(ImmModel, piMatD) TEST(ImmModel, stateSize) { - using namespace vortex::models; - using TestModel = ImmModel, IdentityDynamicModel<2>, IdentityDynamicModel<4>>; + using vortex::models::ConstantAcceleration; + using vortex::models::ConstantPosition; + using vortex::models::ConstantVelocity; - EXPECT_EQ(TestModel::get_n_dim_x(), (std::array{3, 2, 4})); + using TestModel = vortex::models::ImmModel; + + EXPECT_EQ(TestModel::N_DIMS_x, (std::array{2, 4, 6})); } /////////////////////////////// @@ -116,76 +126,130 @@ TEST(ImmModel, stateSize) TEST(ImmFilter, init) { - using DynModT = vortex::models::IdentityDynamicModel<2>; - using SensModT = vortex::models::IdentitySensorModel<2, 1>; - using ImmModelT = vortex::models::ImmModel; + using vortex::models::ConstantPosition; + using SensModT = vortex::models::IdentitySensorModel<2, 1>; + using ImmModelT = vortex::models::ImmModel; + using ImmFilterT = vortex::filter::ImmFilter; - vortex::filter::ImmFilter imm_filter; + EXPECT_EQ(ImmFilterT::N_MODELS, 2u); + EXPECT_EQ(ImmFilterT::N_DIM_z, SensModT::N_DIM_z); } TEST(ImmFilter, calculateMixingProbs) { - using namespace vortex::models; - using namespace vortex::filter; - using namespace vortex::prob; + using vortex::models::ConstantPosition; - double dt = 1.0; + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; - Eigen::Matrix2d jump_mat; - jump_mat << 0, 1, 1, 0; - Eigen::Vector2d hold_times; - hold_times << 1, 1; + double dt = 1.0; double std = 1.0; - using IMM = ImmModel, IdentityDynamicModel<2>>; - IMM imm_model(jump_mat, hold_times, {std}, {std}); + 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}}); - auto sensor_model = std::make_shared>(dt); + auto sensor_model = std::make_shared>(dt); - ImmFilter, IMM> imm_filter; + using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; - Eigen::Vector2d model_weights; + Eigen::Vector2d model_weights = {0.5, 0.5}; - model_weights << 0.5, 0.5; // Since the weights are equal, the mixing probabilities should be equal to the discrete time Markov chain Eigen::Matrix2d mixing_probs_true = imm_model.get_pi_mat_d(dt); - Eigen::Matrix2d mixing_probs = imm_filter.calculate_mixing_probs(imm_model, model_weights, dt); + Eigen::Matrix2d mixing_probs = ImmFilterT::calculate_mixing_probs(imm_model.get_pi_mat_d(dt), model_weights); EXPECT_EQ(isApproxEqual(mixing_probs, mixing_probs_true, 1e-6), true); // When all of the weight is in the first model, the probability that the previous model was the second model should be 0 - model_weights << 1, 0; - mixing_probs_true << 1, 1, 0, 0; - mixing_probs = imm_filter.calculate_mixing_probs(imm_model, model_weights, dt); + model_weights = {1, 0}; + mixing_probs_true = Eigen::Matrix2d{{1, 1}, {0, 0}}; + + mixing_probs = ImmFilterT::calculate_mixing_probs(imm_model.get_pi_mat_d(dt), model_weights); EXPECT_EQ(isApproxEqual(mixing_probs, mixing_probs_true, 1e-6), true); } -TEST(ImmFilter, mixing) +TEST(ImmFilter, mixing_two_of_the_same_model) +{ + using vortex::models::ConstantPosition; + using vortex::prob::Gauss2d; + + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; + + double dt = 1; + 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}}); + + auto sensor_model = std::make_shared>(dt); + + using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; + + Eigen::Vector2d model_weights{0.5, 0.5}; + + Gauss2d x_est_prev_1{Gauss2d::Standard()}; + Gauss2d x_est_prev_2{{1, 0}, Eigen::Matrix2d::Identity() * 100}; + + 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()); + + // 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 + 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; + std::cout << "x_est_1:\n" << x_est_1 << std::endl; + std::cout << "x_est_2:\n" << x_est_2 << std::endl; + + EXPECT_LT((x_est_2.mean() - x_est_prev_2.mean()).norm(), (x_est_1.mean() - x_est_prev_1.mean()).norm()); +} + +TEST(ImmFilter, mixing_two_different_models) { using namespace vortex::models; using namespace vortex::filter; using namespace vortex::prob; - double dt = 1; + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; - Eigen::Matrix2d jump_mat; - jump_mat << 0, 1, 1, 0; - Eigen::Vector2d hold_times; - hold_times << 1, 1; - double high_std = 10; - double low_std = 0.1; + using ImmModelT = ImmModel; + using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; - using IMM = ImmModel, IdentityDynamicModel<2>>; - IMM imm_model(jump_mat, hold_times, {high_std}, {low_std}); + double dt = 1; + double std_pos = 0.1; + double std_vel = 0.1; - auto sensor_model = std::make_shared>(dt); + 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}}}; - ImmFilter, IMM> imm_filter; + auto sensor_model = std::make_shared>(dt); - Eigen::Vector2d model_weights; - model_weights << 0.5, 0.5; + Gauss2d x_est_prev_1{Gauss2d::Standard()}; + Gauss4d x_est_prev_2{{1, 0, 0, 0}, Eigen::Matrix4d::Identity() * 100}; + + 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}} + }; + // 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); - std::vector x_est_prevs = {Gauss2d::Standard(), Gauss2d::Standard()}; - std::vector moment_based_approx = imm_filter.mixing(x_est_prevs, imm_model.get_pi_mat_d(dt)); + 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; + std::cout << "x_est_1:\n" << x_est_1 << std::endl; + std::cout << "x_est_2:\n" << x_est_2 << std::endl; + + // EXPECT_LT((x_est_2.mean().head<2>() - x_est_prev_2.mean().head<2>()).norm(), (x_est_1.mean() - x_est_prev_1).norm()); } TEST(ImmFilter, modeMatchedFilter) @@ -194,38 +258,42 @@ TEST(ImmFilter, modeMatchedFilter) using namespace vortex::filter; using namespace vortex::prob; - // auto const_pos = std::make_shared>(0.1); - // auto const_vel = std::make_shared>(0.1); - - double dt = 1; + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; - Eigen::Matrix2d jump_mat; - jump_mat << 0, 1, 1, 0; - Eigen::Vector2d hold_times; - hold_times << 1, 1; - - using IMM = ImmModel, ConstantVelocity<1>>; - using IMMFilter = ImmFilter, IMM>; + using ImmModelT = ImmModel; + using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; + double dt = 1; double std_pos = 0.1; double std_vel = 0.1; - IMM imm_model(jump_mat, hold_times, {std_pos}, {std_vel}); + 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}}}; - auto sensor_model = std::make_shared>(dt); + IdentitySensorModel<2, 2> sensor_model{dt}; - IMMFilter imm_filter; + std::tuple x_est_prevs = {Gauss2d::Standard(), {{0, 0, 0.9, 0}, Eigen::Matrix4d::Identity()}}; + Eigen::Vector2d z_meas = {1, 0}; - std::vector x_est_prevs = {Gauss2d::Standard(), {{0, 1}, Eigen::Matrix2d::Identity()}}; - Eigen::Vector2d z_meas = {1, 1}; + auto [x_est_upds, x_est_preds, z_est_preds] = ImmFilterT::mode_matched_filter(imm_model, sensor_model, dt, x_est_prevs, z_meas); - auto [x_est_upd, x_est_pred, z_est_pred] = imm_filter.mode_matched_filter(imm_model, sensor_model, dt, x_est_prevs, z_meas); + std::cout << "x_est_upds:\n" << x_est_upds << std::endl; + std::cout << "x_est_preds:\n" << x_est_preds << std::endl; + std::cout << "z_est_preds:\n" << z_est_preds << std::endl; - EXPECT_EQ(IMM::N_MODELS, x_est_upd.size()); + EXPECT_EQ(ImmFilterT::N_MODELS, std::tuple_size::value); + EXPECT_EQ(ImmFilterT::N_MODELS, std::tuple_size::value); + EXPECT_EQ(ImmFilterT::N_MODELS, z_est_preds.size()); // Expect the second filter to predict closer to the measurement - EXPECT_FALSE(isApproxEqual(z_est_pred.at(0).mean(), z_meas, 0.1)); - EXPECT_TRUE(isApproxEqual(z_est_pred.at(1).mean(), z_meas, 0.1)); + double dist_to_meas_0 = (std::get<0>(x_est_preds).mean().head<2>() - z_meas).norm(); + double dist_to_meas_1 = (std::get<1>(x_est_preds).mean().head<2>() - z_meas).norm(); + + EXPECT_LT(dist_to_meas_1, dist_to_meas_0); } TEST(ImmFilter, updateProbabilities) @@ -234,35 +302,90 @@ TEST(ImmFilter, updateProbabilities) using namespace vortex::filter; using namespace vortex::prob; - auto const_pos = std::make_shared>(1); - auto const_vel = std::make_shared>(1); - double dt = 1; - Eigen::Matrix2d jump_mat; - jump_mat << 0, 1, 1, 0; - Eigen::Vector2d hold_times; - hold_times << 1, 1; + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; double std_pos = 1; double std_vel = 1; - using IMM = ImmModel, ConstantVelocity<1>>; - using IMMFilter = ImmFilter, IMM>; + using ImmModelT = ImmModel; + using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; - IMM imm_model(jump_mat, hold_times, {std_pos}, {std_vel}); + 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}}}; auto sensor_model = std::make_shared>(dt); - IMMFilter imm_filter; - Eigen::Vector2d model_weights; model_weights << 0.5, 0.5; Eigen::Vector2d z_meas = {1, 1}; - std::vector z_preds = {Gauss2d::Standard(), {{1, 1}, Eigen::Matrix2d::Identity()}}; + std::array z_preds = {Gauss2d::Standard(), {{1, 1}, Eigen::Matrix2d::Identity()}}; - Eigen::Vector2d upd_weights = imm_filter.update_probabilities(imm_model, dt, z_preds, z_meas, model_weights); + Eigen::Vector2d upd_weights = ImmFilterT::update_probabilities(imm_model.get_pi_mat_d(dt), z_preds, z_meas, model_weights); EXPECT_GT(upd_weights(1), upd_weights(0)); +} + +TEST(ImmFilter, step) +{ + using namespace vortex::models; + using namespace vortex::filter; + using namespace vortex::prob; + + double dt = 1; + // clang-format off + Eigen::Matrix3d jump_mat{ + {0, 0.5, 0.5}, + {0.5, 0, 0.5}, + {0.5, 0.5, 0}}; + // clang-format on + Eigen::Vector3d hold_times{10, 10, 10}; + double std_pos = 0.1; + double std_vel = 0.1; + double std_turn_rate = 0.1; + + using ImmModelT = ImmModel; + using ImmFilterT = vortex::filter::ImmFilter, ImmModelT>; + + 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}}; + + IdentitySensorModel<2, 2> sensor_model{dt}; + + Eigen::Vector3d model_weights{1 / 3.0, 1 / 3.0, 1 / 3.0}; + + std::tuple x_est_prevs = { + 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}}}; + + 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); + + EXPECT_EQ(ImmFilterT::N_MODELS, std::tuple_size::value); + EXPECT_EQ(ImmFilterT::N_MODELS, std::tuple_size::value); + EXPECT_EQ(ImmFilterT::N_MODELS, z_est_preds.size()); + EXPECT_EQ(ImmFilterT::N_MODELS, weights_upd.size()); + + for (int i = 2; i < 50; i++) { + z_meas << i, 0; + std::tie(weights_upd, x_est_upds, std::ignore, std::ignore) = + ImmFilterT::step(imm_model, sensor_model, dt, x_est_upds, z_meas, weights_upd, states_min_max); + } + + std::cout << "weights_upd:\n" << weights_upd << std::endl; + std::cout << "x_est_upds:\n" << x_est_upds << std::endl; + + // Expect the constant velocity model to have the highest probability + EXPECT_GT(weights_upd(1), weights_upd(0)); + EXPECT_GT(weights_upd(1), weights_upd(2)); } \ No newline at end of file diff --git a/vortex-filtering/test/numerical_integration_test.cpp b/vortex-filtering/test/numerical_integration_test.cpp index 6c07b5aa..7f738467 100644 --- a/vortex-filtering/test/numerical_integration_test.cpp +++ b/vortex-filtering/test/numerical_integration_test.cpp @@ -76,7 +76,7 @@ class NumericalIntegration : public ::testing::Test { void plot_result(std::string title = "ERK convergence") { - #ifdef GNUPLOT_ENABLE + #if (GNUPLOT_ENABLE) // Plot first state and true solution against time Gnuplot gp; gp << "set terminal wxt size 1200,800\n"; diff --git a/vortex-filtering/test/probability_test.cpp b/vortex-filtering/test/probability_test.cpp index 29cabc82..af966edd 100644 --- a/vortex-filtering/test/probability_test.cpp +++ b/vortex-filtering/test/probability_test.cpp @@ -94,7 +94,7 @@ TEST(MultiVarGauss, sample) double minorAxisLength = cov_ellipse.minor_axis(); double angle = cov_ellipse.angle_deg(); - #ifdef GNUPLOT_ENABLE + #if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set xrange [-10:10]\nset yrange [-10:10]\n"; gp << "set style circle radius 0.05\n"; @@ -123,4 +123,121 @@ TEST(MultiVarGauss, mahalanobisDistanceIdentityCovariance) EXPECT_DOUBLE_EQ(gaussian.mahalanobis_distance({1, 0}), 1); EXPECT_DOUBLE_EQ(gaussian.mahalanobis_distance({0, 1}), 1); EXPECT_DOUBLE_EQ(gaussian.mahalanobis_distance({1, 1}), std::sqrt(2)); -} \ No newline at end of file +} + +TEST(isContainerConcept, compileTimeChecks) +{ + static_assert(vortex::prob::concepts::is_container, double>); + static_assert(vortex::prob::concepts::is_container, double>); + static_assert(vortex::prob::concepts::is_container, vortex::prob::Gauss2d>); + static_assert(vortex::prob::concepts::is_container); + static_assert(vortex::prob::concepts::is_container); + static_assert(vortex::prob::concepts::is_container); + + static_assert(!vortex::prob::concepts::is_container); + + EXPECT_TRUE(true); +} + +TEST(GaussianMixture, defaultConstructor) +{ + vortex::prob::GaussianMixture<2> mixture; + + EXPECT_EQ(mixture.size(), 0u); +} + +TEST(GaussianMixture, stdVectorConstructor) +{ + using vortex::prob::Gauss2d; + std::vector weights{1, 2}; + std::vector gaussians{Gauss2d::Standard(), Gauss2d::Standard()}; + + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + + EXPECT_EQ(mixture.size(), 2u); + + Eigen::VectorXd weights_eigen(2); + weights_eigen << 1, 2; + EXPECT_EQ(mixture.weights(), weights_eigen); + EXPECT_EQ(mixture.gaussians(), gaussians); +} + +TEST(GaussianMixture, stdArrayConstructor) +{ + using vortex::prob::Gauss2d; + std::array weights{1, 2}; + std::array gaussians{Gauss2d::Standard(), Gauss2d::Standard()}; + + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + + EXPECT_EQ(mixture.size(), 2u); + + Eigen::VectorXd weights_eigen(2); + weights_eigen << 1, 2; + EXPECT_EQ(mixture.weights(), weights_eigen); + EXPECT_EQ(mixture.gaussians().at(0), gaussians.at(0)); + EXPECT_EQ(mixture.gaussians().at(1), gaussians.at(1)); +} + +TEST(GaussianMixture, eigenVectorConstructor) +{ + using vortex::prob::Gauss2d; + Eigen::VectorXd weights(2); + weights << 1, 2; + std::vector gaussians{Gauss2d::Standard(), Gauss2d::Standard()}; + + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + + EXPECT_EQ(mixture.size(), 2u); + EXPECT_EQ(mixture.weights(), weights); + EXPECT_EQ(mixture.gaussians().at(0), gaussians.at(0)); + EXPECT_EQ(mixture.gaussians().at(1), gaussians.at(1)); +} + +TEST(GaussianMixture, mixTwoEqualWeightEqualCovarianceComponents) +{ + using vortex::prob::Gauss2d; + std::vector weights{0.5, 0.5}; + + Gauss2d gaussian1{{0, 0}, Eigen::Matrix2d::Identity()}; + Gauss2d gaussian2{{10, 0}, Eigen::Matrix2d::Identity()}; + std::vector gaussians{gaussian1, gaussian2}; + + Eigen::Vector2d center{5, 0}; + + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + + EXPECT_TRUE(isApproxEqual(mixture.reduce().mean(), center, 1e-15)); +} + +TEST(GaussianMixture, mixTwoEqualWeightDifferentCovarianceComponents) +{ + using vortex::prob::Gauss2d; + std::vector weights{0.5, 0.5}; + + Gauss2d gaussian1{{0, 0}, Eigen::Matrix2d::Identity()}; + Gauss2d gaussian2{{10, 0}, Eigen::Matrix2d::Identity() * 2}; + std::vector gaussians{gaussian1, gaussian2}; + + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + + Eigen::Vector2d center{5, 0}; + + EXPECT_TRUE(isApproxEqual(mixture.reduce().mean(), center, 1e-15)); +} + +TEST(GaussianMixture, mixTwoDifferentWeightEqualCovarianceComponents) +{ + using vortex::prob::Gauss2d; + std::vector weights{0.25, 0.75}; + + Gauss2d gaussian1{{0, 0}, Eigen::Matrix2d::Identity()}; + Gauss2d gaussian2{{10, 0}, Eigen::Matrix2d::Identity()}; + std::vector gaussians{gaussian1, gaussian2}; + + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + + Eigen::Vector2d center{7.5, 0}; + + EXPECT_TRUE(isApproxEqual(mixture.reduce().mean(), center, 1e-15)); +} diff --git a/vortex-filtering/test/sensor_model_test.cpp b/vortex-filtering/test/sensor_model_test.cpp index b9fe60d7..5986f3f1 100644 --- a/vortex-filtering/test/sensor_model_test.cpp +++ b/vortex-filtering/test/sensor_model_test.cpp @@ -1,21 +1,25 @@ #include "test_models.hpp" + #include #include #include #include +#include namespace simple_sensor_model_test { using SensorModel = vortex::models::IdentitySensorModel<2, 1>; -using Measurement = typename SensorModel::Vec_z; -using Vec_x = typename SensorModel::Vec_x; -using Vec_z = typename SensorModel::Vec_z; -using Mat_xx = typename SensorModel::Mat_xx; -using Mat_zz = typename SensorModel::Mat_zz; -using Mat_zx = typename SensorModel::Mat_zx; -using Mat_xz = typename SensorModel::Mat_xz; -using Gauss_x = typename SensorModel::Gauss_x; -using Gauss_z = typename SensorModel::Gauss_z; + +using T = vortex::Types_xz; + +using Vec_x = T::Vec_x; +using Vec_z = T::Vec_z; +using Mat_xx = T::Mat_xx; +using Mat_zz = T::Mat_zz; +using Mat_zx = T::Mat_zx; +using Mat_xz = T::Mat_xz; +using Gauss_x = T::Gauss_x; +using Gauss_z = T::Gauss_z; TEST(SensorModel, initSimpleModel) { diff --git a/vortex-filtering/test/test_models.hpp b/vortex-filtering/test/test_models.hpp index 37db91b7..0aef6c4c 100644 --- a/vortex-filtering/test/test_models.hpp +++ b/vortex-filtering/test/test_models.hpp @@ -1,63 +1,84 @@ #pragma once #include #include +#include +#include -class SimpleDynamicModel : public vortex::models::interface::DynamicModelCTLTV<2> { +using vortex::models::interface::DynamicModel; +using vortex::models::interface::DynamicModelCTLTV; +class SimpleDynamicModel : public DynamicModelCTLTV<2> { public: - using DynModI = vortex::models::interface::DynamicModel<2>; - using typename DynModI::Mat_xx; - using typename DynModI::Vec_x; - constexpr static int N_DIM_x = DynModI::N_DIM_x; + constexpr static int N_DIM_x = DynamicModel<2>::N_DIM_x; + constexpr static int N_DIM_u = DynamicModel<2>::N_DIM_u; + constexpr static int N_DIM_v = DynamicModel<2>::N_DIM_v; - Mat_xx A_c(const Vec_x & = Vec_x::Zero()) const override { return -Mat_xx::Identity(); } + using T = vortex::Types_xuv; - Mat_vv Q_c(const Vec_x & = Vec_x::Zero()) const override { return Mat_xx::Identity(); } + T::Mat_xx A_c(const T::Vec_x & = T::Vec_x::Zero()) const override { return -T::Mat_xx::Identity(); } + + T::Mat_vv Q_c(const T::Vec_x & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity(); } }; -class NonlinearModel1 : public vortex::models::interface::DynamicModel<1, 1, 1> { +class NonlinearModel1 : public DynamicModel<1, 1, 1> { public: - using typename DynamicModel<1, 1, 1>::Vec_x; - using typename DynamicModel<1, 1, 1>::Mat_xx; - using typename DynamicModel<1, 1, 1>::Mat_xv; - using typename DynamicModel<1, 1, 1>::Vec_v; + constexpr static int N_DIM_x = DynamicModel<1, 1, 1>::N_DIM_x; + constexpr static int N_DIM_u = DynamicModel<1, 1, 1>::N_DIM_u; + constexpr static int N_DIM_v = DynamicModel<1, 1, 1>::N_DIM_v; - NonlinearModel1(double std_dev) : cov_(std_dev * std_dev) {} + using T = vortex::Types_xuv; - Vec_x f_d(double, const Vec_x &x, const Vec_u & = Vec_u::Zero(), const Vec_v &v = Vec_v::Zero()) const override + NonlinearModel1(double std_dev) + : cov_(std_dev * std_dev) { - Vec_x x_next; + } + + T::Vec_x f_d(double, const T::Vec_x &x, const T::Vec_u & = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const override + { + typename T::Vec_x x_next; x_next << std::sin(x(0)) + v(0); return x_next; } - Mat_vv Q_d(double = 0.0, const Vec_x & = Vec_x::Zero()) const override { return Mat_xx::Identity() * cov_; } + T::Mat_vv Q_d(double = 0.0, const T::Vec_x & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity() * cov_; } private: const double cov_; }; // https://en.wikipedia.org/wiki/Lorenz_system -class LorenzAttractorCT : public vortex::models::interface::DynamicModelCT<3> { +class LorenzAttractorCT : public DynamicModel<3> { public: - using DynModI = vortex::models::interface::DynamicModel<3>; - using typename DynModI::Vec_u; - using typename DynModI::Vec_v; - using typename DynModI::Vec_x; + static constexpr int N_DIM_x = DynamicModel<3>::N_DIM_x; + static constexpr int N_DIM_u = DynamicModel<3>::N_DIM_u; + static constexpr int N_DIM_v = DynamicModel<3>::N_DIM_v; - using typename DynModI::Mat_vv; - using typename DynModI::Mat_xx; + using T = vortex::Types_xuv; - LorenzAttractorCT(double std_dev) : cov_(std_dev * std_dev), sigma_(10.0), beta_(8.0 / 3.0), rho_(28.0) {} + LorenzAttractorCT(double std_dev) + : cov_(std_dev * std_dev) + , sigma_(10.0) + , beta_(8.0 / 3.0) + , rho_(28.0) + { + } - Vec_x f_c(const Vec_x &x, const Vec_u & = Vec_u::Zero(), const Vec_v &v = Vec_v::Zero()) const override + T::Vec_x f_c(const T::Vec_x &x, const T::Vec_u & = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const { - Vec_x x_next; + typename T::Vec_x x_next; x_next << sigma_ * (x(1) - x(0)), x(0) * (rho_ - x(2)) - x(1), x(0) * x(1) - beta_ * x(2); x_next += v; return x_next; } - Mat_vv Q_d(double dt, const Vec_x & = Vec_x::Zero()) const override { return Mat_xx::Identity() * cov_ * dt; } + T::Vec_x f_d(double dt, const T::Vec_x &x, const T::Vec_u &u = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const override + { + using Dyn_mod_func = std::function; + + Dyn_mod_func f_c = [this, &u, &v](double, const T::Vec_x &x) { return this->f_c(x, u, v); }; + return vortex::integrator::RK4::integrate(f_c, dt, x); + } + + T::Mat_vv Q_d(double dt, const T::Vec_x & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity() * cov_ * dt; } private: const double cov_; @@ -68,20 +89,17 @@ class LorenzAttractorCT : public vortex::models::interface::DynamicModelCT<3> { class LorenzAttractorCTLTV : public vortex::models::interface::DynamicModelCTLTV<3> { public: - using DynModI = vortex::models::interface::DynamicModel<3>; - using typename DynModI::Vec_u; - using typename DynModI::Vec_v; - using typename DynModI::Vec_x; + static constexpr int N_DIM_x = DynamicModelCTLTV<3>::N_DIM_x; + static constexpr int N_DIM_u = DynamicModelCTLTV<3>::N_DIM_u; + static constexpr int N_DIM_v = DynamicModelCTLTV<3>::N_DIM_v; - using typename DynModI::Mat_vv; - using typename DynModI::Mat_xv; - using typename DynModI::Mat_xx; + using T = vortex::Types_xuv; LorenzAttractorCTLTV(double std_dev) : cov_(std_dev * std_dev), sigma_(10.0), beta_(8.0 / 3.0), rho_(28.0) {} - Mat_xx A_c(const Vec_x &x) const override + T::Mat_xx A_c(const T::Vec_x &x) const override { - Mat_xx A; + typename T::Mat_xx A; // clang-format off A << -sigma_ , sigma_, 0.0 , rho_-x(2), -1.0 , -x(0) , @@ -90,7 +108,7 @@ class LorenzAttractorCTLTV : public vortex::models::interface::DynamicModelCTLTV return A; } - Mat_vv Q_c(const Vec_x & = Vec_x::Zero()) const override { return Mat_xx::Identity() * cov_; } + T::Mat_vv Q_c(const T::Vec_x & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity() * cov_; } private: const double cov_; diff --git a/vortex-filtering/test/types_test.cpp b/vortex-filtering/test/types_test.cpp new file mode 100644 index 00000000..e470484c --- /dev/null +++ b/vortex-filtering/test/types_test.cpp @@ -0,0 +1,66 @@ +#include +#include +#include +#include +#include +#include +TEST(Types, x_2_z_1) +{ + using T = vortex::Types_xz<2, 1>; + T::Vec_x x; + T::Vec_z z; + + static_assert(std::is_same::value, "x is not a Vector2d"); + static_assert(std::is_same>::value, "z is not a Vector1d"); + + ASSERT_TRUE(true); +} + +#include +#include +#include + +TEST(Concepts, MatConvertibleTo) +{ + static_assert(std::convertible_to); + static_assert(vortex::concepts::mat_convertible_to); + static_assert(!vortex::concepts::mat_convertible_to); + + ASSERT_TRUE(true); +} + +TEST(Concepts, Models) +{ + constexpr size_t N_DIM_x = 5; + constexpr size_t N_DIM_z = 4; + constexpr size_t N_DIM_u = 3; + constexpr size_t N_DIM_v = 2; + constexpr size_t N_DIM_w = 1; + + using DynMod = vortex::models::interface::DynamicModel; + using DynModLTV = vortex::models::interface::DynamicModelLTV; + using SensMod = vortex::models::interface::SensorModel; + using SensModLTV = vortex::models::interface::SensorModelLTV; + + static_assert(vortex::concepts::DynamicModel); + static_assert(vortex::concepts::DynamicModelLTV); + static_assert(vortex::concepts::SensorModel); + static_assert(vortex::concepts::SensorModelLTV); + + static_assert(vortex::concepts::DynamicModelWithDefinedSizes); + static_assert(vortex::concepts::DynamicModelLTVWithDefinedSizes); + static_assert(vortex::concepts::SensorModelWithDefinedSizes); + static_assert(vortex::concepts::SensorModelLTVWithDefinedSizes); + + static_assert(vortex::concepts::DynamicModelWithDefinedSizes); + static_assert(!vortex::concepts::DynamicModelLTVWithDefinedSizes); + static_assert(vortex::concepts::SensorModelWithDefinedSizes); + static_assert(!vortex::concepts::SensorModelLTVWithDefinedSizes); + + static_assert(!vortex::concepts::DynamicModel); + static_assert(!vortex::concepts::DynamicModelLTV); + static_assert(!vortex::concepts::SensorModel); + static_assert(!vortex::concepts::SensorModelLTV); + + ASSERT_TRUE(true); +} \ No newline at end of file diff --git a/vortex-filtering/test/ukf_test.cpp b/vortex-filtering/test/ukf_test.cpp index 4abc72bc..9217f1f0 100644 --- a/vortex-filtering/test/ukf_test.cpp +++ b/vortex-filtering/test/ukf_test.cpp @@ -10,35 +10,37 @@ #include "test_models.hpp" #include #include +#include class UKFtest : public ::testing::Test { protected: - using Vec_x = typename NonlinearModel1::Vec_x; - using Mat_xx = typename NonlinearModel1::Mat_xx; - using Gauss_x = typename NonlinearModel1::Gauss_x; - using IdentitySensorModel = vortex::models::IdentitySensorModel<1, 1>; - using Vec_z = typename IdentitySensorModel::Vec_z; - using Mat_zz = typename IdentitySensorModel::Mat_zz; - using Gauss_z = typename IdentitySensorModel::Gauss_z; + static constexpr int N_DIM_x = NonlinearModel1::N_DIM_x; + static constexpr int N_DIM_z = IdentitySensorModel::N_DIM_z; + static constexpr int N_DIM_u = NonlinearModel1::N_DIM_u; + static constexpr int N_DIM_v = NonlinearModel1::N_DIM_v; + static constexpr int N_DIM_w = IdentitySensorModel::N_DIM_w; + + using T = vortex::Types_xzuvw; + + using Vec_x = typename T::Vec_x; + using Mat_xx = typename T::Mat_xx; + using Gauss_x = typename T::Gauss_x; + + using Vec_z = typename T::Vec_z; + using Mat_zz = typename T::Mat_zz; + using Gauss_z = typename T::Gauss_z; using UKF = vortex::filter::UKF; - void SetUp() override - { - // Noise - double Q = 0.1; - double R = 0.1; + static constexpr double Q = 0.1; + static constexpr double R = 0.1; + UKFtest() : dynamic_model_(Q), sensor_model_(R) {} - // Create dynamic model - dynamic_model_ = std::make_shared(Q); - // Create sensor model - sensor_model_ = std::make_shared(R); - } - std::shared_ptr dynamic_model_; - std::shared_ptr sensor_model_; + NonlinearModel1 dynamic_model_; + IdentitySensorModel sensor_model_; }; TEST_F(UKFtest, Predict) @@ -49,7 +51,7 @@ TEST_F(UKFtest, Predict) // Predict auto [x_est_pred, z_est_pred] = UKF::predict(dynamic_model_, sensor_model_, dt, x); - Vec_x x_true = dynamic_model_->f_d(dt, x.mean()); + Vec_x x_true = dynamic_model_.f_d(dt, x.mean()); Vec_z z_true = x_true.head(1); ASSERT_TRUE(isApproxEqual(x_est_pred.mean(), x_true, 1e-6)); ASSERT_TRUE(isApproxEqual(z_est_pred.mean(), z_true, 1e-6)); @@ -78,17 +80,17 @@ TEST_F(UKFtest, Convergence) time.push_back(0); x_est.push_back(x0); - z_meas.push_back(sensor_model_->h(x0.mean())); + z_meas.push_back(sensor_model_.h(x0.mean())); x_true.push_back(x0.mean()); - z_est.push_back({sensor_model_->h(x0.mean()), sensor_model_->R()}); + z_est.push_back({sensor_model_.h(x0.mean()), sensor_model_.R()}); for (int i = 0; i < n_steps - 1; i++) { // Simulate Vec_x v; v << d_disturbance(gen); Vec_z w = Vec_z::Zero(); w << d_noise(gen); - Vec_x x_true_i = dynamic_model_->f_d(dt, x_true.back(), Vec_x::Zero(), v); - Vec_z z_meas_i = sensor_model_->h(x_true_i) + w; + Vec_x x_true_i = dynamic_model_.f_d(dt, x_true.back(), Vec_x::Zero(), v); + Vec_z z_meas_i = sensor_model_.h(x_true_i) + w; x_true.push_back(x_true_i); z_meas.push_back(z_meas_i); @@ -115,7 +117,7 @@ TEST_F(UKFtest, Convergence) x_m_std.push_back(x_est.at(i).mean()(0) - std::sqrt(x_est.at(i).cov()(0, 0))); // x_est - std } - #ifdef GNUPLOT_ENABLE + #if (GNUPLOT_ENABLE) Gnuplot gp; gp << "set terminal wxt size 1200,800\n"; gp << "set title 'UKF convergence'\n";