diff --git a/vortex-filtering/include/vortex_filtering/filters/ekf.hpp b/vortex-filtering/include/vortex_filtering/filters/ekf.hpp index fd98a9e3..e1a73a07 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ekf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ekf.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include namespace vortex::filter { @@ -24,41 +25,12 @@ namespace vortex::filter { template class EKF { 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_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; - - using Gauss_x = prob::MultiVarGauss; - using Gauss_z = prob::MultiVarGauss; - using Gauss_v = prob::MultiVarGauss; - using Gauss_w = prob::MultiVarGauss; - - using DynModTPtr = std::shared_ptr; - using SensModTPtr = std::shared_ptr; + using T = Types_xzuvw; EKF() = delete; @@ -67,15 +39,15 @@ template Predicted state, predicted measurement + * @param u T::Vec_x Input. Not used, set to zero. + * @return std::pair 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 DynModT &dyn_mod, const SensModT &sens_mod, double dt, const Gauss_x &x_est_prev, - const Vec_u &u = Vec_u::Zero()) + static std::pair predict(const DynModT &dyn_mod, const SensModT &sens_mod, double dt, const T::Gauss_x &x_est_prev, + const T::Vec_u &u = T::Vec_u::Zero()) { - 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}; } @@ -83,25 +55,25 @@ template step(const DynModT &dyn_mod, const SensModT &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 DynModT &dyn_mod, const SensModT &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()) { 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}; } - [[deprecated("use const DynModT& and const SensModT&")]] static std::pair - predict(DynModTPtr dyn_mod, SensModTPtr sens_mod, double dt, const Gauss_x &x_est_prev, const Vec_u &u = Vec_u::Zero()) + [[deprecated("use const DynModT& and const SensModT&")]] static std::pair + predict(std::shared_ptr dyn_mod, std::shared_ptr sens_mod, double dt, const T::Gauss_x &x_est_prev, const T::Vec_u &u = T::Vec_u::Zero()) { return predict(*dyn_mod, *sens_mod, dt, x_est_prev, u); } - [[deprecated("use const SensModT&")]] static Gauss_x update(SensModTPtr sens_mod, const Gauss_x &x_est_pred, const Gauss_z &z_est_pred, const Vec_z &z_meas) + [[deprecated("use const SensModT&")]] static T::Gauss_x update(std::shared_ptr sens_mod, const T::Gauss_x &x_est_pred, const T::Gauss_z &z_est_pred, + const T::Vec_z &z_meas) { return update(*sens_mod, x_est_pred, z_est_pred, z_meas); } - [[deprecated("use const DynModT& and const SensModT&")]] static std::tuple - step(DynModTPtr dyn_mod, SensModTPtr sens_mod, double dt, const Gauss_x &x_est_prev, const Vec_z &z_meas, const Vec_u &u = Vec_u::Zero()) + [[deprecated("use const DynModT& and const SensModT&")]] static std::tuple + step(std::shared_ptr dyn_mod, std::shared_ptr 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()) { return step(*dyn_mod, *sens_mod, dt, x_est_prev, z_meas, u); } diff --git a/vortex-filtering/include/vortex_filtering/models/type_aliases.hpp b/vortex-filtering/include/vortex_filtering/models/type_aliases.hpp index 275bf4b6..6750b0c2 100644 --- a/vortex-filtering/include/vortex_filtering/models/type_aliases.hpp +++ b/vortex-filtering/include/vortex_filtering/models/type_aliases.hpp @@ -94,6 +94,8 @@ template