MeasurementUpdate.h

Go to the documentation of this file.

#pragma once

 
#include <array>
#include <concepts>
#include <cstddef>
#include <expected>
#include <optional>
#include <span>
#include <tuple>
#include <type_traits>
#include <utility>
#include <variant>
#include <vector>
 
#include <Eigen/Cholesky>
 
#include <EmrysFilter/Core/Allocation.h>
#include <EmrysFilter/Core/Execution.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Multiply.h>
#include <EmrysFilter/Core/Solve.h>
#include <EmrysFilter/Core/Weights.h>
#include <EmrysFilter/Distributions/Gaussian.h>
#include <EmrysFilter/Distributions/Mixture.h>
#include <EmrysFilter/Distributions/Product.h>
#include <EmrysFilter/Models/Linearization.h>
#include <EmrysFilter/Models/Transform.h>
#include <EmrysFilter/Spaces/Composite.h>
#include <EmrysFilter/Spaces/Coordinates.h>
#include <EmrysFilter/Spaces/Indexing.h>
#include <EmrysFilter/Spaces/Space.h>
#include <EmrysFilter/Spaces/Subspace.h>
#include <EmrysFilter/Spaces/TagPath.h>
 
namespace Emrys
{

template <typename Space, typename MeasurementSpace, typename Scalar = double>
struct UpdateBranch
{
    Scalar logWeight;                         
    GaussianParams<Space, Scalar> posterior;  
    Innovation<MeasurementSpace, Scalar> innovation;  
    Eigen::Index mode = 0;   
    Eigen::Index component;  

    std::size_t iterations = 1;

    bool converged = true;

    [[nodiscard]] Scalar
    logLikelihood() const
    {
        return innovation.logLikelihood();
    }

    [[nodiscard]] Scalar
    nis() const
    {
        return innovation.nis();
    }
};
 
namespace internal
{
 
struct MixtureEvidenceAccess;
 
}  // namespace internal

template <
    typename Space,
    typename MeasurementSpace,
    std::size_t Count = std::dynamic_extent,
    typename Scalar_ = double,
    typename Allocator = DefaultAllocator>
struct MeasurementUpdate
{
    using Scalar = Scalar_;                                            
    using SpaceType = Space;                                           
    using MeasurementSpaceType = MeasurementSpace;                     
    using BranchType = UpdateBranch<Space, MeasurementSpace, Scalar>;  
    using allocator_type = Allocator;  

    using Container = std::conditional_t<
        Count == std::dynamic_extent,
        internal::Buffer<BranchType, Allocator>,
        std::array<BranchType, Count>>;
 
    static constexpr std::size_t BranchCount = Count;  
 
    Container branches;  
 
    MeasurementUpdate() = default;  

    MeasurementUpdate(Container branches, Scalar mixtureLogLikelihood = 0)
        : branches{std::move(branches)}
        , m_mixtureLogLikelihood{mixtureLogLikelihood}
    {}

    [[nodiscard]] Scalar
    logLikelihood() const
    {
        if constexpr (Count == 1)
        {
            return branches.front().logLikelihood();
        }
        else
        {
            return m_mixtureLogLikelihood;
        }
    }

    [[nodiscard]] Eigen::Index
    innovationSize() const
    {
        return branches.front().innovation.differenceDimension();
    }

    [[nodiscard]] Allocator
    get_allocator() const
    {
        if constexpr (Count == std::dynamic_extent)
        {
            return Allocator(branches.get_allocator());
        }
        else
        {
            return Allocator{};
        }
    }
 
private:
 
    friend struct internal::MixtureEvidenceAccess;  

    Scalar m_mixtureLogLikelihood = 0;
};
 
namespace internal
{

template <typename T>
inline constexpr bool isMeasurementUpdate = false;

template <typename Space, typename MeasurementSpace, std::size_t Count, typename Scalar, typename Allocator>
inline constexpr bool isMeasurementUpdate<MeasurementUpdate<Space, MeasurementSpace, Count, Scalar, Allocator>> = true;

 
}  // namespace internal

template <typename MeasurementSpace, typename Scalar = double>
struct FilterBranch
{
    Scalar logWeight;  
    Innovation<MeasurementSpace, Scalar> innovation;  
    Eigen::Index mode = 0;       
    Eigen::Index component;      
    std::size_t iterations = 1;  
    bool converged = true;       

