Methods.h

Go to the documentation of this file.

#pragma once

 
#include <array>
#include <cmath>
#include <concepts>
#include <cstddef>
#include <limits>
#include <tuple>
#include <type_traits>
#include <utility>
 
#include <Eigen/Eigenvalues>
 
#include <EmrysFilter/Core/Forwarding.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
#include <EmrysFilter/Distributions/Roots.h>
#include <EmrysFilter/Spaces/TagPath.h>
 
namespace Emrys
{

template <typename Value, typename Jacobian>
struct WithJacobian
{
    Value value;        
    Jacobian jacobian;  
};
 
namespace internal
{

template <typename T>
struct PlainResultOf
{
    using type = T;
};

template <MatrixExpression T>
struct PlainResultOf<T>
{
    using type = typename T::PlainObject;
};
 
template <typename... Blocks>
struct PlainResultOf<std::tuple<Blocks...>>
{
    using type = std::tuple<typename PlainResultOf<std::remove_cvref_t<Blocks>>::type...>;
};


template <typename T>
using PlainResult = typename PlainResultOf<T>::type;

template <typename F, typename... Args>
[[nodiscard]] decltype(auto)
callEvaluated(F & f, Args &&... args)
{
    using Returned = std::remove_cvref_t<decltype(f(std::forward<Args>(args)...))>;
    if constexpr (MatrixExpression<Returned> && !std::same_as<PlainResult<Returned>, Returned>)
    {
        return PlainResult<Returned>{f(std::forward<Args>(args)...)};
    }
    else
    {
        return f(std::forward<Args>(args)...);
    }
}

template <typename F, typename Scalar, typename... Args>
concept TakesScalar = requires(F & f, Args &&... args) { f.template operator()<Scalar>(std::forward<Args>(args)...); };

template <typename Scalar, typename F>
struct WithScalar
{
    F & f;  

