Skip to content

Commit

Permalink
created classes for types based on macros
Browse files Browse the repository at this point in the history
  • Loading branch information
@EirikKolas
EirikKolas committed Mar 10, 2024
1 parent 41f95f4 commit f4eef65
Show file tree
Hide file tree
Showing 5 changed files with 133 additions and 29 deletions.
43 changes: 14 additions & 29 deletions vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#pragma once
#include <vortex_filtering/models/dynamic_model_interfaces.hpp>
#include <vortex_filtering/models/imm_model.hpp>
#include <vortex_filtering/models/type_aliases.hpp>

namespace vortex {
namespace models {
Expand All @@ -13,32 +14,29 @@ constexpr int UNUSED = 1; // For when a template parameter is required but not u
template <size_t n_dim_x> class IdentityDynamicModel : public interface::DynamicModelLTV<n_dim_x> {
public:
using DynModI = interface::DynamicModelLTV<n_dim_x>;
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;
using T = vortex::Types_xv<n_dim_x, n_dim_x>;

/** Identity Dynamic Model
* @param std Standard deviation of process noise
*/
IdentityDynamicModel(double std)
: Q_(Mat_xx::Identity() * std * std)
: Q_(T::Mat_xx::Identity() * std * std)
{
}

/** Identity Dynamic Model
* @param Q Process noise covariance
*/
IdentityDynamicModel(Mat_vv Q)
IdentityDynamicModel(T::Mat_vv Q)
: Q_(Q)
{
}

Mat_xx A_d(double dt, const Vec_x /*x*/ &) const override { return Mat_xx::Identity() * dt; }
Mat_vv Q_d(double /*dt*/, const Vec_x /*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
Expand All @@ -47,20 +45,11 @@ template <size_t n_dim_x> class IdentityDynamicModel : public interface::Dynamic
*/
class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> {
public:
static constexpr int N_STATES = 2;
using DynModI = interface::DynamicModelLTV<N_STATES, UNUSED, N_STATES>;
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;

using Vec_s = Eigen::Vector<double, N_STATES>;
using Mat_ss = Eigen::Matrix<double, N_STATES, N_STATES>;
using DynModI = interface::DynamicModelLTV<2, UNUSED, 2>;
using T = vortex::Types_xv<N_DIM_x, N_DIM_v>;

using ST = StateType;
static constexpr std::array<ST, N_STATES> StateNames{ST::position, ST::position};
static constexpr std::array StateNames{ST::position, ST::position};

/** Constant Position Model in 2D
* x = [x, y]
Expand All @@ -77,21 +66,17 @@ class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> {
* @return Mat_xx
* @note Overriding DynamicModelLTV::A_d
*/
Mat_xx A_d(double /*dt*/, const Vec_x & = Vec_x::Zero()) const override
{
Mat_ss I = Mat_ss::Identity();
return I;
}
T::Mat_xx A_d(double /*dt*/, const T::Vec_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 Mat_xv
* @note Overriding DynamicModelLTV::G_d
*/
Mat_xv G_d(double dt, const Vec_x /*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();
T::Mat_xx I = T::Mat_xx::Identity();
return 0.5 * dt * I;
}

Expand All @@ -101,7 +86,7 @@ class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> {
* @return Mat_xx Process noise covariance
* @note Overriding DynamicModelLTV::Q_d
*/
Mat_vv Q_d(double /*dt*/ = 0.0, const Vec_x /*x*/ & = Vec_x::Zero()) const override { return Mat_vv::Identity() * std_pos_ * std_pos_; }
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_;
Expand Down
101 changes: 101 additions & 0 deletions vortex-filtering/include/vortex_filtering/models/type_aliases.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,101 @@
#pragma once

#include <Eigen/Dense>
#include <vortex_filtering/probability/gaussian_mixture.hpp>
#include <vortex_filtering/probability/multi_var_gauss.hpp>

#define VORTEX_TYPES_1(t1, s1) \
using Vec_##t1 = Eigen::Vector<double, s1>; \
using Mat_##t1##t1 = Eigen::Matrix<double, s1, s1>; \
using Gauss_##t1 = vortex::prob::Gauss<s1>; \
using GaussMix_##t1 = vortex::prob::GaussMix<s1>;

#define VORTEX_TYPES_2(t1, s1, t2, s2) \
using Mat_##t1##t2 = Eigen::Matrix<double, s1, s2>; \
using Mat_##t2##t1 = Eigen::Matrix<double, s2, s1>;

#define VORTEX_TYPES_3(t1, s1, t2, s2, t3, s3) \
VORTEX_TYPES_1(t1, s1) \
VORTEX_TYPES_1(t2, s2) \
VORTEX_TYPES_1(t3, s3) \
VORTEX_TYPES_2(t1, s1, t2, s2) \
VORTEX_TYPES_2(t1, s1, t3, s3) \
VORTEX_TYPES_2(t2, s2, t3, s3)

#define VORTEX_TYPES_4(t1, s1, t2, s2, t3, s3, t4, s4) \
VORTEX_TYPES_1(t1, s1) \
VORTEX_TYPES_1(t2, s2) \
VORTEX_TYPES_1(t3, s3) \
VORTEX_TYPES_1(t4, s4) \
VORTEX_TYPES_2(t1, s1, t2, s2) \
VORTEX_TYPES_2(t1, s1, t3, s3) \
VORTEX_TYPES_2(t1, s1, t4, s4) \
VORTEX_TYPES_2(t2, s2, t3, s3) \
VORTEX_TYPES_2(t2, s2, t4, s4) \
VORTEX_TYPES_2(t3, s3, t4, s4)

#define VORTEX_TYPES_5(t1, s1, t2, s2, t3, s3, t4, s4, t5, s5) \
VORTEX_TYPES_1(t1, s1) \
VORTEX_TYPES_1(t2, s2) \
VORTEX_TYPES_1(t3, s3) \
VORTEX_TYPES_1(t4, s4) \
VORTEX_TYPES_1(t5, s5) \
VORTEX_TYPES_2(t1, s1, t2, s2) \
VORTEX_TYPES_2(t1, s1, t3, s3) \
VORTEX_TYPES_2(t1, s1, t4, s4) \
VORTEX_TYPES_2(t1, s1, t5, s5) \
VORTEX_TYPES_2(t2, s2, t3, s3) \
VORTEX_TYPES_2(t2, s2, t4, s4) \
VORTEX_TYPES_2(t2, s2, t5, s5) \
VORTEX_TYPES_2(t3, s3, t4, s4) \
VORTEX_TYPES_2(t3, s3, t5, s5) \
VORTEX_TYPES_2(t4, s4, t5, s5)

namespace vortex {

template <size_t n_dim_x> struct Types_x {
Types_x() = delete;
VORTEX_TYPES_1(x, n_dim_x)
};

template <size_t n_dim_z> struct Types_z {
Types_z() = delete;
VORTEX_TYPES_1(z, n_dim_z)
};

template <size_t n_dim_x, size_t n_dim_z> struct Types_xz {
Types_xz() = delete;
VORTEX_TYPES_1(x, n_dim_x)
VORTEX_TYPES_1(z, n_dim_z)
VORTEX_TYPES_2(x, n_dim_x, z, n_dim_z)
};

template <size_t n_dim_x, size_t n_dim_v> struct Types_xv {
Types_xv() = delete;
VORTEX_TYPES_1(x, n_dim_x)
VORTEX_TYPES_1(v, n_dim_v)
VORTEX_TYPES_2(x, n_dim_x, v, n_dim_v)
};

template <size_t n_dim_x, size_t n_dim_z, size_t n_dim_u> struct Types_xzu {
Types_xzu() = delete;
VORTEX_TYPES_3(x, n_dim_x, z, n_dim_z, u, n_dim_u)
};

template <size_t n_dim_x, size_t n_dim_z, size_t n_dim_u, size_t n_dim_v> struct Types_xzuv {
Types_xzuv() = delete;
VORTEX_TYPES_4(x, n_dim_x, z, n_dim_z, u, n_dim_u, v, n_dim_v)
};

template <size_t n_dim_x, size_t n_dim_z, size_t n_dim_u, size_t n_dim_v, size_t n_dim_w> struct Types_xzuvw {
Types_xzuvw() = delete;
VORTEX_TYPES_5(x, n_dim_x, z, n_dim_z, u, n_dim_u, v, n_dim_v, w, n_dim_w)
};

} // namespace vortex

#undef VORTEX_TYPES_1
#undef VORTEX_TYPES_2
#undef VOXTEX_TYPES_3
#undef VORTEX_TYPES_4
#undef VORTEX_TYPES_5
1 change: 1 addition & 0 deletions vortex-filtering/include/vortex_filtering/vortex_filtering.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <vortex_filtering/models/dynamic_models.hpp>
#include <vortex_filtering/models/sensor_model_interfaces.hpp>
#include <vortex_filtering/models/sensor_models.hpp>
#include <vortex_filtering/models/type_aliases.hpp>

// Numerical Integration
#include <vortex_filtering/numerical_integration/erk_methods.hpp>
Expand Down
1 change: 1 addition & 0 deletions vortex-filtering/test/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,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
Expand Down
16 changes: 16 additions & 0 deletions vortex-filtering/test/types_test.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
#include <gtest/gtest.h>
#include <vortex_filtering/models/type_aliases.hpp>
#include <iostream>
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<decltype(x), Eigen::Vector2d>::value, "x is not a Vector2d");

ASSERT_TRUE(true);
ASSERT_TRUE(true);

std::cout << "x: " << x << std::endl;
}

0 comments on commit f4eef65

Please sign in to comment.