    [[nodiscard]] Scalar
    logLikelihood() const
    {
        return innovation.logLikelihood();
    }

    [[nodiscard]] Scalar
    nis() const
    {
        return innovation.nis();
    }
};

template <
    typename MeasurementSpace,
    std::size_t Count = std::dynamic_extent,
    typename Scalar_ = double,
    typename Allocator = DefaultAllocator>
struct FilterUpdate
{
    using Scalar = Scalar_;                                     
    using MeasurementSpaceType = MeasurementSpace;              
    using BranchType = FilterBranch<MeasurementSpace, Scalar>;  
    using allocator_type = Allocator;                           

    using Container = std::conditional_t<
        Count == std::dynamic_extent,
        internal::Buffer<BranchType, Allocator>,
        std::array<BranchType, Count>>;
 
    static constexpr std::size_t BranchCount = Count;  
 
    Container branches;  
 
    bool accepted = false;  
 
    FilterUpdate() = default;  

    FilterUpdate(Container branches, Scalar mixtureLogLikelihood = 0, bool accepted = false)
        : branches{std::move(branches)}
        , accepted{accepted}
        , m_mixtureLogLikelihood{mixtureLogLikelihood}
    {}

    [[nodiscard]] Scalar
    logLikelihood() const
    {
        if constexpr (Count == 1)
        {
            return branches.front().logLikelihood();
        }
        else
        {
            return m_mixtureLogLikelihood;
        }
    }

    [[nodiscard]] Eigen::Index
    innovationSize() const
    {
        return branches.front().innovation.differenceDimension();
    }

    [[nodiscard]] Allocator
    get_allocator() const
    {
        if constexpr (Count == std::dynamic_extent)
        {
            return Allocator(branches.get_allocator());
        }
        else
        {
            return Allocator{};
        }
    }
 
private:
 
    friend struct internal::MixtureEvidenceAccess;  

    Scalar m_mixtureLogLikelihood = 0;

    Scalar m_logKeptWeight = 0;
};
 
namespace internal
{

struct MixtureEvidenceAccess
{
    template <typename U>
    static void
    setLogKeptWeight(U & update, typename U::Scalar value)
    {
        update.m_logKeptWeight = value;
    }

    template <typename U>
    [[nodiscard]] static typename U::Scalar
    logKeptWeight(U const & update)
    {
        return update.m_logKeptWeight;
    }

    template <typename U>
    static void
    setMixtureLogLikelihood(U & update, typename U::Scalar value)
    {
        update.m_mixtureLogLikelihood = value;
    }

    template <typename U>
    [[nodiscard]] static typename U::Scalar
    mixtureLogLikelihood(U const & update)
    {
        return update.m_mixtureLogLikelihood;
    }
};
 
}  // namespace internal

template <typename Space, typename MeasurementSpace, typename Scalar = double, typename Allocator = DefaultAllocator>
struct ReusableFilterUpdate : FilterUpdate<MeasurementSpace, std::dynamic_extent, Scalar, Allocator>
{
    using Base = FilterUpdate<MeasurementSpace, std::dynamic_extent, Scalar, Allocator>;  

    using MeasuredType = MeasurementUpdate<Space, MeasurementSpace, std::dynamic_extent, Scalar, Allocator>;
 
    ReusableFilterUpdate() = default;  

    explicit ReusableFilterUpdate(Allocator const & allocator)
        : Base{typename Base::Container(allocator), Scalar(0), false}
        , measured{typename MeasuredType::Container(allocator), Scalar(0)}
    {}
 
    ReusableFilterUpdate(ReusableFilterUpdate const &) = default;      
    ReusableFilterUpdate(ReusableFilterUpdate &&) noexcept = default;  

