Add support for quaternions to GaussianMixture and related

CHANGELOG.md

@@ -10,8 +10,10 @@
 - Implemented MAP (maximum a posteriori) estimate extraction within method EstimatesExtraction::map().
 - Implemented overloaded version of EstimatesExtraction::extract() taking extra arguments (particle weights at the previous time step, likelihoods at the current step and matrix of Markov transition probabilities between previous and current states) required to expose MAP extraction utiliy to the user.
 - Implemented method GaussianMixture::resize().
+- Implemented method `GaussianMixture::augmentWithNoise()`.
 - Implemented method Gaussian::resize().
 - Implemented method ParticleSet::resize().
+- Added support for quaternions in classes `Gaussian`, `GaussianMixture` and `ParticleSet`.
 - Implemented evaluation of a multivariate Gaussian probability density function in method utils::multivariate_gaussian_density.
 - Implemented evaluation of the logarithm of a multivariate Gaussian probability density function in method utils::multivariate_gaussian_log_density.
 - Implemented evaluation of a multivariate Gaussian probability density function with UVR factorized covariance matrix in method `utils::multivariate_gaussian_density_UVR()`.
@@ -62,7 +64,7 @@
 - Reduce number of particles in test_SIS to reduce testing computation time in Debug.
 - Add testUPF_MAP testing MAP (maximum a posteriori) estimate extraction within a UPF particle filter.
 - Add `test_Gaussian_Density_UVR` testing the method `utils::multivariate_gaussian_density_UVR()`.
-- Change test_Gaussian in order to test resizing.
+- Change `test_Gaussian` in order to test resizing, noise augmentation and support for quaternions.
 - Update `test_KF`, `test_UKF`, `test_mixed_KF_UKF`, `test_mixed_UKF_KF`, `test_mixed_KF_SUKF` to account for the removed measurements freeze within `GaussianCorrection::correct()`.
 
 ## 🔖 Version 0.8.101

src/BayesFilters/include/BayesFilters/Gaussian.h

@@ -26,7 +26,7 @@ class bfl::Gaussian : public bfl::GaussianMixture
 
     Gaussian(const std::size_t dim_linear) noexcept;
 
-    Gaussian(const std::size_t dim_linear, const std::size_t dim_circular) noexcept;
+    Gaussian(const std::size_t dim_linear, const std::size_t dim_circular, const bool use_quaternion = false) noexcept;
 
     virtual ~Gaussian() noexcept;
 

src/BayesFilters/include/BayesFilters/GaussianMixture.h

@@ -24,7 +24,7 @@ class bfl::GaussianMixture
 
     GaussianMixture(const std::size_t components, const std::size_t dim) noexcept;
 
-    GaussianMixture(const std::size_t components, const std::size_t dim_linear, const std::size_t dim_circular) noexcept;
+    GaussianMixture(const std::size_t components, const std::size_t dim_linear, const std::size_t dim_circular, const bool use_quaternion = false) noexcept;
 
     virtual ~GaussianMixture() noexcept;
 
@@ -66,6 +66,10 @@ class bfl::GaussianMixture
 
     std::size_t components;
 
+    bool use_quaternion;
+
+    std::size_t dim_circular_component;
+
     std::size_t dim;
 
     std::size_t dim_linear;
@@ -74,6 +78,8 @@ class bfl::GaussianMixture
 
     std::size_t dim_noise;
 
+    std::size_t dim_covariance;
+
 protected:
     Eigen::MatrixXd mean_;
 

src/BayesFilters/include/BayesFilters/ParticleSet.h

@@ -24,7 +24,7 @@ class bfl::ParticleSet : public bfl::GaussianMixture
 
     ParticleSet(const std::size_t components, const std::size_t dim) noexcept;
 
-    ParticleSet(const std::size_t components, const std::size_t dim_linear, const std::size_t dim_circular) noexcept;
+    ParticleSet(const std::size_t components, const std::size_t dim_linear, const std::size_t dim_circular, const bool use_quaternion = false) noexcept;
 
     virtual ~ParticleSet() noexcept;
 

src/BayesFilters/src/Gaussian.cpp

@@ -13,17 +13,22 @@ using namespace bfl;
 using namespace Eigen;
 
 Gaussian::Gaussian() noexcept :
-    GaussianMixture(1, 1)
+    GaussianMixture(1, 1, 0, false)
 { }
 
 
 Gaussian::Gaussian(const std::size_t dim_linear) noexcept :
-    GaussianMixture(1, dim_linear)
+    GaussianMixture(1, dim_linear, 0, false)
 { }
 
 
-Gaussian::Gaussian(const std::size_t dim_linear, const std::size_t dim_circular) noexcept :
-    GaussianMixture(1, dim_linear, dim_circular)
+Gaussian::Gaussian
+(
+    const std::size_t dim_linear,
+    const std::size_t dim_circular,
+    const bool use_quaternion
+) noexcept :
+    GaussianMixture(1, dim_linear, dim_circular, use_quaternion)
 { }
 
 

src/BayesFilters/src/GaussianMixture.cpp

