Skip to content

Commit

Permalink
Changed all except imm test
Browse files Browse the repository at this point in the history
  • Loading branch information
@EirikKolas
EirikKolas committed Mar 13, 2024
1 parent e360974 commit 18a4496
Show file tree
Hide file tree
Showing 15 changed files with 385 additions and 393 deletions.
1 change: 1 addition & 0 deletions vortex-filtering/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(
-Wall -Wextra -Wpedantic
-fopenmp # For parallel processing with Eigen
-fconcepts-diagnostics-depth=2 # For better concepts error messages
)
endif()

Expand Down
65 changes: 25 additions & 40 deletions vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,36 +28,21 @@ namespace vortex::filter {

template <concepts::model::SensorModelWithDefinedSizes SensModT, models::concepts::ImmModel ImmModelT> class ImmFilter {
public:
using SensModTPtr = std::shared_ptr<SensModT>;
using SensModI = typename SensModT::SensModI;

static constexpr size_t N_MODELS = ImmModelT::N_MODELS;

static constexpr int N_DIM_z = SensModI::N_DIM_z;

using Vec_n = typename ImmModelT::Vec_n;
using Mat_nn = typename ImmModelT::Mat_nn;

using Vec_x = typename SensModI::Vec_x;
using Vec_z = typename SensModI::Vec_z;

using Mat_xz = typename SensModI::Mat_xz;
using Mat_zz = typename SensModI::Mat_zz;
static constexpr auto N_DIMS_x = ImmModelT::N_DIMS_x;
static constexpr int N_DIM_z = SensModT::N_DIM_z;

using Gauss_z = typename SensModI::Gauss_z;
using GaussArr_z = std::array<Gauss_z, N_MODELS>;
template <size_t i> using T = Types_xz<N_DIMS_x(i), N_DIM_z>;

using GaussMix_z = prob::GaussianMixture<N_DIM_z>;

template <size_t i> using DynModT = typename ImmModelT::template DynModT<i>;
template <size_t i> using DynModTPtr = typename ImmModelT::template DynModTPtr<i>;
template <size_t i> using DynModI = typename ImmModelT::template DynModI<i>;
template <size_t i> using DynModIPtr = typename ImmModelT::template DynModIPtr<i>;

template <size_t i> using Gauss_x = typename ImmModelT::template Gauss_x<i>;
template <size_t i> using GaussMix_x = typename ImmModelT::template GaussMix_x<i>;
template <size_t i> using DynModT = typename ImmModelT::template DynModT<i>;

using Vec_n = Eigen::Vector<double, N_MODELS>;
using Mat_nn = Eigen::Matrix<double, N_MODELS, N_MODELS>;
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<Gauss_z, N_MODELS>;

/// No need to instantiate this class. All methods are static.
ImmFilter() = delete;
Expand Down Expand Up @@ -178,7 +163,7 @@ template <concepts::model::SensorModelWithDefinedSizes SensModT, models::concept
{

// Calculate mode-matched filter outputs and save them in a tuple of tuples
std::tuple<std::tuple<Gauss_x<Is>, Gauss_x<Is>, Gauss_z>...> ekf_outs;
std::tuple<std::tuple<typename T<Is>::Gauss_x, typename T<Is>::Gauss_x, Gauss_z>...> ekf_outs;
((std::get<Is>(ekf_outs) = step_kalman_filter<Is>(imm_model.template get_model<Is>(), sensor_model, dt, std::get<Is>(moment_based_preds), z_meas)), ...);

// Convert tuple of tuples to tuple of tuples
Expand All @@ -204,18 +189,18 @@ template <concepts::model::SensorModelWithDefinedSizes SensModT, models::concept
* @return Tuple of updated state, predicted state, predicted measurement
*/
template <size_t i>
static std::tuple<Gauss_x<i>, Gauss_x<i>, Gauss_z> step_kalman_filter(const DynModT<i> &dyn_model, const SensModT &sensor_model, double dt,
const Gauss_x<i> &x_est_prev, const Vec_z &z_meas)
static std::tuple<typename T<i>::Gauss_x, typename T<i>::Gauss_x, Gauss_z> step_kalman_filter(const DynModT<i> &dyn_model, const SensModT &sensor_model,
double dt, const T<i>::Gauss_x &x_est_prev, const Vec_z &z_meas)
{
if constexpr (models::concepts::DynamicModelLTV<DynModT<i>> && models::concepts::SensorModelLTV<SensModT>) {
if constexpr (concepts::model::DynamicModelLTVWithDefinedSizes<DynModT<i>> && concepts::model::SensorModelLTVWithDefinedSizes<SensModT>) {
using ImmSensMod = models::ImmSensorModelLTV<ImmModelT::N_DIM_x(i), SensModT>;
using EKF = filter::EKF<DynModI<i>, ImmSensMod>;
using EKF = filter::EKF<DynModT<i>, ImmSensMod>;
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<ImmModelT::N_DIM_x(i), SensModT>;
using UKF = filter::UKF<DynModI<i>, ImmSensMod>;
using UKF = filter::UKF<DynModT<i>, ImmSensMod>;
ImmSensMod imm_sens_mod{sensor_model};
return UKF::step(dyn_model, imm_sens_mod, dt, x_est_prev, z_meas);
}
Expand All @@ -230,9 +215,9 @@ template <concepts::model::SensorModelWithDefinedSizes SensModT, models::concept
* @return std::tuple<Gauss_x<model_indices>...>
*/
template <size_t... model_indices>
static std::tuple<Gauss_x<model_indices>...> 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<size_t, model_indices...>)
static std::tuple<typename T<model_indices>::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<size_t, model_indices...>)
{
return {mix_one_component<model_indices>(x_est_prevs, mixing_probs.col(model_indices), state_names, states_min_max)...};
}
Expand All @@ -247,8 +232,8 @@ template <concepts::model::SensorModelWithDefinedSizes SensModT, models::concept
* @note This is the function that actually does the mixing of the models. It is called for each model in the IMM filter.
*/
template <size_t target_model_index>
static Gauss_x<target_model_index> mix_one_component(const GaussTuple_x &x_est_prevs, const Vec_n &weights, const ImmModelT::StateNames &state_names,
const models::StateMap &states_min_max = {})
static T<target_model_index>::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<N_DIM_x>;
Expand All @@ -265,9 +250,9 @@ template <concepts::model::SensorModelWithDefinedSizes SensModT, models::concept
* @return std::array<Gauss_x<target_model_index>, N_MODELS>
*/
template <size_t target_model_index, size_t... mixing_model_indices>
static std::array<Gauss_x<target_model_index>, N_MODELS> prepare_models(const GaussTuple_x &x_est_prevs, const ImmModelT::StateNames &state_names,
const models::StateMap &states_min_max,
std::integer_sequence<size_t, mixing_model_indices...>)
static std::array<typename T<target_model_index>::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<size_t, mixing_model_indices...>)
{
return {prepare_mixing_model<target_model_index, mixing_model_indices>(x_est_prevs, state_names, states_min_max)...};
}
Expand All @@ -284,8 +269,8 @@ template <concepts::model::SensorModelWithDefinedSizes SensModT, models::concept
* are initialized with the mean and covariance from the target model or with a uniform distribution if `states_min_max` is provided.
*/
template <size_t target_model_index, size_t mixing_model_index>
static Gauss_x<target_model_index> prepare_mixing_model(const GaussTuple_x &x_est_prevs, const ImmModelT::StateNames &state_names,
const models::StateMap &states_min_max = {})
static T<target_model_index>::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<mixing_model_index>(x_est_prevs);
Expand Down
10 changes: 5 additions & 5 deletions vortex-filtering/include/vortex_filtering/filters/ukf.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,11 @@ namespace filter {
*/
template <concepts::model::DynamicModelWithDefinedSizes DynModT, concepts::model::SensorModelWithDefinedSizes SensModT, double alpha = 1.0, double beta = 2.0, double kappa = 0.0> class UKF {
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;
static constexpr int N_DIM_x = DynModT::N_DIM_x;
static constexpr int N_DIM_u = DynModT::N_DIM_u;
static constexpr int N_DIM_z = SensModT::N_DIM_z;
static constexpr int N_DIM_v = DynModT::N_DIM_v;
static constexpr int N_DIM_w = SensModT::N_DIM_w;

using T = Types_xzuvw<N_DIM_x, N_DIM_z, N_DIM_u, N_DIM_v, N_DIM_w>;

Expand Down
56 changes: 32 additions & 24 deletions vortex-filtering/include/vortex_filtering/models/dynamic_model_interfaces.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,11 @@ template <size_t n_dim_x, size_t n_dim_u = n_dim_x, size_t n_dim_v = n_dim_x> cl
*/
template <size_t n_dim_x, size_t n_dim_u = n_dim_x, size_t n_dim_v = n_dim_x> class DynamicModelLTV : public DynamicModel<n_dim_x, n_dim_u, n_dim_v> {
public:
using T = Types_xuv<n_dim_x, n_dim_u, n_dim_v>;
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 T = Types_xuv<N_DIM_x, N_DIM_u, N_DIM_v>;

/** 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
Expand All @@ -96,17 +100,17 @@ template <size_t n_dim_x, size_t n_dim_u = n_dim_x, size_t n_dim_v = n_dim_x> cl
* @note - G_d (optional if n_dim_x == n_dim_v)
*/
DynamicModelLTV()
: DynamicModel<n_dim_x, n_dim_u, n_dim_v>()
: DynamicModel<N_DIM_x, N_DIM_u, N_DIM_v>()
{
}
virtual ~DynamicModelLTV() = default;

/** Discrete time dynamics
* @param dt Time step
* @param x T::Vec_x State
* @param u T::Vec_u Input
* @param v T::Vec_v Process noise
* @return T::Vec_x
* @param x Vec_x State
* @param u Vec_u Input
* @param v Vec_v Process noise
* @return Vec_x
*/
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
{
Expand Down Expand Up @@ -147,7 +151,7 @@ template <size_t n_dim_x, size_t n_dim_u = n_dim_x, size_t n_dim_v = n_dim_x> cl
*/
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 T::Mat_xv::Identity();
Expand Down Expand Up @@ -208,15 +212,19 @@ template <size_t n_dim_x, size_t n_dim_u = n_dim_x, size_t n_dim_v = n_dim_x> cl
*/
template <size_t n_dim_x, size_t n_dim_u = n_dim_x, size_t n_dim_v = n_dim_x> class DynamicModelCTLTV : public DynamicModelLTV<n_dim_x, n_dim_u, n_dim_v> {
public:
using T = Types_xuv<n_dim_x, n_dim_u, n_dim_v>;
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 T = Types_xuv<N_DIM_x, N_DIM_u, N_DIM_v>;

/** 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<n_dim_x, n_dim_u, n_dim_v>()
: DynamicModelLTV<N_DIM_x, N_DIM_u, N_DIM_v>()
{
}
virtual ~DynamicModelCTLTV() = default;
Expand Down Expand Up @@ -257,7 +265,7 @@ template <size_t n_dim_x, size_t n_dim_u = n_dim_x, size_t n_dim_v = n_dim_x> cl
*/
virtual T::Mat_xv G_c(const T::Vec_x &x) const
{
if (n_dim_x != n_dim_v) {
if (N_DIM_x != N_DIM_v) {
throw std::runtime_error("G_c not implemented");
}
(void)x; // unused
Expand All @@ -283,7 +291,7 @@ template <size_t n_dim_x, size_t n_dim_u = n_dim_x, size_t n_dim_v = n_dim_x> cl
*/
T::Mat_xu B_d(double dt, const T::Vec_x &x) const override
{
Eigen::Matrix<double, n_dim_x + n_dim_u, n_dim_x + n_dim_u> van_loan;
Eigen::Matrix<double, N_DIM_x + N_DIM_u, N_DIM_x + N_DIM_u> van_loan;
van_loan << A_c(x), B_c(x), T::Mat_ux::Zero(), T::Mat_uu::Zero();
van_loan *= dt;
van_loan = van_loan.exp();
Expand All @@ -295,17 +303,17 @@ template <size_t n_dim_x, size_t n_dim_u = n_dim_x, size_t n_dim_v = n_dim_x> cl

T::Mat_xv G_d(double dt, const T::Vec_x &x) const override
{
Eigen::Matrix<double, n_dim_x + n_dim_v, n_dim_x + n_dim_v> van_loan;
van_loan.template topLeftCorner<n_dim_x, n_dim_x>() = A_c(x);
van_loan.template topRightCorner<n_dim_x, n_dim_v>() = G_c(x);
van_loan.template bottomLeftCorner<n_dim_v, n_dim_x>() = T::Mat_vx::Zero();
van_loan.template bottomRightCorner<n_dim_v, n_dim_v>() = T::Mat_vv::Zero();
Eigen::Matrix<double, N_DIM_x + N_DIM_v, N_DIM_x + N_DIM_v> van_loan;
van_loan.template topLeftCorner<N_DIM_x, N_DIM_x>() = A_c(x);
van_loan.template topRightCorner<N_DIM_x, N_DIM_v>() = G_c(x);
van_loan.template bottomLeftCorner<N_DIM_v, N_DIM_x>() = T::Mat_vx::Zero();
van_loan.template bottomRightCorner<N_DIM_v, N_DIM_v>() = T::Mat_vv::Zero();

van_loan *= dt;
van_loan = van_loan.exp();

// T::Mat_xx A_d = van_loan.template block<N_DIM_x, N_DIM_x>(0, 0);
typename T::Mat_xv G_d = van_loan.template block<n_dim_x, n_dim_v>(0, N_DIM_x);
typename T::Mat_xv G_d = van_loan.template block<N_DIM_x, N_DIM_v>(0, N_DIM_x);
return G_d;
}

Expand All @@ -331,17 +339,17 @@ template <size_t n_dim_x, size_t n_dim_u = n_dim_x, size_t n_dim_v = n_dim_x> cl
typename T::Mat_vv Q_c = this->Q_c(x);
typename T::Mat_xv G_c = this->G_c(x);

Eigen::Matrix<double, 2 * n_dim_x, 2 * n_dim_x> van_loan;
van_loan.template topLeftCorner<n_dim_x, n_dim_x>() = -A_c;
van_loan.template topRightCorner<n_dim_x, n_dim_x>() = G_c * Q_c * G_c.transpose();
van_loan.template bottomLeftCorner<n_dim_x, n_dim_x>() = T::Mat_xx::Zero();
van_loan.template bottomRightCorner<n_dim_x, n_dim_x>() = A_c.transpose();
Eigen::Matrix<double, 2 * N_DIM_x, 2 * N_DIM_x> van_loan;
van_loan.template topLeftCorner<N_DIM_x, N_DIM_x>() = -A_c;
van_loan.template topRightCorner<N_DIM_x, N_DIM_x>() = G_c * Q_c * G_c.transpose();
van_loan.template bottomLeftCorner<N_DIM_x, N_DIM_x>() = T::Mat_xx::Zero();
van_loan.template bottomRightCorner<N_DIM_x, N_DIM_x>() = A_c.transpose();

van_loan *= dt;
van_loan = van_loan.exp();

typename T::Mat_xx A_d = van_loan.template block<n_dim_x, n_dim_x>(n_dim_x, n_dim_x).transpose();
typename T::Mat_xx A_d_inv_GQGT_d = van_loan.template block<n_dim_x, n_dim_x>(0, n_dim_x); // A_d^(-1) * G * Q * G^T
typename T::Mat_xx A_d = van_loan.template block<N_DIM_x, N_DIM_x>(N_DIM_x, N_DIM_x).transpose();
typename T::Mat_xx A_d_inv_GQGT_d = van_loan.template block<N_DIM_x, N_DIM_x>(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;
}
Expand Down
28 changes: 22 additions & 6 deletions vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,14 @@ constexpr int UNUSED = 1; // For when a template parameter is required but not u
* @tparam n_dim_x Number of dimensions in state vector
*/
template <size_t n_dim_x> class IdentityDynamicModel : public interface::DynamicModelLTV<n_dim_x> {
using Parent = interface::DynamicModelLTV<n_dim_x>;

public:
using DynModI = interface::DynamicModelLTV<n_dim_x>;
using T = vortex::Types_xv<n_dim_x, n_dim_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;

using T = vortex::Types_xuv<N_DIM_x, N_DIM_u, N_DIM_v>;

/** Identity Dynamic Model
* @param std Standard deviation of process noise
Expand Down Expand Up @@ -44,9 +49,14 @@ template <size_t n_dim_x> class IdentityDynamicModel : public interface::Dynamic
* @tparam n_spatial_dim Number of spatial dimensions
*/
class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> {
using Parent = interface::DynamicModelLTV<2, UNUSED, 2>;

public:
using DynModI = interface::DynamicModelLTV<2, UNUSED, 2>;
using T = vortex::Types_xv<N_DIM_x, N_DIM_v>;
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<N_DIM_x, N_DIM_u, N_DIM_v>;

using ST = StateType;
static constexpr std::array StateNames{ST::position, ST::position};
Expand All @@ -66,7 +76,7 @@ class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> {
* @return T::Mat_xx
* @note Overriding DynamicModelLTV::A_d
*/
T::Mat_xx A_d(double /*dt*/, const T::Vec_x & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity(); }
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
Expand Down Expand Up @@ -97,11 +107,17 @@ class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> {
* @tparam n_spatial_dim Number of spatial dimensions
*/
class ConstantVelocity : public interface::DynamicModelLTV<4, UNUSED, 2> {
using Parent = interface::DynamicModelLTV<4, 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;

static constexpr int N_SPATIAL_DIM = 2;
static constexpr int N_STATES = 2 * N_SPATIAL_DIM;

using T = vortex::Types_xv<N_STATES, N_DIM_v>;
using T = vortex::Types_xuv<N_DIM_x, N_DIM_u, N_DIM_v>;


using Vec_s = Eigen::Matrix<double, N_SPATIAL_DIM, 1>;
Expand Down
Loading

0 comments on commit 18a4496

Please sign in to comment.