    ReusableFilterUpdate(ReusableFilterUpdate const & other, Allocator const & allocator)
        : Base{
              typename Base::Container(other.branches, typename Base::Container::allocator_type(allocator)),
              internal::MixtureEvidenceAccess::mixtureLogLikelihood(other),
              other.accepted
          }
        , measured{
              typename MeasuredType::Container(
                  other.measured.branches,
                  typename MeasuredType::Container::allocator_type(allocator)
              ),
              internal::MixtureEvidenceAccess::mixtureLogLikelihood(other.measured)
          }
    {
        internal::MixtureEvidenceAccess::setLogKeptWeight(*this, internal::MixtureEvidenceAccess::logKeptWeight(other));
    }

    ReusableFilterUpdate(ReusableFilterUpdate && other, Allocator const & allocator)
        : Base{
              typename Base::Container(std::move(other.branches), typename Base::Container::allocator_type(allocator)),
              internal::MixtureEvidenceAccess::mixtureLogLikelihood(other),
              other.accepted
          }
        , measured{
              typename MeasuredType::Container(
                  std::move(other.measured.branches),
                  typename MeasuredType::Container::allocator_type(allocator)
              ),
              internal::MixtureEvidenceAccess::mixtureLogLikelihood(other.measured)
          }
    {
        internal::MixtureEvidenceAccess::setLogKeptWeight(*this, internal::MixtureEvidenceAccess::logKeptWeight(other));
    }
 
    ReusableFilterUpdate &
    operator=(ReusableFilterUpdate const &) = default;  
 
    ReusableFilterUpdate &
    operator=(ReusableFilterUpdate &&) noexcept = default;  
 
    ~ReusableFilterUpdate() = default;
 
    MeasuredType measured;  
};
 
namespace internal
{

template <typename MeasurementSpace, std::size_t Count, typename Scalar, typename Allocator>
std::true_type
isFilterUpdateTest(FilterUpdate<MeasurementSpace, Count, Scalar, Allocator> const *);

std::false_type
isFilterUpdateTest(void const *);

template <typename U>
inline constexpr bool isFilterUpdate = decltype(isFilterUpdateTest(static_cast<U const *>(nullptr)))::value;

template <typename Space, typename MeasurementSpace, std::size_t Count, typename Scalar, typename Allocator>
struct FilterUpdateWithScratchOf
{
    using type = FilterUpdate<MeasurementSpace, Count, Scalar>;
};

template <typename Space, typename MeasurementSpace, typename Scalar, typename Allocator>
struct FilterUpdateWithScratchOf<Space, MeasurementSpace, std::dynamic_extent, Scalar, Allocator>
{
    using type = ReusableFilterUpdate<Space, MeasurementSpace, Scalar, Allocator>;  
};


template <typename Space, typename MeasurementSpace, std::size_t Count, typename Scalar, typename Allocator>
using FilterUpdateWithScratch =
    typename FilterUpdateWithScratchOf<Space, MeasurementSpace, Count, Scalar, Allocator>::type;

template <typename Update, typename Allocator>
struct RebindUpdateAllocatorOf
{
    using type = Update;
};

template <typename Space, typename MeasurementSpace, typename Scalar, typename Old, typename Allocator>
struct RebindUpdateAllocatorOf<ReusableFilterUpdate<Space, MeasurementSpace, Scalar, Old>, Allocator>
{
    using type = ReusableFilterUpdate<Space, MeasurementSpace, Scalar, Allocator>;
};
 
template <typename Update, typename Error, typename Allocator>
struct RebindUpdateAllocatorOf<std::expected<Update, Error>, Allocator>
{
    using type = std::expected<typename RebindUpdateAllocatorOf<Update, Allocator>::type, Error>;
};


template <typename Update, typename Allocator>
using RebindUpdateAllocator = typename RebindUpdateAllocatorOf<Update, Allocator>::type;

template <typename Update, typename Allocator>
concept EmptyFromAllocator = std::is_constructible_v<Update, Allocator const &>;

template <typename Update, typename Allocator>
    requires EmptyFromAllocator<Update, Allocator>
[[nodiscard]] Update
emptyUpdate(Allocator const & allocator)
{
    return Update(allocator);
}
 
}  // namespace internal
 
}  // namespace Emrys