@@ -12,32 +12,52 @@ using namespace Eigen;
 
 
 GaussianMixture::GaussianMixture() noexcept:
-    GaussianMixture(1, 1, 0)
+    GaussianMixture(1, 1, 0, false)
 { }
 
 
 GaussianMixture::GaussianMixture(const std::size_t components, const std::size_t dim) noexcept :
-    GaussianMixture(components, dim, 0)
+    GaussianMixture(components, dim, 0, false)
 { }
 
 
 GaussianMixture::GaussianMixture
 (
     const std::size_t components,
     const std::size_t dim_linear,
-    const std::size_t dim_circular
+    const std::size_t dim_circular,
+    const bool use_quaternion
 ) noexcept :
     components(components),
-    dim(dim_linear + dim_circular),
+    use_quaternion(use_quaternion),
+    dim_circular_component(use_quaternion ? 4 : 1),
+    dim(dim_linear + dim_circular * dim_circular_component),
     dim_linear(dim_linear),
     dim_circular(dim_circular),
     dim_noise(0),
+    dim_covariance(use_quaternion ? dim_linear + dim_circular * (dim_circular_component - 1) : dim),
     mean_(dim, components),
-    covariance_(dim, dim * components),
+    covariance_(dim_covariance, dim_covariance * components),
     weight_(components)
 {
     for (int i = 0; i < this->components; ++i)
         weight_(i) = 1.0 / this->components;
+
+    /* Note:
+       When using use_quaternion == false, the hypothesis is that there are dim_circular
+       independent states each belonging to the manifold S1 (i.e. dim_circular angles).
+       In this implementation they are treated as belonging to R^(dim_circular).
+       Hence, the size of the covariance matrix is dim_covariance x (dim_covariance * components)
+       where dim_covariance = dim.
+
+       When using use_quaternion == true, instead the hypothesis is that there are dim_circular
+       quaternions in the state, each belonging to the manifold S3.
+       In this case, dim_circular_component = 4, since they are represented using 4 numbers.
+       However, the covariance is represented using rotation vectors in R^3 that belong to the tangent
+       space of the quaternion manifold. Hence, the size of the covariance matrix is
+       dim_covariance x (dim_covariance * components) where dim_covariance = dim_linear + dim_circular * 3.
+    */
+
 }
 
 
@@ -47,27 +67,29 @@ GaussianMixture::~GaussianMixture() noexcept
 
 void GaussianMixture::resize(const std::size_t components, const std::size_t dim_linear, const std::size_t dim_circular)
 {
-    std::size_t new_dim = dim_linear + dim_circular;
+    std::size_t new_dim = dim_linear + dim_circular * dim_circular_component;
+    std::size_t new_dim_covariance = use_quaternion ? dim_linear + dim_circular * (dim_circular_component - 1) : new_dim;
 
     if ((this->dim_linear == dim_linear) && (this->dim_circular == dim_circular) && (this->components == components))
         return;
     else if ((this->dim == new_dim) && (this->components != components))
     {
         mean_.conservativeResize(NoChange, components);
-        covariance_.conservativeResize(NoChange, dim * components);
+        covariance_.conservativeResize(NoChange, dim_covariance * components);
         weight_.conservativeResize(components);
     }
     else
     {
         // In any other case, it does not make sense to do conservative resize
         // since either old data is truncated or new data is incomplete
         mean_.resize(new_dim, components);
-        covariance_.resize(new_dim, new_dim * components);
+        covariance_.resize(new_dim_covariance, new_dim_covariance * components);
         weight_.resize(components);
     }
 
     this->components = components;
     this->dim = new_dim;
+    this->dim_covariance = new_dim_covariance;
     this->dim_linear = dim_linear;
     this->dim_circular = dim_circular;
 }
@@ -117,13 +139,13 @@ Ref<MatrixXd> GaussianMixture::covariance()
 
 Ref<MatrixXd> GaussianMixture::covariance(const std::size_t i)
 {
-    return covariance_.middleCols(this->dim * i, this->dim);
+    return covariance_.middleCols(this->dim_covariance * i, this->dim_covariance);
 }
 
 
 double& GaussianMixture::covariance(const std::size_t i, const std::size_t j, const std::size_t k)
 {
-    return covariance_(j, (this->dim * i) + k);
+    return covariance_(j, (this->dim_covariance * i) + k);
 }
 
 
@@ -135,13 +157,13 @@ const Ref<const MatrixXd> GaussianMixture::covariance() const
 
 const Ref<const MatrixXd> GaussianMixture::covariance(const std::size_t i) const
 {
-    return covariance_.middleCols(this->dim * i, this->dim);
+    return covariance_.middleCols(this->dim_covariance * i, this->dim_covariance);
 }
 
 
 const double& GaussianMixture::covariance(const std::size_t i, const std::size_t j, const std::size_t k) const
 {
-    return covariance_(j, (this->dim * i) + k);
+    return covariance_(j, (this->dim_covariance * i) + k);
 }
 
 
