Consistency.h

Go to the documentation of this file.

#pragma once

 
#include <algorithm>
#include <array>
#include <cstddef>
#include <expected>
#include <limits>
#include <type_traits>
#include <utility>
 
#include <EmrysFilter/Core/Allocation.h>
#include <EmrysFilter/Core/ChiSquared.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
#include <EmrysFilter/Core/NumericError.h>
#include <EmrysFilter/Spaces/TagPath.h>
#include <EmrysFilter/Distributions/GaussianForm.h>
#include <EmrysFilter/Models/Measurement.h>
 
namespace Emrys
{

inline constexpr int Cumulative = -2;
 
static_assert(Cumulative != Eigen::Dynamic, "a cumulative window is not a dynamic one");

enum class Consistency
{
    Consistent,      
    Overconfident,   
    Underconfident,  
};
 
namespace internal
{

template <typename Derived, typename Scalar>
class ChiSquaredTest
{
public:

    [[nodiscard]] std::expected<Scalar, NumericError>
    average() const
    {
        if (derived().count() == 0)
        {
            return std::unexpected{NumericError::Empty};
        }
        Scalar const total = derived().sum();
        if (isNaN(total))
        {
            return std::unexpected{NumericError::NonFinite};
        }
        return total / internal::scalarFromCount<Scalar>(derived().degreesOfFreedom());
    }

    [[nodiscard]] std::expected<std::pair<Scalar, Scalar>, NumericError>
    bounds(Scalar probability) const
    {
        if (derived().count() == 0)
        {
            return std::unexpected{NumericError::Empty};
        }
        Scalar const dof = internal::scalarFromCount<Scalar>(derived().degreesOfFreedom());
        return std::pair<Scalar, Scalar>{
            chiSquaredQuantile<Scalar>((Scalar(1) - probability) / Scalar(2), dof),
            chiSquaredQuantile<Scalar>((Scalar(1) + probability) / Scalar(2), dof)
        };
    }

    [[nodiscard]] std::expected<Consistency, NumericError>
    verdict(Scalar probability) const
    {
        auto const range = bounds(probability);
        if (!range)
        {
            return std::unexpected{range.error()};
        }
        Scalar const total = derived().sum();
        if (isNaN(total))
        {
            return std::unexpected{NumericError::NonFinite};
        }
        if (total > range->second)
        {
            return Consistency::Overconfident;
        }
        if (total < range->first)
        {
            return Consistency::Underconfident;
        }
        return Consistency::Consistent;
    }
 
private:

    [[nodiscard]] Derived const &
    derived() const
    {
        return static_cast<Derived const &>(*this);
    }
};

template <int Window, int MaxWindow, typename Scalar, typename Allocator>
class ChiSquaredWindow : public ChiSquaredTest<ChiSquaredWindow<Window, MaxWindow, Scalar, Allocator>, Scalar>
{
    static_assert(Window == Eigen::Dynamic || Window > 0, "a window holds at least one statistic");
    static_assert(
        Window == Eigen::Dynamic ? MaxWindow == Eigen::Dynamic || MaxWindow > 0 : MaxWindow == Window,
        "a window's bound is positive, and a fixed window is its own bound"
    );
 
    using Entry = std::pair<Scalar, Eigen::Index>;               
    static constexpr bool Inline = MaxWindow != Eigen::Dynamic;  
    using Entries = std::conditional_t<
        Inline,
        std::array<Entry, std::size_t(Inline ? MaxWindow : 1)>,
        internal::Buffer<Entry, Allocator>>;  
    using Length = std::conditional_t<
        Window == Eigen::Dynamic,
        Eigen::Index,
        std::integral_constant<Eigen::Index, Window == Eigen::Dynamic ? 1 : Window>>;  
 
public:
 
    using allocator_type = Allocator;  

    ChiSquaredWindow()
        requires(Window != Eigen::Dynamic)
    = default;

    ChiSquaredWindow(Eigen::Index window, Allocator const & allocator)
        requires(Window == Eigen::Dynamic)
        : m_window{window}
        , m_entries(entries(window, allocator))
    {
        EMRYS_FILTER_ASSERT(window > 0 && "the window must hold at least one statistic");
        EMRYS_FILTER_ASSERT((!Inline || window <= MaxWindow) && "the window must be within its bound");
    }