    template <typename... Args>
        requires(TakesScalar<F, Scalar, Args...> || std::invocable<F &, Args...>)
    [[nodiscard]] decltype(auto)
    operator()(Args &&... args) const
    {
        if constexpr (TakesScalar<F, Scalar, Args...>)
        {
            return f.template operator()<Scalar>(std::forward<Args>(args)...);
        }
        else
        {
            return f(std::forward<Args>(args)...);
        }
    }
};

template <typename Scalar, typename F, typename... Args>
[[nodiscard]] decltype(auto)
callWithScalar(F & f, Args &&... args)
{
    auto const supplied = WithScalar<Scalar, F>{f};
    return callEvaluated(supplied, std::forward<Args>(args)...);
}

template <typename F, typename In, typename Out, typename X, typename... Rest>
inline constexpr int functionForm = [] {
    using G = WithScalar<typename X::Scalar, F const> const &;
    return std::invocable<G, In const &, Out const &, X const &, Rest...> ? 3
         : std::invocable<G, In const &, X const &, Rest...>              ? 2
         : std::invocable<G, X const &, Rest...>                          ? 1
                                                                          : 0;
}();

template <typename F, typename In, typename Out, typename X, typename... Rest>
[[nodiscard]] decltype(auto)
callForm(F const & f, In const & in, [[maybe_unused]] Out const & out, X const & x, Rest &&... rest)
{
    constexpr int Form = functionForm<F, In, Out, X, Rest &&...>;
    static_assert(
        Form != 0,
        "a model's function takes h(x), h(in, x), or h(in, out, x), and a process model's f(x, dt), f(in, x, dt), or "
        "f(in, out, x, dt), with in the input space, out the output space, and x a vector in the input space; a "
        "batched function takes its output Y last; and each may take x's scalar as its one template parameter, "
        "[]<typename T>(auto const & x)"
    );
    using Scalar = typename X::Scalar;
    if constexpr (Form == 3)
    {
        return callWithScalar<Scalar>(f, in, out, x, std::forward<Rest>(rest)...);
    }
    else if constexpr (Form == 2)
    {
        return callWithScalar<Scalar>(f, in, x, std::forward<Rest>(rest)...);
    }
    else
    {
        return callWithScalar<Scalar>(f, x, std::forward<Rest>(rest)...);
    }
}
 
}  // namespace internal

template <typename Value, typename Jacobian>
[[nodiscard]] WithJacobian<
    internal::PlainResult<std::remove_cvref_t<Value>>,
    internal::PlainResult<std::remove_cvref_t<Jacobian>>>
withJacobian(Value && value, Jacobian && jacobian)
{
    return {std::forward<Value>(value), std::forward<Jacobian>(jacobian)};
}

inline constexpr int MaxPointCount = 4096;
 
namespace internal
{

template <typename Scalar>
inline constexpr bool takesDoubleConstants = std::same_as<Scalar, float> || std::same_as<Scalar, double>;

template <typename Scalar, int Order>
[[nodiscard]] std::pair<Eigen::Matrix<Scalar, Order, 1>, Eigen::Matrix<Scalar, Order, 1>>
hermiteRule()
{
    using Vector = Eigen::Matrix<Scalar, Order, 1>;
    using Matrix = Eigen::Matrix<Scalar, Order, Order>;
    Matrix recursion = Matrix::Zero();
    for (int k = 1; k < Order; ++k)
    {
        recursion(k - 1, k) = Eigen::numext::sqrt(Scalar(k));
        recursion(k, k - 1) = recursion(k - 1, k);
    }
    auto const solver = Eigen::SelfAdjointEigenSolver<Matrix>{recursion, Eigen::EigenvaluesOnly};
    Vector const nodes = solver.eigenvalues();
    Scalar factorial = 1;
    for (int k = 2; k <= Order; ++k)
    {
        factorial *= Scalar(k);
    }
    auto weights = Vector{};
    for (int i = 0; i < Order; ++i)
    {
        // H_{p-1}(x) by H_{k+1} = x H_k - k H_{k-1}, from H_0 = 1 and H_1 = x.
        Scalar previous = 1;
        Scalar current = nodes(i);
        for (int k = 1; k < Order - 1; ++k)
        {
            Scalar const next = nodes(i) * current - Scalar(k) * previous;
            previous = current;
            current = next;
        }
        Scalar const value = Order == 1 ? Scalar(1) : current;
        weights(i) = factorial / (Scalar(Order) * Scalar(Order) * value * value);
    }
    return {nodes, weights};
}
 
}  // namespace internal

template <typename Scalar>
struct UnscentedParams
{
    Scalar alpha;  
    Scalar beta;   
    Scalar kappa;  
};

namespace methods
{

struct Linearized
{};

[[nodiscard]] constexpr Linearized
linearized()
{
    return {};
}

template <double Alpha = 1.0, double Beta = 2.0, double Kappa = 0.0>
struct Unscented
{
    static_assert(Alpha > 0, "the unscented transform's alpha must be positive");
 
    static constexpr double alpha = Alpha;  
    static constexpr double beta = Beta;    
    static constexpr double kappa = Kappa;  

    template <typename Scalar>
    [[nodiscard]] static constexpr UnscentedParams<Scalar>
    params(Eigen::Index /*n*/)
    {
        return {Scalar(Alpha), Scalar(Beta), Scalar(Kappa)};
    }
};

template <double Alpha = 1.0, double Beta = 2.0, double Kappa = 0.0>
[[nodiscard]] constexpr Unscented<Alpha, Beta, Kappa>
unscented()
{
    return {};
}

struct FourthMomentUnscented
{
    template <typename Scalar>
    [[nodiscard]] static constexpr UnscentedParams<Scalar>
    params(Eigen::Index n)
    {
        return {Scalar(1), Scalar(0), Scalar(3) - Scalar(n)};
    }
};

[[nodiscard]] constexpr FourthMomentUnscented
fourthMomentUnscented()
{
    return {};
}

struct Cubature
{
    [[nodiscard]] static constexpr Eigen::Index
    pointCount(Eigen::Index n)
    {
        return 2 * n;
    }