@@ -180,26 +202,27 @@ bool GaussianMixture::augmentWithNoise(const Eigen::Ref<const Eigen::MatrixXd>&
 
     dim_noise = noise_covariance_matrix.rows();
     dim += dim_noise;
+    dim_covariance += dim_noise;
 
     /* Add zero mean noise to each mean. */
     mean_.conservativeResize(dim, NoChange);
     mean_.bottomRows(dim_noise) = MatrixXd::Zero(dim_noise, components);
 
     /* Resize covariance matrix. */
-    covariance_.conservativeResizeLike(MatrixXd::Zero(dim, dim * components));
+    covariance_.conservativeResizeLike(MatrixXd::Zero(dim_covariance, dim_covariance * components));
 
     /* Move old covariance matrices from right to left to avoid aliasing.
 
      Note that the covariance matrix of the 0-th coomponent,
      i.e. in the top-left corner of the matrix covariance_,
      is already in the correct place.
     */
-    std::size_t dim_old = dim_linear + dim_circular;
+    std::size_t dim_old = use_quaternion ? dim_linear + dim_circular * (dim_circular_component - 1) : dim_linear + dim_circular;
     for (std::size_t i = 0; i < (components - 1); i++)
     {
         std::size_t i_index = components - 1 - i;
 
-        Ref<MatrixXd> new_block = covariance_.block(0, i_index * dim,     dim_old, dim_old);
+        Ref<MatrixXd> new_block = covariance_.block(0, i_index * dim_covariance, dim_old, dim_old);
         Ref<MatrixXd> old_block = covariance_.block(0, i_index * dim_old, dim_old, dim_old);
 
         /* Swap columns from to right to left to avoid aliasing. */
@@ -213,12 +236,15 @@ bool GaussianMixture::augmentWithNoise(const Eigen::Ref<const Eigen::MatrixXd>&
 
     for (std::size_t i = 0; i < components; i++)
     {
-        /* Copy the noise covariance matrix in the bottom-right block of
-           each covariance matrix. */
-        covariance_.block(dim_old, i * dim + dim_old, dim_noise, dim_noise) = noise_covariance_matrix;
+        /* Copy the noise covariance matrix in the bottom-right block of each covariance matrix. */
+        covariance_.block(dim_old, i * dim_covariance + dim_old, dim_noise, dim_noise) = noise_covariance_matrix;
 
         /* Clean part of the matrix that should be zero. */
-        covariance_.block(0, i * dim + dim_old, dim_old, dim_noise) = MatrixXd::Zero(dim_old, dim_noise);
+        covariance_.block(0, i * dim_covariance + dim_old, dim_old, dim_noise) = MatrixXd::Zero(dim_old, dim_noise);
+
+        /* The part in covariance_.block(dim_old, i * dim_covariance, dim_noise, dim_old) was set to 0 when doing
+           covariance_.conservativeResizeLike(MatrixXd::Zero(dim_covariance, dim_covariance * components));
+           since it is appended in order to expand the matrix. */
     }
 
     return true;

src/BayesFilters/src/ParticleSet.cpp

@@ -11,22 +11,23 @@ using namespace bfl;
 using namespace Eigen;
 
 ParticleSet::ParticleSet() noexcept :
-    ParticleSet(1, 1, 0)
+    ParticleSet(1, 1, 0, false)
 { }
 
 
 ParticleSet::ParticleSet(const std::size_t components, const std::size_t dim) noexcept:
-    ParticleSet(components, dim, 0)
+    ParticleSet(components, dim, 0, false)
 { }
 
 
 ParticleSet::ParticleSet
 (
     const std::size_t components,
     const std::size_t dim_linear,
-    const std::size_t dim_circular
+    const std::size_t dim_circular,
+    const bool use_quaternion
 ) noexcept :
-    GaussianMixture(components, dim_linear, dim_circular),
+    GaussianMixture(components, dim_linear, dim_circular, use_quaternion),
     state_(dim, components)
 { }
 
@@ -37,7 +38,7 @@ ParticleSet::~ParticleSet() noexcept
 
 void ParticleSet::resize(const std::size_t components, const std::size_t dim_linear, const std::size_t dim_circular)
 {
-    std::size_t new_dim = dim_linear + dim_circular;
+    std::size_t new_dim = dim_linear + dim_circular * dim_circular_component;
 
     if ((this->dim_linear == dim_linear) && (this->dim_circular = dim_circular) && (this->components == components))
         return;
@@ -66,8 +67,8 @@ ParticleSet& ParticleSet::operator+=(const ParticleSet& rhs)
     mean_.conservativeResize(NoChange,  new_components);
     mean_.rightCols(rhs.components) = rhs.mean_;
 
-    covariance_.conservativeResize(NoChange, dim * new_components);
-    covariance_.rightCols(dim * rhs.components) = rhs.covariance_;
+    covariance_.conservativeResize(NoChange, dim_covariance * new_components);
+    covariance_.rightCols(dim_covariance * rhs.components) = rhs.covariance_;
 
     weight_.conservativeResize(new_components);
     weight_.tail(rhs.components) = rhs.weight_;