    ChiSquaredWindow(ChiSquaredWindow const & other, Allocator const & allocator)
        requires(!Inline)
        : m_window{other.m_window}
        , m_entries(other.m_entries, internal::ReBound<Allocator, Entry>(allocator))
        , m_count{other.m_count}
        , m_next{other.m_next}
    {}

    [[nodiscard]] Allocator
    get_allocator() const
        requires(!Inline)
    {
        return Allocator(m_entries.get_allocator());
    }

    void
    add(Scalar value, Eigen::Index dimension)
    {
        EMRYS_FILTER_ASSERT(dimension > 0 && "the statistic must have at least one degree of freedom");
        m_entries[std::size_t(m_next)] = Entry{value, dimension};
        m_next = m_next + 1 == window() ? 0 : m_next + 1;
        m_count = std::min(m_count + 1, window());
    }

    void
    reset()
    {
        m_count = 0;
        m_next = 0;
    }

    [[nodiscard]] Eigen::Index
    window() const
    {
        return m_window;
    }

    [[nodiscard]] Eigen::Index
    count() const
    {
        return m_count;
    }

    [[nodiscard]] Scalar
    sum() const
    {
        Scalar total = 0;
        for (Eigen::Index i = 0; i < m_count; ++i)
        {
            total += m_entries[std::size_t(i)].first;
        }
        return total;
    }

    [[nodiscard]] Eigen::Index
    degreesOfFreedom() const
    {
        Eigen::Index total = 0;
        for (Eigen::Index i = 0; i < m_count; ++i)
        {
            total += m_entries[std::size_t(i)].second;
        }
        return total;
    }
 
private:

    [[nodiscard]] static Entries
    entries(Eigen::Index window, Allocator const & allocator)
    {
        if constexpr (Inline)
        {
            return Entries{};
        }
        else
        {
            return Entries(std::size_t(window), Entry{}, internal::ReBound<Allocator, Entry>(allocator));
        }
    }
 
    EMRYS_FILTER_NO_UNIQUE_ADDRESS Length m_window{};  
    Entries m_entries{};                               
    Eigen::Index m_count = 0;                          
    Eigen::Index m_next = 0;                           
};

template <typename Scalar>
class ChiSquaredSum : public ChiSquaredTest<ChiSquaredSum<Scalar>, Scalar>
{
public:

    void
    add(Scalar value, Eigen::Index dimension)
    {
        EMRYS_FILTER_ASSERT(dimension > 0 && "the statistic must have at least one degree of freedom");
        m_sum += value;
        m_degreesOfFreedom += dimension;
        ++m_count;
    }

    void
    reset()
    {
        m_sum = 0;
        m_degreesOfFreedom = 0;
        m_count = 0;
    }

    [[nodiscard]] Eigen::Index
    count() const
    {
        return m_count;
    }

    [[nodiscard]] Scalar
    sum() const
    {
        return m_sum;
    }

    [[nodiscard]] Eigen::Index
    degreesOfFreedom() const
    {
        return m_degreesOfFreedom;
    }
 
private:
 
    Scalar m_sum = 0;                     
    Eigen::Index m_degreesOfFreedom = 0;  
    Eigen::Index m_count = 0;             
};

template <int Window, int MaxWindow, typename Scalar, typename Allocator>
using ChiSquaredWindowFor = std::conditional_t<
    Window == Cumulative,  // a sum since the first statistic, or a ring of the latest
    ChiSquaredSum<Scalar>,
    ChiSquaredWindow<Window, MaxWindow, Scalar, Allocator>>;
 
}  // namespace internal

template <
    int Window = Eigen::Dynamic,
    int MaxWindow = Window,
    typename Scalar = double,
    typename Allocator = DefaultAllocator>
class NisMonitor : public internal::ChiSquaredWindowFor<Window, MaxWindow, Scalar, Allocator>
{
    static_assert(Window != Cumulative || MaxWindow == Cumulative, "a cumulative window has no bound");
 
    using Base = internal::ChiSquaredWindowFor<Window, MaxWindow, Scalar, Allocator>;  
 
public:

    NisMonitor()
        requires(Window != Eigen::Dynamic)
    = default;

    explicit NisMonitor(Eigen::Index window, Allocator const & allocator = {})
        requires(Window == Eigen::Dynamic)
        : Base{window, allocator}
    {}