    template <typename Points, typename Weights>
    static Eigen::Index
    unitPoints(Eigen::Index n, Points & points, Weights & weights)
    {
        using Scalar = typename Points::Scalar;
        constexpr int N = Points::RowsAtCompileTime;
        points.setZero();
        if constexpr (N != Eigen::Dynamic && N > 0 && internal::takesDoubleConstants<Scalar>)
        {
            constexpr Scalar radius = Scalar(internal::compileTimeSqrt(double(N)));
            points.diagonal().setConstant(radius);
            points.rightCols(N).diagonal().setConstant(-radius);
            weights.setConstant(Scalar(1.0 / (2.0 * N)));
        }
        else
        {
            Scalar const radius = Eigen::numext::sqrt(Scalar(n));
            for (Eigen::Index i = 0; i < n; ++i)
            {
                points(i, i) = radius;
                points(i, n + i) = -radius;
            }
            weights.setConstant(Scalar(1) / Scalar(2 * n));
        }
        return 0;
    }
};

[[nodiscard]] constexpr Cubature
cubature()
{
    return {};
}

template <double CenterWeight = 0.0>
struct SphericalSimplex
{
    static_assert(CenterWeight >= 0 && CenterWeight < 1, "a spherical simplex's center weight must be in [0, 1)");
 
    static constexpr double centerWeight = CenterWeight;  

    [[nodiscard]] static constexpr Eigen::Index
    pointCount(Eigen::Index n)
    {
        return n + 2;
    }

    template <typename Points, typename Weights>
    static Eigen::Index
    unitPoints(Eigen::Index n, Points & points, Weights & weights)
    {
        using Scalar = typename Points::Scalar;
        points.setZero();
        Scalar const center = Scalar(CenterWeight);
        Scalar const pointWeight = (Scalar(1) - center) / Scalar(n + 1);
        weights(0) = center;
        weights.tail(n + 1).setConstant(pointWeight);
        for (Eigen::Index j = 1; j <= n; ++j)
        {
            Scalar const step = Scalar(1) / Eigen::numext::sqrt(Scalar(j * (j + 1)) * pointWeight);
            points.row(j - 1).segment(1, j).setConstant(-step);
            points(j - 1, j + 1) = Scalar(j) * step;
        }
        return 0;
    }
};

template <double CenterWeight = 0.0>
[[nodiscard]] constexpr SphericalSimplex<CenterWeight>
sphericalSimplex()
{
    return {};
}

template <int Order = 3>
struct GaussHermite
{
    static_assert(Order >= 1, "a Gauss-Hermite rule has at least one point per coordinate");
    static constexpr int PointsPerCoordinate = Order;  

    [[nodiscard]] static constexpr Eigen::Index
    pointCount(Eigen::Index n)
    {
        Eigen::Index count = 1;
        for (Eigen::Index i = 0; i < n && count <= MaxPointCount; ++i)
        {
            count *= Order;
        }
        return std::min<Eigen::Index>(count, MaxPointCount + 1);
    }

    template <typename Points, typename Weights>
    static Eigen::Index
    unitPoints(Eigen::Index n, Points & points, Weights & weights)
    {
        using Scalar = typename Points::Scalar;
        Eigen::Index const count = weights.size();
        EMRYS_FILTER_ASSERT(
            count <= MaxPointCount && "a Gauss-Hermite rule over this many coordinates has too many points"
        );
        auto const [nodes, nodeWeights] = internal::hermiteRule<Scalar, Order>();
        for (Eigen::Index k = 0; k < count; ++k)
        {
            Eigen::Index digits = k;
            Scalar weight = 1;
            for (Eigen::Index i = 0; i < n; ++i)
            {
                Eigen::Index const digit = digits % Order;
                digits /= Order;
                points(i, k) = nodes(digit);
                weight *= nodeWeights(digit);
            }
            weights(k) = weight;
        }
        Eigen::Index reference = 0;
        weights.maxCoeff(&reference);
        return reference;
    }
};

template <int Order = 3>
[[nodiscard]] constexpr GaussHermite<Order>
gaussHermite()
{
    return {};
}
 
}  // namespace methods

template <typename T>
concept UnscentedRule = requires(Eigen::Index n) {
    { T::template params<double>(n) } -> std::same_as<UnscentedParams<double>>;
};

template <typename T>
concept PointSet = requires(Eigen::Index n, Eigen::MatrixXd & points, Eigen::VectorXd & weights) {
    { T::pointCount(n) } -> std::convertible_to<Eigen::Index>;
    { T::unitPoints(n, points, weights) } -> std::same_as<Eigen::Index>;
};
 
namespace methods
{

template <typename Rule, typename A>
struct Spread
{
    static_assert(
        UnscentedRule<Rule> || PointSet<Rule>,
        "spread() takes a sigma-point rule, an UnscentedRule or a PointSet, not linearized(), iterated(), or a "
        "rule spread already"
    );
    static_assert(Axes<A>, "spread() takes axes, such as roots::principal(), after its rule");
 
