RbParticleFilter.h

Go to the documentation of this file.

#pragma once

 
#include <expected>
#include <optional>
#include <random>
#include <type_traits>
#include <utility>
 
#include <Eigen/Cholesky>
 
#include <EmrysFilter/Core/Allocation.h>
#include <EmrysFilter/Core/Execution.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
#include <EmrysFilter/Core/Multiply.h>
#include <EmrysFilter/Core/NumericError.h>
#include <EmrysFilter/Core/Print.h>
#include <EmrysFilter/Core/Weights.h>
#include <EmrysFilter/Distributions/Categorical.h>
#include <EmrysFilter/Distributions/Gaussian.h>
#include <EmrysFilter/Distributions/Hybrid.h>
#include <EmrysFilter/Distributions/Moments.h>
#include <EmrysFilter/Distributions/ParticleGaussian.h>
#include <EmrysFilter/Distributions/SampleSet.h>
#include <EmrysFilter/Distributions/Sampling.h>
#include <EmrysFilter/Filters/FilterUpkeep.h>
#include <EmrysFilter/Filters/ImmFilter.h>
#include <EmrysFilter/Filters/RbUpdate.h>
#include <EmrysFilter/Filters/Recursion.h>
#include <EmrysFilter/Models/Measurement.h>
#include <EmrysFilter/Models/ProcessModel.h>
#include <EmrysFilter/Models/Reduction.h>
#include <EmrysFilter/Models/Transform.h>
 
namespace Emrys
{
 
namespace internal
{

template <typename Step, typename Split, typename Pinned>
class PinnedStep
{
public:

    static constexpr bool EvaluatesBatches = requires { requires Step::EvaluatesBatches; };

    PinnedStep(Step const & step, Split const & split, Pinned const & pinned)
        : m_step{step}
        , m_split{split}
        , m_pinned{pinned}
    {}

    template <typename Rest, typename... Inputs>
    [[nodiscard]] auto
    operator()(Rest const & rest, Inputs const &... inputs) const
    {
        return m_step(m_split.assemble(rest, m_pinned), inputs...);
    }

    template <typename Y, typename Rests, typename... Inputs>
    void
    evaluateBatch(Y & y, Rests const & rests, Inputs const &... inputs) const
        requires EvaluatesBatches
    {
        using T = typename Rests::Scalar;
        using States =
            MatrixOf<T, Y::RowsAtCompileTime, Y::ColsAtCompileTime, Y::MaxRowsAtCompileTime, Y::MaxColsAtCompileTime>;
        auto states = States{};
        states.resize(y.rows(), rests.cols());
        for (Eigen::Index j = 0; j < rests.cols(); ++j)
        {
            states.col(j) = m_split.assemble(rests.col(j), m_pinned);
        }
        m_step.evaluateBatch(y, states, inputs...);
    }
 
private:
 
    Step const & m_step;      
    Split const & m_split;    
    Pinned const & m_pinned;  
};

template <typename Rest, typename Scalar, typename Allocator, typename Model, bool = isSwitchingPointModel<Model>>
struct ParticleComponentOf
{
    using type = Gaussian<Rest, Scalar>;  
};

template <typename Rest, typename Scalar, typename Allocator, typename Model>
struct ParticleComponentOf<Rest, Scalar, Allocator, Model, true>
{
    using type = Hybrid<Gaussian<Rest, Scalar>, typename Model::ModeSpaceType, Allocator>;  
};

template <typename Split, typename CovDerived, typename OffsetDerived>
[[nodiscard]] std::expected<typename CovDerived::Scalar, NumericError>
pinnedLogDensity(
    Split const & split,
    Eigen::MatrixBase<CovDerived> const & cov,
    Eigen::MatrixBase<OffsetDerived> const & offset
)
{
    using Scalar = typename CovDerived::Scalar;
    using PinnedMatrix = typename Moments<typename Split::PinnedSpace, Scalar>::MatrixType;
    constexpr int PinnedRows = Moments<typename Split::PinnedSpace, Scalar>::DifferenceDimensionAtCompileTime;
    BlockRows const rows = split.pinnedDifferenceRows();
    PinnedMatrix const pinned = blockOf<PinnedRows, PinnedRows>(cov, rows.start, rows.start, rows.size, rows.size);
    auto const llt = Eigen::LLT<PinnedMatrix>{pinned};
    if (llt.info() != Eigen::Success)
    {
        return std::unexpected{NumericError::NotPositiveDefinite};
    }
    auto const whitened = llt.matrixL().solve(offset.derived()).eval();
    return Scalar(-0.5) * whitened.squaredNorm() - llt.matrixLLT().diagonal().array().log().sum();
}
 
}  // namespace internal

template <typename Scalar = double, typename Allocator = DefaultAllocator>
class RbParticleFilterUpdate
{
public:
 