    NisMonitor(NisMonitor const & other, Allocator const & allocator)
        requires(Window == Eigen::Dynamic && MaxWindow == Eigen::Dynamic)
        : Base{other, allocator}
    {}
 
    using Base::add;

    template <typename Update>
        requires requires(Update const & update) {
            update.branches.front().nis();
            update.innovationSize();
        }
    void
    add(Update const & update)
    {
        EMRYS_FILTER_ASSERT(
            update.branches.size() == 1 && "with several branches, pass the NIS to test with add(nis, dimension)"
        );
        add(update.branches.front().nis(), update.innovationSize());
    }
};

template <GaussianForm Estimate, typename Derived>
[[nodiscard]] std::expected<typename Estimate::Scalar, GaussianError>
nees(Eigen::MatrixBase<Derived> const & truth, Estimate const & estimate)
{
    EMRYS_FILTER_ASSERT(truth.size() == estimate.valueDimension() && "the true state must fit the estimate's space");
    if constexpr (requires { estimate.toDensity(); })
    {
        auto const density = estimate.toDensity();
        if (!density)
        {
            return std::unexpected{density.error()};
        }
        return typename Estimate::Scalar(density->squaredMahalanobisDistance(truth));
    }
    else
    {
        auto const params = toGaussian(estimate);
        if (!params)
        {
            return std::unexpected{params.error()};
        }
        return nees(truth, *params);
    }
}

template <typename Path, GaussianForm Estimate, typename Derived>
    requires requires(Estimate const & estimate) { estimate.template marginal<Path>(); }
[[nodiscard]] std::expected<typename Estimate::Scalar, GaussianError>
nees(Eigen::MatrixBase<Derived> const & truth, Estimate const & estimate)
{
    EMRYS_FILTER_ASSERT(truth.size() == estimate.valueDimension() && "the true state must fit the estimate's space");
    // block() is a vector even for one coordinate, where truth(idx<Path>()) would be a scalar.
    auto const block = Emrys::block<Path>(estimate.space(), truth);
    auto const marginal = estimate.template marginal<Path>();
    if constexpr (internal::isExpected<std::remove_cvref_t<decltype(marginal)>>)
    {
        if (!marginal)
        {
            return std::unexpected{marginal.error()};
        }
        return nees(block, *marginal);
    }
    else
    {
        return nees(block, marginal);
    }
}

template <
    int Window = Eigen::Dynamic,
    int MaxWindow = Window,
    typename Scalar = double,
    typename Allocator = DefaultAllocator>
class NeesMonitor : public internal::ChiSquaredWindowFor<Window, MaxWindow, Scalar, Allocator>
{
    static_assert(Window != Cumulative || MaxWindow == Cumulative, "a cumulative window has no bound");
 
    using Base = internal::ChiSquaredWindowFor<Window, MaxWindow, Scalar, Allocator>;  
 
public:

    NeesMonitor()
        requires(Window != Eigen::Dynamic)
    = default;

    explicit NeesMonitor(Eigen::Index window, Allocator const & allocator = {})
        requires(Window == Eigen::Dynamic)
        : Base{window, allocator}
    {}

    NeesMonitor(NeesMonitor const & other, Allocator const & allocator)
        requires(Window == Eigen::Dynamic && MaxWindow == Eigen::Dynamic)
        : Base{other, allocator}
    {}
 
    using Base::add;

    template <GaussianForm Estimate, typename Derived>
        requires requires(Estimate const & estimate, Eigen::MatrixBase<Derived> const & truth) {
            nees(truth, estimate);
        }
    void
    add(Eigen::MatrixBase<Derived> const & truth, Estimate const & estimate)
    {
        auto const value = nees(truth, estimate);
        add(value ? Scalar(*value) : std::numeric_limits<Scalar>::quiet_NaN(), estimate.differenceDimension());
    }

    template <typename Path, typename Derived, typename Estimate>
        requires requires(Estimate const & estimate, Eigen::MatrixBase<Derived> const & truth) {
            nees<Path>(truth, estimate);
        }
    void
    add(Eigen::MatrixBase<Derived> const & truth, Estimate const & estimate)
    {
        auto const value = nees<Path>(truth, estimate);
        add(value ? Scalar(*value) : std::numeric_limits<Scalar>::quiet_NaN(),
            internal::differenceBlockRows<Path>(estimate.space()).size);
    }
};
 
}  // namespace Emrys