Simulation.h

Go to the documentation of this file.

#pragma once

 
#include <random>
 
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Multiply.h>
#include <EmrysFilter/Core/Time.h>
#include <EmrysFilter/Core/Weights.h>
#include <EmrysFilter/Distributions/Categorical.h>
#include <EmrysFilter/Distributions/ContinuousTimeMarkovChain.h>
#include <EmrysFilter/Distributions/Moments.h>
#include <EmrysFilter/Distributions/Sampling.h>
#include <EmrysFilter/Models/Measurement.h>
#include <EmrysFilter/Models/ProcessModel.h>
#include <EmrysFilter/Spaces/Space.h>
 
namespace Emrys
{
 
namespace internal
{

template <typename Space_, typename Vector, typename Noise, typename G>
[[nodiscard]] Vector
drawProcessNoise(Space_ const & space, Vector const & moved, Eigen::MatrixBase<Noise> const & noise, G & rng)
{
    using Scalar = typename Vector::Scalar;
    using Difference = DifferenceVector<Space_, Scalar>;
    Difference z = Difference::Zero(differenceDimension(space));
    fillStandardNormal(z, rng);
    auto const factor = defaultRoot(noise);
    return Emrys::plus(space, moved, Difference{multiply(factor, z)});
}

template <typename Distribution, typename G>
[[nodiscard]] auto
drawMeasurementNoise(Distribution const & distribution, G & rng)
{
    if constexpr (requires { distribution.toDensity(); })
    {
        auto const density = distribution.toDensity();
        EMRYS_FILTER_ASSERT(density && "the measurement's noise must factorize");
        return drawSample(*density, rng);
    }
    else
    {
        return drawSample(distribution, rng);
    }
}

template <typename R, typename Derived, typename Scalar, typename G>
[[nodiscard]] typename Moments<typename R::SpaceType, Scalar>::VectorType
drawStep(R const & resolved, Eigen::MatrixBase<Derived> const & x, Scalar step, G & rng)
{
    using Vector = typename Moments<typename R::SpaceType, Scalar>::VectorType;
    auto const draws = std::apply(
        [&](auto const &... inputs) {
            return std::tuple{drawMeasurementNoise(inputOf(inputs), rng)...};
        },
        resolved.inputs()
    );
    return std::apply(
        [&](auto const &... drawn) {
            return Vector{valueOf(resolved.propagate(x.derived(), step, drawn...))};
        },
        draws
    );
}
 
}  // namespace internal

template <Space Space_, typename Derived, ProcessModel Model, std::uniform_random_bit_generator G, typename... Args>
[[nodiscard]] typename Moments<Space_, typename Derived::Scalar>::VectorType
simulate(
    Space_ const & space,
    Eigen::MatrixBase<Derived> const & x,
    Model const & model,
    G & rng,
    TimeStep<typename Derived::Scalar> auto const & dt,
    Args const &... args
)
{
    using Scalar = typename Derived::Scalar;
    EMRYS_FILTER_ASSERT(x.size() == valueDimension(space) && "the state does not fit the space");
    internal::assertPartitions(model, space);
    Scalar const step = internal::toScalarTime<Scalar>(dt);
    auto const call = internal::modelWith(std::cref(model), args...);
    auto const resolved = internal::resolveProcess(call, space);
    return internal::drawProcessNoise(
        space,
        internal::drawStep(resolved, x, step, rng),
        resolved.template noise<Scalar>(step),
        rng
    );
}

template <Space Space_, typename Derived, typename Model, std::uniform_random_bit_generator G, typename... Args>
    requires isFixedStep<Model>
[[nodiscard]] typename Moments<Space_, typename Derived::Scalar>::VectorType
simulate(Space_ const & space, Eigen::MatrixBase<Derived> const & x, Model const & model, G & rng, Args const &... args)
{
    using Scalar = typename Derived::Scalar;
    EMRYS_FILTER_ASSERT(x.size() == valueDimension(space) && "the state does not fit the space");
    internal::assertPartitions(model, space, "the model must fit the state space");
    auto const call = internal::modelWith(std::cref(model.model()), args...);
    auto const resolved = internal::resolveProcess(call, space);
    return internal::drawProcessNoise(
        space,
        internal::drawStep(resolved, x, Scalar(model.timeStep()), rng),
        model.noise().template cast<Scalar>(),
        rng
    );
}

template <Space Space_, typename Derived, typename Step, std::uniform_random_bit_generator G, typename... Args>
    requires isUnboundStep<Step>
[[nodiscard]] typename Moments<Space_, typename Derived::Scalar>::VectorType
simulate(Space_ const & space, Eigen::MatrixBase<Derived> const & x, Step const & step, G & rng, Args const &... args)
{
    return simulate(space, x, step.model(), rng, step.timeStep(), args...);
}

template <Space Space_, typename Derived, MeasurementModel M, std::uniform_random_bit_generator G, typename... Args>
[[nodiscard]] auto
simulate(Space_ const & space, Eigen::MatrixBase<Derived> const & x, M const & model, G & rng, Args const &... args)
{
    using Scalar = typename Derived::Scalar;
    EMRYS_FILTER_ASSERT(x.size() == valueDimension(space) && "the state does not fit the space");
    auto const observed = internal::modelWith(std::cref(model), args...);
    auto const resolved = internal::resolveMeasurement(observed, space);
    using R = std::remove_cvref_t<decltype(resolved)>;
    static_assert(
        R::HasFunction || R::HasSample,
        "a measurement model known by its likelihood alone has no function to draw a measurement from; give it "
        "sample(g), a draw of the observation at a state"
    );
    // Every input drawn first, in the model's order, then the model's own draw, or the function at
    // the state and the draws and then the noise.
    auto const draws = std::apply(
        [&](auto const &... inputs) {
            return std::tuple{internal::drawMeasurementNoise(inputs, rng)...};
        },
        resolved.inputs()
    );
    if constexpr (!R::HasFunction)
    {
        return std::apply(
            [&](auto const &... drawn) {
                return resolved.sampleAtState(x, rng, drawn...);
            },
            draws
        );
    }
    else
    {
        auto const & measurementSpace = resolved.space();
        using Vector = typename Moments<typename R::Out, Scalar>::VectorType;
        Vector predicted = std::apply(
            [&](auto const &... drawn) {
                return internal::valueOf(resolved.evaluateAtState(x, drawn...)).eval();
            },
            draws
        );
        normalizeValues(measurementSpace, predicted);
        if constexpr (R::HasAdditiveNoise)
        {
            auto const noise = internal::drawMeasurementNoise(resolved.noise().distribution(), rng);
            return Vector{Emrys::plus(measurementSpace, predicted, noise.template cast<Scalar>())};
        }
        else
        {
            return predicted;
        }
    }
}

template <
    FixedSizeSpace Space_,
    typename Derived,
    typename Model,
    std::uniform_random_bit_generator G,
    typename... Inputs>
    requires(Space<Space_> && !isFixedStep<Model> && !isUnboundStep<Model>)
[[nodiscard]] typename Moments<Space_, typename Derived::Scalar>::VectorType
simulate(
    Eigen::MatrixBase<Derived> const & x,
    Model const & model,
    G & rng,
    TimeStep<typename Derived::Scalar> auto const & dt,
    Inputs const &... inputs
)
{
    return simulate(Space_{}, x, model, rng, dt, inputs...);
}

template <
    FixedSizeSpace Space_,
    typename Derived,
    typename Model,
    std::uniform_random_bit_generator G,
    typename... Inputs>
    requires(Space<Space_> && (isFixedStep<Model> || isUnboundStep<Model>))
[[nodiscard]] typename Moments<Space_, typename Derived::Scalar>::VectorType
simulate(Eigen::MatrixBase<Derived> const & x, Model const & model, G & rng, Inputs const &... inputs)
{
    return simulate(Space_{}, x, model, rng, inputs...);
}

template <
    FixedSizeSpace Space_,
    typename Derived,
    MeasurementModel M,
    std::uniform_random_bit_generator G,
    typename... Args>
    requires Space<Space_>
[[nodiscard]] auto
simulate(Eigen::MatrixBase<Derived> const & x, M const & model, G & rng, Args const &... args)
{
    return simulate(Space_{}, x, model, rng, args...);
}

template <typename Space, typename Scalar, std::uniform_random_bit_generator G>
[[nodiscard]] Eigen::Index
simulate(Eigen::Index from, MarkovChain<Space, Scalar> const & chain, G & rng)
{
    EMRYS_FILTER_ASSERT(from >= 0 && from < chain.space().cardinality() && "the value is not in the chain's space");
    return internal::drawIndex<Scalar>(
        chain.space().cardinality(),
        [&](Eigen::Index to) {
            return chain.transition(from, to);
        },
        rng
    );
}

template <typename Space, typename Scalar, std::uniform_random_bit_generator G>
[[nodiscard]] Eigen::Index
simulate(
    Eigen::Index from,
    ContinuousTimeMarkovChain<Space, Scalar> const & chain,
    G & rng,
    TimeStep<Scalar> auto const & dt
)
{
    return simulate(from, chain.transition(dt), rng);
}
 
}  // namespace Emrys