    using ArrayType = Eigen::Map<Eigen::Array<Scalar, Eigen::Dynamic, 1> const>;  
    using allocator_type = Allocator;  

    explicit RbParticleFilterUpdate(Allocator const & allocator = {})
        : m_points{allocator}
    {}

    [[nodiscard]] ArrayType
    logLikelihoods() const
    {
        return m_points.logLikelihoods();
    }

    [[nodiscard]] ArrayType
    nis() const
    {
        return m_points.nis();
    }

    [[nodiscard]] Eigen::Index
    innovationSize() const
    {
        return m_points.innovationSize;
    }

    [[nodiscard]] allocator_type
    get_allocator() const
    {
        return allocator_type(m_points.logLikelihoodData.get_allocator());
    }

    [[nodiscard]] Scalar
    logLikelihood() const
    {
        return m_logLikelihood;
    }
 
    bool accepted = false;  
 
private:
 
    friend struct internal::PointLikelihoodsAccess;
    friend struct internal::UpdateAccess;
 
    Scalar m_logLikelihood = 0;  
 
    internal::PointLikelihoods<Scalar, Allocator> m_points;  
};

template <
    Space Space_,
    typename Path_,
    typename Model_,
    typename Covariance_ = internal::PointCovarianceFor<Model_>,
    typename Method_ = DefaultMethodFor<Model_>,
    RealScalar Scalar_ = double,
    typename Allocator_ = DefaultAllocator,
    typename Upkeep_ = upkeep::None>
class RbParticleFilter
    : public internal::HeldUpkeep<
          RbParticleFilter<Space_, Path_, Model_, Covariance_, Method_, Scalar_, Allocator_, Upkeep_>,
          Upkeep_>
{
    using UpkeepBase = internal::HeldUpkeep<RbParticleFilter, Upkeep_>;  
 
    static constexpr bool Shared = std::is_same_v<Covariance_, SharedCovariance>;  

    static constexpr bool Switching = internal::isSwitchingPointModel<Model_>;
 
    using RestSpace = typename internal::BlockSplit<Space_, Path_>::RestSpace;  

    template <typename P>
    static constexpr void
    assertParticlesRunUnder()
    {
        static_assert(
            !Switching || internal::isSequential<P>,
            "a particle's IMM step writes the recursion's scratch, so the particles of a hybrid state run in "
            "order: pass Sequential"
        );
    }
 
    static_assert(
        internal::predictsBy<Model_, Method_>,
        "a filter with a fixed-step model predicts by the model's method"
    );
    static_assert(internal::checkPredictionMethod<Method_>());
 
public:
 
    using Scalar = Scalar_;      
    using SpaceType = Space_;    
    using ModelType = Model_;    
    using MethodType = Method_;  
    using StateType = ParticleGaussianParams<
        Space_,
        Path_,
        Covariance_,
        Scalar_,
        Allocator_,
        typename internal::ParticleComponentOf<RestSpace, Scalar_, Allocator_, Model_>::type>;
    using ConditionalType = typename StateType::ConditionalType;  
    using allocator_type = Allocator_;                            

    RbParticleFilter(
        std::type_identity_t<Model_> model,
        StateType state,
        MethodFor<Model_, Method_> method = {},
        std::type_identity_t<Upkeep_> upkeep = {}
    )
        : UpkeepBase{std::move(upkeep)}
        , m_steps{std::move(model), std::move(method), state.get_allocator()}
        , m_state{std::move(state)}
        , m_posterior{m_state.m_conditionals, m_state.get_allocator()}
        , m_normals(ScalarAllocator(m_state.get_allocator()))
        , m_moved(ScalarAllocator(m_state.get_allocator()))
    {
        assertModelPartitions(m_state);
        reserveScratch();
    }

    RbParticleFilter(std::type_identity_t<Model_> model, StateType state, std::type_identity_t<Upkeep_> upkeep)
        requires(!std::same_as<Upkeep_, Emrys::upkeep::None>)
        : RbParticleFilter{std::move(model), std::move(state), MethodFor<Model_, Method_>{}, std::move(upkeep)}
    {}

    [[nodiscard]] allocator_type
    get_allocator() const
    {
        return m_state.get_allocator();
    }

    [[nodiscard]] StateType const &
    state() const
    {
        return m_state;
    }

    [[nodiscard]] StateType &
    state()
    {
        return m_state;
    }

    void
    setState(StateType state)
    {
        assertModelPartitions(state);
        m_state = std::move(state);
        if (m_normals.get_allocator() != ScalarAllocator(m_state.get_allocator()))
        {
            // The assignment carried the new state's allocator in, so the scratch follows it.
            m_posterior = decltype(m_posterior){m_state.m_conditionals, m_state.get_allocator()};
            m_normals = internal::Buffer<Scalar_, Allocator_>(ScalarAllocator(m_state.get_allocator()));
            m_moved = internal::Buffer<Scalar_, Allocator_>(ScalarAllocator(m_state.get_allocator()));
            if constexpr (Switching)
            {
                m_steps.resetScratch(m_state.get_allocator());
            }
        }
        reserveScratch();
    }

    [[nodiscard]] Model_ const &
    model() const
    {
        return m_steps.model();
    }

    [[nodiscard]] Model_ &
    model()
    {
        return m_steps.model();
    }

    [[nodiscard]] Method_ const &
    method() const
    {
        return m_steps.method();
    }

    template <std::uniform_random_bit_generator G, typename... Inputs>
        requires(!isFixedStep<Model_>)
    [[nodiscard]] auto
    predict(G & rng, TimeStep<Scalar_> auto const & dt, Inputs const &... inputs)
    {
        return predict(execution::sequential, rng, dt, inputs...);
    }

    template <ExecutionPolicy P, std::uniform_random_bit_generator G, typename... Inputs>
        requires(!isFixedStep<Model_>)
    [[nodiscard]] auto
    predict(P && policy, G & rng, TimeStep<Scalar_> auto const & dt, Inputs const &... inputs)
    {
        Scalar_ const step = internal::toScalarTime<Scalar_>(dt);
        if constexpr (Switching)
        {
            assertParticlesRunUnder<P>();
            return moveHybridParticles(rng, [&](auto const & model, auto const & method, auto const & predictBy) {
                auto const call = internal::modelWith(std::cref(model), inputs...);
                auto const resolved = internal::resolveProcess(call, m_state.space());
                return std::apply(
                    [&](auto const &... uncertain) {
                        return predictBy(
                            method,
                            resolved.template noise<Scalar_>(step),
                            internal::ProcessStepValues<std::remove_cvref_t<decltype(resolved)>, Scalar_>{
                                resolved,
                                step
                            },
                            internal::inputOf(uncertain)...
                        );
                    },
                    resolved.inputs()
                );
            });
        }
        else
        {
            predictOne(std::forward<P>(policy), rng, step, inputs...);
        }
    }

    template <std::uniform_random_bit_generator G, typename... Inputs>
        requires(isFixedStep<Model_> && !internal::leadsWithSteps<Inputs...>)
    [[nodiscard]] auto
    predict(G & rng, Inputs const &... inputs)
    {
        internal::rejectTimeStep<Inputs...>();
        return predict(execution::sequential, rng, inputs...);
    }

    template <ExecutionPolicy P, std::uniform_random_bit_generator G, typename... Inputs>
        requires(isFixedStep<Model_> && !internal::leadsWithSteps<Inputs...>)
    [[nodiscard]] auto
    predict(P && policy, G & rng, Inputs const &... inputs)
    {
        internal::rejectTimeStep<Inputs...>();
        if constexpr (Switching)
        {
            assertParticlesRunUnder<P>();
            return moveHybridParticles(rng, [&](auto const & model, auto const & method, auto const & predictBy) {
                auto const call = internal::modelWith(std::cref(model.model()), inputs...);
                auto const resolved = internal::resolveProcess(call, m_state.space());
                return std::apply(
                    [&](auto const &... uncertain) {
                        return predictBy(
                            method,
                            model.noise(),
                            internal::ProcessStepValues<
                                std::remove_cvref_t<decltype(resolved)>,
                                std::remove_cvref_t<decltype(model.timeStep())>>{resolved, model.timeStep()},
                            internal::inputOf(uncertain)...
                        );
                    },
                    resolved.inputs()
                );
            });
        }
        else
        {
            predictOneFixed(std::forward<P>(policy), rng, inputs...);
        }
    }

    template <std::uniform_random_bit_generator G, typename... Inputs>
        requires isFixedStep<Model_>
    [[nodiscard]] auto
    predict(G & rng, Steps steps, Inputs const &... inputs)
    {
        return predict(execution::sequential, rng, steps, inputs...);
    }

    template <ExecutionPolicy P, std::uniform_random_bit_generator G, typename... Inputs>
        requires isFixedStep<Model_>
    [[nodiscard]] auto
    predict(P && policy, G & rng, Steps steps, Inputs const &... inputs)
    {
        EMRYS_FILTER_ASSERT(steps.count >= 0 && "the number of steps must be nonnegative");
        if constexpr (Switching)
        {
            for (Eigen::Index i = 0; i < steps.count; ++i)
            {
                if (auto moved = predict(policy, rng, inputs...); !moved)
                {
                    return moved;
                }
            }
            return std::expected<void, NumericError>{};
        }
        else
        {
            for (Eigen::Index i = 0; i < steps.count; ++i)
            {
                predict(policy, rng, inputs...);
            }
        }
    }

    using UpdateType = RbParticleFilterUpdate<Scalar, Allocator_>;

    template <internal::WeighsPoints M>
    [[nodiscard]] std::expected<UpdateType, NumericError>
    update(M const & measurement)
    {
        return update(execution::sequential, measurement);
    }

    template <ExecutionPolicy P, internal::WeighsPoints M>
    [[nodiscard]] std::expected<UpdateType, NumericError>
    update(P && executionPolicy, M const & measurement)
    {
        auto result = UpdateType{get_allocator()};
        auto const updated = update(std::forward<P>(executionPolicy), measurement, result);
        if (!updated)
        {
            return std::unexpected{updated.error()};
        }
        return result;
    }

    template <internal::WeighsPoints M, typename ResultAllocator>
    [[nodiscard]] std::expected<void, NumericError>
    update(M const & measurement, RbParticleFilterUpdate<Scalar, ResultAllocator> & result)
    {
        return update(execution::sequential, measurement, result);
    }

    template <ExecutionPolicy P, internal::WeighsPoints M, typename ResultAllocator>
    [[nodiscard]] std::expected<void, NumericError>
    update(P && executionPolicy, M const & measurement, RbParticleFilterUpdate<Scalar, ResultAllocator> & result)
    {
        if constexpr (!internal::hasOwnLikelihood<M> && !Shared)
        {
            return update(std::forward<P>(executionPolicy), measurement, reductions::collapse(), result);
        }
        else
        {
            if (auto const total = m_state.logTotalWeight(executionPolicy); !total)
            {
                return std::unexpected{total.error()};
            }
            auto & points = internal::PointLikelihoodsAccess::points(result);
            if constexpr (internal::hasOwnLikelihood<M>)
            {
                internal::updateGivenLikelihood(
                    executionPolicy,
                    m_state.points(),
                    m_state.m_conditionals,
                    measurement,
                    points
                );
                finish<false>(std::forward<P>(executionPolicy), result);
            }
            else
            {
                auto const updated = internal::updateGivenShared(
                    executionPolicy,
                    m_state.points(),
                    m_state.m_conditionals,
                    m_state.mostProbable(),
                    measurement,
                    points,
                    m_posterior
                );
                if (!updated)
                {
                    return std::unexpected{updated.error()};
                }
                finish(std::forward<P>(executionPolicy), result);
            }
            return {};
        }
    }

    template <internal::WeighsPoints M, typename Policy>
    [[nodiscard]] std::expected<UpdateType, NumericError>
    update(M const & measurement, Policy const & policy)
        requires(!Shared)
    {
        return update(execution::sequential, measurement, policy);
    }

    template <ExecutionPolicy P, internal::WeighsPoints M, typename Policy>
    [[nodiscard]] std::expected<UpdateType, NumericError>
    update(P && executionPolicy, M const & measurement, Policy const & policy)
        requires(!Shared)
    {
        auto result = UpdateType{get_allocator()};
        auto const updated = update(std::forward<P>(executionPolicy), measurement, policy, result);
        if (!updated)
        {
            return std::unexpected{updated.error()};
        }
        return result;
    }

    template <internal::WeighsPoints M, typename Policy, typename ResultAllocator>
    [[nodiscard]] std::expected<void, NumericError>
    update(M const & measurement, Policy const & policy, RbParticleFilterUpdate<Scalar, ResultAllocator> & result)
        requires(!Shared)
    {
        return update(execution::sequential, measurement, policy, result);
    }

    template <ExecutionPolicy P, internal::WeighsPoints M, typename Policy, typename ResultAllocator>
    [[nodiscard]] std::expected<void, NumericError>
    update(
        P && executionPolicy,
        M const & measurement,
        Policy const & policy,
        RbParticleFilterUpdate<Scalar, ResultAllocator> & result
    )
        requires(!Shared)
    {
        static_assert(
            !internal::hasOwnLikelihood<M>,
            "a measurement weighted by its own logLikelihood() has no branches to reduce, so it takes no policy"
        );
        // Weights the caller left undefined fail the update before any particle is conditioned.
        if (auto const total = m_state.logTotalWeight(executionPolicy); !total)
        {
            return std::unexpected{total.error()};
        }
        if constexpr (Switching && internal::isByMode<M>)
        {
            // Each mode's measurement holds its own noise component, so the recursion resolves it.
            return updateParticles(
                std::forward<P>(executionPolicy),
                m_steps.template emptyGivenUpdate<M, Space_, Path_>(get_allocator(), modeCount()),
                [&](auto & hybrid, auto const & pinned, auto & one) {
                    using Value = std::remove_cvref_t<decltype(pinned)>;
                    return m_steps.update(
                        hybrid,
                        measurement,
                        GivenBlock<Space_, Path_, Value>{m_state.space(), pinned},
                        policy,
                        one
                    );
                },
                result
            );
        }
        else if constexpr (Switching)
        {
            auto const f = internal::PinnedMeasurement<Space_, Path_, M>{m_state.space(), measurement};
            return updateParticles(
                std::forward<P>(executionPolicy),
                m_steps.template emptyGivenUpdate<M, Space_, Path_>(get_allocator(), modeCount()),
                [&](auto & hybrid, auto const & pinned, auto & one) {
                    return m_steps.updateGiven(hybrid, f, pinned, policy, one);
                },
                result
            );
        }
        else
        {
            auto const updated = internal::updateGivenEach(
                executionPolicy,
                m_state.points(),
                m_state.m_conditionals,
                measurement,
                policy,
                internal::PointLikelihoodsAccess::points(result),
                m_posterior
            );
            if (!updated)
            {
                return std::unexpected{updated.error()};
            }
            finish(std::forward<P>(executionPolicy), result);
            return {};
        }
    }
 
private:
 
    using ScalarAllocator = internal::ReBound<Allocator_, Scalar_>;  

    void
    assertModelPartitions(StateType const & state) const
    {
        internal::assertPartitions(
            m_steps.model(),
            state.space(),
            Switching ? "every mode's process model must write every coordinate of the state space once"
                      : "the process model must write every coordinate of the state space once"
        );
    }

    [[nodiscard]] Eigen::Index
    modeCount() const
        requires Switching
    {
        return m_steps.modeSpace().cardinality();
    }

    template <typename P, typename G, typename... Inputs>
    void
    predictOne(P && policy, G & rng, Scalar_ step, Inputs const &... inputs)
    {
        auto const call = internal::modelWith(std::cref(m_steps.model()), inputs...);
        auto const resolved = internal::resolveProcess(call, m_state.space());
        std::apply(
            [&](auto const &... uncertain) {
                moveParticles(
                    std::forward<P>(policy),
                    rng,
                    resolved.template noise<Scalar_>(step),
                    [&](auto const & x, auto const &... in) {
                        return internal::valueOf(resolved.propagate(x, step, in...));
                    },
                    internal::inputOf(uncertain)...
                );
            },
            resolved.inputs()
        );
    }

    template <typename P, typename G, typename... Inputs>
    void
    predictOneFixed(P && policy, G & rng, Inputs const &... inputs)
    {
        auto const & model = m_steps.model();
        auto const call = internal::modelWith(std::cref(model.model()), inputs...);
        auto const resolved = internal::resolveProcess(call, m_state.space());
        std::apply(
            [&](auto const &... uncertain) {
                moveParticles(
                    std::forward<P>(policy),
                    rng,
                    model.noise(),
                    [&](auto const & x, auto const &... in) {
                        return internal::valueOf(resolved.propagate(x, model.timeStep(), in...));
                    },
                    internal::inputOf(uncertain)...
                );
            },
            resolved.inputs()
        );
    }

    template <typename M, typename NoiseDerived, typename Propagate, typename PinnedVector, typename... Inputs>
    [[nodiscard]] GaussianParams<Space_, Scalar_>
    predictWhole(
        M const & method,
        Eigen::MatrixBase<NoiseDerived> const & noise,
        Propagate const & propagate,
        PinnedVector const & pinned,
        GaussianParams<RestSpace, Scalar_> const & conditional,
        Inputs const &... inputs
    ) const
    {
        auto const & split = m_state.m_split;
        auto const f = internal::PinnedStep<Propagate, std::remove_cvref_t<decltype(split)>, PinnedVector>{
            propagate,
            split,
            pinned
        };
        auto predicted = internal::transformBy(method, f, m_state.space(), conditional, inputs...);
        predicted.setCovariance(predicted.covarianceLower() + noise);
        return predicted;
    }

    template <typename G, typename StepOf>
    [[nodiscard]] std::expected<void, NumericError>
    moveHybridParticles(G & rng, StepOf const & stepOf)
    {
        using PinnedSpace = typename StateType::PinnedSpace;
        using PinnedSamples = SampleSet<PinnedSpace, Scalar_>;
        using PinnedNormals = SampleSet<DifferenceSpace<PinnedSpace>, Scalar_>;
        using PinnedVector = typename Moments<PinnedSpace, Scalar_>::VectorType;
        using RestVector = typename Moments<RestSpace, Scalar_>::VectorType;
        using ComponentType = typename ConditionalType::ComponentType;
        using ModesType = typename ConditionalType::ModesType;
        using Whole = GaussianParams<Space_, Scalar_>;
        constexpr int PinnedValues = valueDimensionAtCompileTime<PinnedSpace>;
        constexpr Scalar_ none = -Eigen::NumTraits<Scalar_>::infinity();
        auto const & split = m_state.m_split;
        BlockRows const rows = split.pinnedRows();
        Eigen::Index const count = m_state.size();
        Eigen::Index const normalSize = split.pinnedDifferenceRows().size;
        m_normals.resize(std::size_t(normalSize * count));
        auto z = Eigen::Map<PinnedNormals>{m_normals.data(), normalSize, count};
        internal::fillStandardNormal(z, rng);
        m_moved.resize(std::size_t(rows.size * count));
        auto moved = Eigen::Map<PinnedSamples>{m_moved.data(), rows.size, count};
        if (m_posterior.size() != count)
        {
            m_posterior = m_state.m_conditionals;
        }
        auto const & model = m_steps.model();
        auto const & transitions = model.chain().matrix();
        // Under the identity chain, each mode starts from its own component, as ImmRecursion's does.
        bool const mixes = !transitions.isIdentity(Scalar_(0));
        Eigen::Index const modes = modeCount();
        auto logMixing = typename ModesType::ArrayType{};
        logMixing.resize(modes);
        auto logLikelihoods = typename ModesType::ArrayType{};
        logLikelihoods.resize(modes);
        for (Eigen::Index i = 0; i < count; ++i)
        {
            auto const & prior = m_state.m_conditionals.conditional(i);
            auto & posterior = m_posterior.conditional(i);
            PinnedVector const pinned = m_state.particle(i);
            ModesType predictedModes = mixes ? Emrys::predict(prior.modes(), model.chain()) : prior.modes();
            auto const logTotal = predictedModes.logTotalWeight();
            if (!logTotal)
            {
                return std::unexpected{logTotal.error()};
            }
            Eigen::Index const drawn = internal::drawIndex<Scalar_>(
                modes,
                [&](Eigen::Index m) {
                    return Eigen::numext::exp(predictedModes.logWeight(m) - *logTotal);
                },
                rng
            );
            logLikelihoods.setConstant(none);
            for (Eigen::Index pass = 0; pass < modes; ++pass)
            {
                // The drawn mode first, since every mode is conditioned on its draw.
                Eigen::Index const j = pass == 0 ? drawn : (pass - 1 < drawn ? pass - 1 : pass);
                bool const reachable = predictedModes.logWeight(j) > none;
                auto const predictFrom = [&](ComponentType const & start) {
                    auto predicted = std::optional<Whole>{};
                    model.visit(j, m_steps.method(), [&](auto const & mode, auto const & modeMethod) {
                        predicted.emplace(stepOf(
                            mode,
                            modeMethod,
                            [&](auto const & method, auto const & noise, auto const & propagate, auto const &... in) {
                                return predictWhole(method, noise, propagate, pinned, start, in...);
                            }
                        ));
                    });
                    return std::move(*predicted);
                };
                Whole const predicted = [&] {
                    if (!mixes)
                    {
                        return predictFrom(prior.conditional(j));
                    }
                    // A mode the chain can't reach starts from its own component, which is never weighted.
                    for (Eigen::Index m = 0; m < modes; ++m)
                    {
                        logMixing(m) = reachable ? Eigen::numext::log(transitions(m, j)) + prior.modes().logWeight(m)
                                                 : (m == j ? Scalar_(0) : none);
                    }
                    ComponentType const mixed = ComponentTraits<ComponentType>::collapse(
                        modes,
                        [&](Eigen::Index m) -> ComponentType const & {
                            return prior.conditional(m);
                        },
                        [&](Eigen::Index m) {
                            return logMixing(m);
                        }
                    );
                    return predictFrom(mixed);
                }();
                auto conditioning = internal::pinnedConditioning(split, predicted.covarianceLower());
                auto const pinnedMean = internal::middleRowsOf<PinnedValues>(predicted.mean(), rows.start, rows.size);
                auto mean = RestVector{};
                if (pass == 0)
                {
                    internal::drawPinned(split, conditioning, predicted.mean(), z.col(i), moved.col(i), mean);
                }
                auto const offset = Emrys::minus(split.pinnedSpace(), moved.col(i), pinnedMean).eval();
                if (pass != 0)
                {
                    mean = Emrys::plus(
                        split.restSpace(),
                        predicted.mean()(split.restIndices()),
                        internal::multiply(conditioning.gain, offset)
                    );
                }
                if (reachable)
                {
                    auto const logDensity = internal::pinnedLogDensity(split, predicted.covarianceLower(), offset);
                    if (!logDensity)
                    {
                        return std::unexpected{logDensity.error()};
                    }
                    logLikelihoods(j) = *logDensity;
                }
                posterior.setConditional(
                    j,
                    ComponentType{split.restSpace(), std::move(mean), std::move(conditioning.cov)}
                );
            }
            // The drawn mode has a finite density, so the posterior modes define a distribution.
            [[maybe_unused]] auto const evidence = predictedModes.update(logLikelihoods);
            EMRYS_FILTER_ASSERT(evidence && "the draw's mode has a density");
            posterior.setModes(std::move(predictedModes));
        }
        // The particles and their hybrids change here, so a density of the state is stale.
        m_state.m_stamp.touchValues();
        m_state.m_conditionals.swap(m_posterior);
        m_state.m_particles.setStates(moved);
        return {};
    }

    template <typename P, typename OneResult, typename UpdateOne, typename ResultAllocator>
    [[nodiscard]] std::expected<void, NumericError>
    updateParticles(
        P && policy,
        OneResult oneResult,
        UpdateOne const & updateOne,
        RbParticleFilterUpdate<Scalar, ResultAllocator> & result
    )
    {
        assertParticlesRunUnder<P>();
        // Each particle's hybrid is updated in place below, so a density of the state is stale.
        m_state.m_stamp.touchValues();
        auto const updated = internal::updateHybridsGiven(
            m_state.points(),
            m_state.m_conditionals,
            internal::PointLikelihoodsAccess::points(result),
            oneResult,
            updateOne
        );
        if (!updated)
        {
            return updated;
        }
        finish<false>(std::forward<P>(policy), result);
        return {};
    }

    void
    reserveScratch()
    {
        Eigen::Index const count = m_state.particles().capacity().value_or(m_state.size());
        m_posterior.reserve(count);
        m_normals.reserve(std::size_t(m_state.pinnedDifferenceRows().size * count));
        m_moved.reserve(std::size_t(m_state.pinnedRows().size * count));
    }

    template <typename P, typename G, typename Derived, typename Propagate, typename... Inputs>
    void
    moveParticles(
        P && policy,
        G & rng,
        Eigen::MatrixBase<Derived> const & noise,
        Propagate const & propagate,
        Inputs const &... inputs
    )
    {
        using PinnedSamples = SampleSet<typename StateType::PinnedSpace, Scalar_>;
        using PinnedNormals = SampleSet<DifferenceSpace<typename StateType::PinnedSpace>, Scalar_>;
        using PinnedVector = typename Moments<typename StateType::PinnedSpace, Scalar_>::VectorType;
        using WholeMoments = Moments<Space_, Scalar_>;
        using RestMoments = Moments<typename StateType::RestSpace, Scalar_>;
        // The transition's columns for the rest, and their product with a rest covariance.
        using RestColumns = internal::MatrixOf<
            Scalar_,
            WholeMoments::DifferenceDimensionAtCompileTime,
            RestMoments::DifferenceDimensionAtCompileTime,
            WholeMoments::MaxDifferenceDimensionAtCompileTime,
            RestMoments::MaxDifferenceDimensionAtCompileTime>;
        using RestCross = internal::MatrixOf<
            Scalar_,
            RestMoments::DifferenceDimensionAtCompileTime,
            WholeMoments::DifferenceDimensionAtCompileTime,
            RestMoments::MaxDifferenceDimensionAtCompileTime,
            WholeMoments::MaxDifferenceDimensionAtCompileTime>;
        constexpr bool cached = internal::cachesTransition<Model_> && sizeof...(Inputs) == 0;
        auto const & space = m_state.space();
        auto const & split = m_state.m_split;
        Eigen::Index const count = m_state.size();
        Eigen::Index const pinnedSize = split.pinnedRows().size;
        Eigen::Index const normalSize = split.pinnedDifferenceRows().size;
        m_normals.resize(std::size_t(normalSize * count));
        auto z = Eigen::Map<PinnedNormals>{m_normals.data(), normalSize, count};
        internal::fillStandardNormal(z, rng);
        // The particles and their Gaussians change below, so a density of the state is stale.
        m_state.m_stamp.touchValues();
        auto & conditionals = m_state.m_conditionals;
        auto const restColumns = [&] {
            if constexpr (cached)
            {
                return RestColumns{
                    m_steps.model().transition()(Eigen::placeholders::all, split.restDifferenceIndices())
                };
            }
            else
            {
                return internal::NoTransition{};
            }
        }();
        auto const predictGiven = [&](Eigen::Index i) -> GaussianParams<Space_, Scalar_> {
            PinnedVector const pinned = m_state.particle(i);
            auto const & conditional = conditionals.conditional(i);
            if constexpr (cached)
            {
                typename WholeMoments::VectorType mean = propagate(split.assemble(conditional.mean(), pinned));
                normalizeValues(space, mean);
                RestCross const cross = internal::multiply(conditional.covarianceLower(), restColumns.transpose());
                typename WholeMoments::MatrixType cov = internal::multiply(restColumns, cross);
                cov = (Scalar_(0.5) * (cov + cov.transpose()) + noise).eval();
                return {space, std::move(mean), std::move(cov)};
            }
            else
            {
                return predictWhole(m_steps.method(), noise, propagate, pinned, conditional, inputs...);
            }
        };
 
        m_moved.resize(std::size_t(pinnedSize * count));
        auto moved = Eigen::Map<PinnedSamples>{m_moved.data(), pinnedSize, count};
        if constexpr (Shared)
        {
            auto const conditioning =
                internal::pinnedConditioning(split, predictGiven(m_state.mostProbable()).covarianceLower());
            auto means = conditionals.means();
            internal::forEachIndex(std::forward<P>(policy), count, [&](Eigen::Index i) {
                auto mean = propagate(split.assemble(means.col(i), m_state.particle(i)), internal::meanOf(inputs)...);
                normalizeValues(space, mean);
                internal::drawPinned(split, conditioning, mean, z.col(i), moved.col(i), means.col(i));
            });
            conditionals.sharedCovariance() = conditioning.cov;
        }
        else
        {
            internal::forEachIndex(std::forward<P>(policy), count, [&](Eigen::Index i) {
                auto mean = typename Moments<typename StateType::RestSpace, Scalar_>::VectorType{};
                auto const predicted = predictGiven(i);
                auto conditioning = internal::pinnedConditioning(split, predicted.covarianceLower());
                internal::drawPinned(split, conditioning, predicted.mean(), z.col(i), moved.col(i), mean);
                conditionals.conditional(i) =
                    ConditionalType{split.restSpace(), std::move(mean), std::move(conditioning.cov)};
            });
        }
        m_state.m_particles.setStates(moved);
    }

    template <bool Posterior = true, typename P, typename ResultAllocator>
    void
    finish(P && policy, RbParticleFilterUpdate<Scalar, ResultAllocator> & result)
    {
        auto & points = internal::PointLikelihoodsAccess::points(result);
        auto const [logLikelihood, accepted] = [&] {
            if constexpr (Posterior)
            {
                return m_state.reweight(std::forward<P>(policy), points.logLikelihoods(), m_posterior);
            }
            else
            {
                return m_state.reweight(std::forward<P>(policy), points.logLikelihoods());
            }
        }();
        internal::UpdateAccess::logLikelihood(result) = logLikelihood;
        result.accepted = accepted;
    }

    using StepsType = std::conditional_t<
        Switching,
        ImmRecursion<Gaussian<RestSpace, Scalar_>, Model_, Method_, Allocator_>,
        internal::PointSteps<Model_, Method_>>;
 
    StepsType m_steps;  
    StateType m_state;  
 
    // Scratch, reused across steps.
 
    typename StateType::ConditionalsType m_posterior;  
    internal::Buffer<Scalar_, Allocator_> m_normals;   
    internal::Buffer<Scalar_, Allocator_> m_moved;     
};

// One guide with a defaulted method rather than one per arity, its parameters in the class's
// order: otherwise MSVC deduces the scalar as the method. The state's component follows from the
// model, so it is not carried over.
template <
    typename Space,
    typename Path,
    typename Model,
    typename Covariance,
    typename Method = DefaultMethodFor<Model>,
    typename Scalar,
    typename Allocator,
    typename Component>
    requires(!internal::UpkeepArgument<Method>)
RbParticleFilter(Model, ParticleGaussianParams<Space, Path, Covariance, Scalar, Allocator, Component>, Method = {})
    -> RbParticleFilter<Space, Path, internal::BoundStep<Model, Space, Scalar>, Covariance, Method, Scalar, Allocator>;
 
template <
    typename Space,
    typename Path,
    typename Model,
    typename Covariance,
    typename Scalar,
    typename Allocator,
    typename Component,
    typename U>
    requires internal::UpkeepArgument<U>
RbParticleFilter(Model, ParticleGaussianParams<Space, Path, Covariance, Scalar, Allocator, Component>, U)
    -> RbParticleFilter<
        Space,
        Path,
        internal::BoundStep<Model, Space, Scalar>,
        Covariance,
        DefaultMethodFor<Model>,
        Scalar,
        Allocator,
        U>;
 
template <
    typename Space,
    typename Path,
    typename Model,
    typename Covariance,
    typename Method,
    typename Scalar,
    typename Allocator,
    typename Component,
    typename U>
RbParticleFilter(
    Model,
    ParticleGaussianParams<Space, Path, Covariance, Scalar, Allocator, Component>,
    Method,
    U
) -> RbParticleFilter<Space, Path, internal::BoundStep<Model, Space, Scalar>, Covariance, Method, Scalar, Allocator, U>;
 
}  // namespace Emrys
 
#ifndef EMRYS_FILTER_NO_IO
 
namespace Emrys
{
 
namespace internal
{

template <
    typename Space_,
    typename Path_,
    typename Model_,
    typename Covariance_,
    typename Method_,
    typename Scalar_,
    typename Allocator_,
    typename Upkeep_>
void
describe(
    std::ostream & stream,
    RbParticleFilter<Space_, Path_, Model_, Covariance_, Method_, Scalar_, Allocator_, Upkeep_> const & filter,
    Indent indent
)
{
    describeFilter(stream, "Rao-Blackwellized particle filter", filter.state(), indent);
}
 
}  // namespace internal
 
}  // namespace Emrys
 
#endif  // EMRYS_FILTER_NO_IO