    Rule rule{};  
    A axes{};     
};

template <typename Rule, typename A>
[[nodiscard]] constexpr Spread<Rule, A>
spread(Rule rule, A axes)
{
    return {std::move(rule), std::move(axes)};
}
 
}  // namespace methods
 
namespace internal
{

template <typename T>
inline constexpr bool isPointSet = PointSet<T>;

template <typename T>
inline constexpr bool isUnscented = UnscentedRule<T>;

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

template <typename Rule, typename A>
inline constexpr bool isSpread<methods::Spread<Rule, A>>{true};


template <typename T>
inline constexpr bool isSigmaPointMethod = isUnscented<T> || isPointSet<T> || isSpread<T>;

template <typename Method>
[[nodiscard]] constexpr decltype(auto)
ruleOf(Method const & method)
{
    if constexpr (isSpread<Method>)
    {
        return (method.rule);
    }
    else
    {
        return (method);
    }
}

template <typename Method>
[[nodiscard]] constexpr decltype(auto)
axesOf(Method const & method)
{
    if constexpr (isSpread<Method>)
    {
        return (method.axes);
    }
    else
    {
        return roots::DefaultAxes{};
    }
}

template <typename Method>
using RuleOf = std::remove_cvref_t<decltype(ruleOf(std::declval<Method const &>()))>;

template <typename Method>
using AxesOf = std::remove_cvref_t<decltype(axesOf(std::declval<Method const &>()))>;

template <typename A>
consteval void
refuseFailingAxes()
{
    static_assert(
        !A::canFail,
        "a transform returns no std::expected, so its axes cannot fail: append roots::pivoted() to the list, or take "
        "the root with roots::squareRoot(), which returns one"
    );
}

template <typename Result>
[[nodiscard]] decltype(auto)
rootValue(Result && result)
{
    if constexpr (requires { result.has_value(); })
    {
        EMRYS_FILTER_ASSERT(result.has_value() && "a transform's axes cannot fail");
        return *std::forward<Result>(result);
    }
    else
    {
        return std::forward<Result>(result);
    }
}

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

template <>
inline constexpr bool isLinearMethod<methods::Linearized> = true;

 
}  // namespace internal

template <RealScalar Scalar = double>
struct IteratedOptions
{
    Scalar tolerance = Scalar(1e-3);  
    std::size_t maxIterations = 10;   
};
 
namespace methods
{

template <typename Method = Linearized, RealScalar Scalar = double>
struct Iterated
{
    static_assert(
        internal::isLinearMethod<Method> || internal::isSigmaPointMethod<Method>,
        "Iterated relinearizes Linearized or a sigma-point method (an UnscentedRule such as Unscented, or a PointSet "
        "such as Cubature, SphericalSimplex, GaussHermite, or the user's own) (decisions.md [iterated-update], "
        "[sigma-point-sets])"
    );
 
    Method method{};                  
    Scalar tolerance = Scalar(1e-3);  
    std::size_t maxIterations = 10;   
};

template <RealScalar Scalar = double, typename Method = Linearized>
[[nodiscard]] Iterated<Method, Scalar>
iterated(Method method = {}, IteratedOptions<Scalar> options = {})
{
    EMRYS_FILTER_ASSERT(options.tolerance >= Scalar(0) && "an iteration's tolerance must not be negative");
    EMRYS_FILTER_ASSERT(options.maxIterations >= 1 && "an Iterated method makes at least one linearization");
    return {std::move(method), options.tolerance, options.maxIterations};
}
 
}  // namespace methods
 
namespace internal
{

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

template <typename Method, typename Scalar>
inline constexpr bool isIterated<methods::Iterated<Method, Scalar>>{true};

 
}  // namespace internal

template <typename M>
concept TransformMethod =
    internal::isLinearMethod<std::remove_cvref_t<M>> || internal::isSigmaPointMethod<std::remove_cvref_t<M>>
    || internal::isIterated<std::remove_cvref_t<M>>;
 
}  // namespace Emrys