ImmFilter.h

Go to the documentation of this file.

#pragma once

 
#include <cstddef>
#include <expected>
#include <optional>
#include <span>
#include <type_traits>
#include <utility>
 
#include <EmrysFilter/Core/Allocation.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
#include <EmrysFilter/Core/Print.h>
#include <EmrysFilter/Core/Weights.h>
#include <EmrysFilter/Distributions/Categorical.h>
#include <EmrysFilter/Distributions/Component.h>
#include <EmrysFilter/Distributions/Gaussian.h>
#include <EmrysFilter/Distributions/Hybrid.h>
#include <EmrysFilter/Filters/FilterUpkeep.h>
#include <EmrysFilter/Filters/KalmanFilter.h>
#include <EmrysFilter/Models/Measurement.h>
#include <EmrysFilter/Models/ProcessModel.h>
#include <EmrysFilter/Models/Reduction.h>
#include <EmrysFilter/Models/SwitchingModel.h>
#include <EmrysFilter/Models/Transform.h>
#include <EmrysFilter/Spaces/Discrete.h>
#include <EmrysFilter/Spaces/Space.h>
 
namespace Emrys
{

template <typename ModeUpdate, typename Scalar_ = double, typename Allocator = DefaultAllocator>
struct ImmFilterUpdate
{
    using Scalar = Scalar_;                                           
    using ModeUpdateType = ModeUpdate;                                
    using ModeUpdatesType = internal::Buffer<ModeUpdate, Allocator>;  
 
    ImmFilterUpdate() = default;  

    explicit ImmFilterUpdate(Allocator const & allocator)
        : modes(internal::ReBound<Allocator, ModeUpdate>(allocator))
    {}

    [[nodiscard]] Allocator
    get_allocator() const
    {
        return Allocator(modes.get_allocator());
    }

    [[nodiscard]] Scalar
    logLikelihood() const
    {
        return m_logLikelihood;
    }
 
    ModeUpdatesType modes;  
    bool accepted = false;  
 
private:
 
    friend struct internal::UpdateAccess;
 
    Scalar m_logLikelihood = 0;  
};
 
namespace internal
{

template <typename ModeUpdate, typename Scalar, typename Old, typename Allocator>
struct RebindUpdateAllocatorOf<ImmFilterUpdate<ModeUpdate, Scalar, Old>, Allocator>
{
    using type = ImmFilterUpdate<RebindUpdateAllocator<ModeUpdate, Allocator>, Scalar, Allocator>;
};

 
}  // namespace internal

template <Space Space_, DiscreteSpace ModeSpace_, RealScalar Scalar_ = double, typename Allocator_ = DefaultAllocator>
struct HybridPredictedJoint
{
    using Scalar = Scalar_;                                                         
    using SpaceType = Space_;                                                       
    using ModeSpaceType = ModeSpace_;                                               
    using ModesType = CategoricalParams<ModeSpace_, Scalar_>;                       
    using JointType = PredictedJoint<Space_, Scalar_>;                              
    using JointsType = internal::Buffer<JointType, Allocator_>;                     
    using TransitionsType = typename MarkovChain<ModeSpace_, Scalar_>::MatrixType;  
    using PredictionsType = internal::Buffer<GaussianParams<Space_, Scalar_>, Allocator_>;  
 
    ModesType modes;              
    JointsType joints;            
    TransitionsType transitions;  

    PredictionsType throughOtherModels;

    [[nodiscard]] GaussianParams<Space_, Scalar_> const &
    predictedThrough(Eigen::Index from, Eigen::Index model) const
    {
        if (from == model)
        {
            return joints[std::size_t(from)].predicted;
        }
        Eigen::Index const count = Eigen::Index(joints.size());
        return throughOtherModels[std::size_t(from * (count - 1) + (model < from ? model : model - 1))];
    }
};
 
namespace internal
{

template <typename C, typename M, bool = isOtherGaussianForm<C>>
struct ImmMeasured
{
    using type = typename decltype(Emrys::update(std::declval<C const &>(), std::declval<M const &>()))::value_type;
};

template <typename C, typename M>
struct ImmMeasured<C, M, true>
{
    using type = typename decltype(updateInPlace<true>(std::declval<C &>(), std::declval<M const &>()))::value_type;
};

template <typename Lazy, typename Policy>
struct UnlessGivenBlock
{
    using type = typename Lazy::type;  
};

template <typename Lazy, typename Space, typename P, typename Value>
struct UnlessGivenBlock<Lazy, GivenBlock<Space, P, Value>>
{};


template <typename M>
struct MeasurementModeAxisOf
{
    using type = void;  
};

template <typename M>
    requires(M::ModelType::template has<PolicyKind::additiveNoise>)
struct MeasurementModeAxisOf<M>
{
    using type = NoiseModeAxis<typename M::ModelType::template Policy<PolicyKind::additiveNoise>>;
};


template <typename M>
inline constexpr bool isByMode = !std::is_void_v<typename MeasurementModeAxisOf<M>::type>;

template <typename Space, typename P, typename One, typename Value>
struct GivenComponent
{
    GivenComponent(One one, Space whole, Value const & given)
        : measurement{std::move(one)}
        , space{std::move(whole)}
        , pinned{space, measurement}
        , value{given}
    {}

    GivenComponent(GivenComponent const & other)
        : GivenComponent{other.measurement, other.space, other.value}
    {}
 
    One measurement;                          
    Space space;                              
    PinnedMeasurement<Space, P, One> pinned;  
    Value const & value;                      
};
 
}  // namespace internal

template <
    Component Component_,
    typename Model_,
    typename Method_ = DefaultMethodFor<Model_>,
    typename Allocator_ = DefaultAllocator>
class ImmRecursion
{
    static_assert(
        internal::predictsBy<Model_, Method_>,
        "a filter with fixed-step models predicts each mode by its model's own method, so it takes "
        "ModelsMethod, the default, as its method"
    );
    static_assert(internal::checkPredictionMethod<Method_>());

    static constexpr bool IsForm = internal::isOtherGaussianForm<Params<Component_>>;
 
public:
 
    using Scalar = typename Params<Component_>::Scalar;                     
    using SpaceType = typename Params<Component_>::SpaceType;               
    using ModelType = Model_;                                               
    using MethodType = Method_;                                             
    using ModeSpaceType = typename Model_::ModeSpaceType;                   
    using ComponentType = Params<Component_>;                               
    using StateType = HybridParams<Component_, ModeSpaceType, Allocator_>;  
    using allocator_type = Allocator_;                                      

    using JointType = HybridPredictedJoint<SpaceType, ModeSpaceType, Scalar, Allocator_>;
 
    static constexpr bool Reentrant = false;  

    template <Measurement M>
    using MeasuredType = typename internal::ImmMeasured<ComponentType, M>::type;

    template <Measurement M>
    using ResultType = ImmFilterUpdate<
        std::conditional_t<
            internal::isByMode<M>,
            FilterUpdate<typename MeasuredType<M>::MeasurementSpaceType, 1, Scalar>,
            std::conditional_t<
                IsForm,
                MeasuredType<M>,
                internal::FilterUpdateWithScratch<
                    SpaceType,
                    typename MeasuredType<M>::MeasurementSpaceType,
                    MeasuredType<M>::BranchCount,
                    Scalar,
                    Allocator_>>>,
        Scalar,
        Allocator_>;

    template <Measurement M>
    using UpdateType = std::expected<ResultType<M>, NumericError>;

    template <Measurement M>
    struct UpdateTypeOf
    {
        using type = UpdateType<M>;  
    };

    template <typename S>
    using SwitchingResultType = ResultType<typename S::template MeasurementType<0>>;

    template <typename S>
    using SwitchingUpdateType = std::expected<SwitchingResultType<S>, NumericError>;

    template <Measurement M, typename Space, typename P>
    using GivenMeasuredType = typename decltype(internal::updateBranchesWith<true>(
        std::declval<ComponentType const &>(),
        std::declval<internal::PinnedMeasurement<Space, P, M> const &>(),
        std::declval<typename Moments<internal::PathTag<P>, Scalar>::VectorType const &>()
    ))::value_type;

    template <Measurement M, typename Space, typename P>
    using GivenResultType = ImmFilterUpdate<
        std::conditional_t<
            internal::isByMode<M>,
            FilterUpdate<typename GivenMeasuredType<M, Space, P>::MeasurementSpaceType, 1, Scalar>,
            internal::FilterUpdateWithScratch<
                SpaceType,
                typename GivenMeasuredType<M, Space, P>::MeasurementSpaceType,
                GivenMeasuredType<M, Space, P>::BranchCount,
                Scalar,
                Allocator_>>,
        Scalar,
        Allocator_>;

    template <Measurement M, typename Space, typename P>
    using GivenUpdateType = std::expected<GivenResultType<M, Space, P>, NumericError>;

    explicit ImmRecursion(Model_ model, MethodFor<Model_, Method_> method = {}, Allocator_ const & allocator = {})
        : m_model{std::move(model)}
        , m_method{internal::predictionMethod(m_model, std::move(method))}
        , m_predicted(ComponentAllocator(allocator))
        , m_posterior(ComponentAllocator(allocator))
    {
        reserveScratch();
    }

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

    void
    resetScratch(Allocator_ const & allocator)
    {
        m_predicted = ConditionalsType(ComponentAllocator(allocator));
        m_posterior = ConditionalsType(ComponentAllocator(allocator));
        reserveScratch();
    }

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

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

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

    [[nodiscard]] ModeSpaceType const &
    modeSpace() const
    {
        return m_model.modeSpace();
    }

    [[nodiscard]] bool
    partitions(SpaceType const & space) const
    {
        return m_model.partitions(space);
    }

    template <typename... Inputs>
        requires(!isFixedStep<Model_>)
    [[nodiscard]] auto
    predict(StateType & state, TimeStep<Scalar> auto const & dt, Inputs const &... inputs)
    {
        Scalar const step = internal::toScalarTime<Scalar>(dt);
        if constexpr (IsForm)
        {
            return moveModes(state, [&](ComponentType & moved, auto const & model, auto const & method) {
                return internal::predictInPlace(method, moved, model, step, inputs...);
            });
        }
        else
        {
            moveModes(state, [&](ComponentType const & mixed, auto const & model, auto const & method) {
                return Emrys::predict(method, mixed, model, step, inputs...);
            });
        }
    }

    template <typename... Inputs>
        requires isFixedStep<Model_>
    [[nodiscard]] auto
    predict(StateType & state, Inputs const &... inputs)
    {
        if constexpr (IsForm)
        {
            return moveModes(state, [&](ComponentType & moved, auto const & model, auto const &) {
                return internal::predictInPlace(moved, model, inputs...);
            });
        }
        else
        {
            moveModes(state, [&](ComponentType const & mixed, auto const & model, auto const &) {
                return Emrys::predict(mixed, model, inputs...);
            });
        }
    }

    template <typename... Inputs>
        requires(!isFixedStep<Model_> && !IsForm)
    [[nodiscard]] JointType
    predictJoint(StateType & state, TimeStep<Scalar> auto const & dt, Inputs const &... inputs)
    {
        auto joint = emptyJoint(state);
        predictJoint(state, dt, joint, inputs...);
        return joint;
    }

    template <typename JointAllocator, typename... Inputs>
        requires(!isFixedStep<Model_> && !IsForm)
    void
    predictJoint(
        StateType & state,
        TimeStep<Scalar> auto const & dt,
        HybridPredictedJoint<SpaceType, ModeSpaceType, Scalar, JointAllocator> & joint,
        Inputs const &... inputs
    )
    {
        Scalar const step = internal::toScalarTime<Scalar>(dt);
        recordJoint(
            state,
            joint,
            [&](ComponentType const & filtered, auto const & model, auto const & method) {
                return Emrys::predictJoint(method, filtered, model, step, inputs...);
            },
            [&](ComponentType const & filtered, auto const & model, auto const & method) {
                return Emrys::predict(method, filtered, model, step, inputs...);
            }
        );
        predict(state, step, inputs...);
    }

    template <typename... Inputs>
        requires(isFixedStep<Model_> && !IsForm)
    [[nodiscard]] JointType
    predictJoint(StateType & state, Inputs const &... inputs)
    {
        auto joint = emptyJoint(state);
        predictJoint(state, joint, inputs...);
        return joint;
    }

    template <typename JointAllocator, typename... Inputs>
        requires(isFixedStep<Model_> && !IsForm)
    void
    predictJoint(
        StateType & state,
        HybridPredictedJoint<SpaceType, ModeSpaceType, Scalar, JointAllocator> & joint,
        Inputs const &... inputs
    )
    {
        recordJoint(
            state,
            joint,
            [&](ComponentType const & filtered, auto const & model, auto const &) {
                return Emrys::predictJoint(filtered, model, inputs...);
            },
            [&](ComponentType const & filtered, auto const & model, auto const &) {
                return Emrys::predict(filtered, model, inputs...);
            }
        );
        predict(state, inputs...);
    }

    template <Measurement M, typename Policy = reductions::Collapse>
        requires(!internal::isGivenBlock<Policy>)
    [[nodiscard]] typename internal::UnlessGivenBlock<UpdateTypeOf<M>, Policy>::type
    update(StateType & state, M const & measurement, Policy const & policy = {})
    {
        auto result = emptyUpdate<M>(get_allocator(), state.size());
        auto const updated = update(state, measurement, policy, result);
        if (!updated)
        {
            return std::unexpected{updated.error()};
        }
        return result;
    }

    template <Measurement M, typename Policy, typename ModeUpdate, typename ResultAllocator>
    [[nodiscard]] std::expected<void, NumericError>
    update(
        StateType & state,
        M const & measurement,
        Policy const & policy,
        ImmFilterUpdate<ModeUpdate, Scalar, ResultAllocator> & result
    )
    {
        if constexpr (internal::isByMode<M>)
        {
            return updateByAxis(state, measurement, policy, result, [](auto one) {
                return one;
            });
        }
        else
        {
            return updateModes(
                state,
                policy,
                result,
                [&](Eigen::Index, auto const & updateMode) {
                    return updateMode(measurement);
                },
                [](auto &, Eigen::Index) {}
            );
        }
    }

    template <typename P, Measurement... Ms, typename Policy = reductions::Collapse>
    [[nodiscard]] SwitchingUpdateType<SwitchingMeasurement<P, Ms...>>
    update(StateType & state, SwitchingMeasurement<P, Ms...> const & measurement, Policy const & policy = {})
    {
        auto result = SwitchingResultType<SwitchingMeasurement<P, Ms...>>{get_allocator()};
        result.modes.reserve(std::size_t(state.size()));
        auto const updated = update(state, measurement, policy, result);
        if (!updated)
        {
            return std::unexpected{updated.error()};
        }
        return result;
    }

    template <typename P, Measurement... Ms, typename Policy, typename ModeUpdate, typename ResultAllocator>
    [[nodiscard]] std::expected<void, NumericError>
    update(
        StateType & state,
        SwitchingMeasurement<P, Ms...> const & measurement,
        Policy const & policy,
        ImmFilterUpdate<ModeUpdate, Scalar, ResultAllocator> & result
    )
    {
        static_assert(
            internal::pathResolves<P, ModeSpaceType>(),
            "a switching measurement is selected by a mode axis that the filter's mode space does not have"
        );
        static_assert(
            !(internal::isByMode<Ms> || ...),
            "a measurement of a switching measurement has noise by mode axis, byMode<Axis>(): give each value's "
            "measurement the noise it has in the modes with that value"
        );
        static_assert(
            (std::is_same_v<ResultType<Ms>, SwitchingResultType<SwitchingMeasurement<P, Ms...>>> && ...),
            "the measurements of a switching measurement update a mode into one type: give them one measurement "
            "space, and noise that is Gaussian in all of them or a mixture in all of them"
        );
        if constexpr (internal::pathResolves<P, ModeSpaceType>())
        {
            auto const [stride, axis] = internal::locateAxisByPath<P>(state.modeSpace());
            Eigen::Index const values = axis->cardinality();
            EMRYS_FILTER_ASSERT(
                values == Eigen::Index(sizeof...(Ms)) && "give one measurement per value of the axis that selects it"
            );
            return updateModes(
                state,
                policy,
                result,
                [&, stride](Eigen::Index m, auto const & updateMode) {
                    return measurement.visit(std::size_t((m / stride) % values), updateMode);
                },
                [](auto &, Eigen::Index) {}
            );
        }
        else
        {
            return std::expected<void, NumericError>{};
        }
    }
 
private:

    template <Measurement M, typename Policy, typename ModeUpdate, typename ResultAllocator, typename Wrap>
    [[nodiscard]] std::expected<void, NumericError>
    updateByAxis(
        StateType & state,
        M const & measurement,
        Policy const & policy,
        ImmFilterUpdate<ModeUpdate, Scalar, ResultAllocator> & result,
        Wrap const & wrap
    )
    {
        using Axis = typename internal::MeasurementModeAxisOf<M>::type;
        static_assert(
            internal::pathResolves<Axis, ModeSpaceType>(),
            "the measurement's noise is by a mode axis, byMode<Axis>(), that the filter's mode space does not have"
        );
        if constexpr (internal::pathResolves<Axis, ModeSpaceType>())
        {
            return updateByAxisIn<Axis>(state, measurement, policy, result, wrap);
        }
        else
        {
            return std::expected<void, NumericError>{};
        }
    }

    template <
        typename Axis,
        Measurement M,
        typename Policy,
        typename ModeUpdate,
        typename ResultAllocator,
        typename Wrap>
    [[nodiscard]] std::expected<void, NumericError>
    updateByAxisIn(
        StateType & state,
        M const & measurement,
        Policy const & policy,
        ImmFilterUpdate<ModeUpdate, Scalar, ResultAllocator> & result,
        Wrap const & wrap
    )
    {
        auto const [stride, axis] = internal::locateAxisByPath<Axis>(state.modeSpace());
        Eigen::Index const values = axis->cardinality();
        auto const & noise = measurement.model().template policy<internal::PolicyKind::additiveNoise>().distribution();
        EMRYS_FILTER_ASSERT(noise.size() == values && "the noise needs one component per value of the mode axis");
        using Component = std::remove_cvref_t<decltype(noise.begin()->component)>;
        using One = internal::WithNoiseComponent<M, Component>;
        // Each mode is updated with the one component its value names, not with every component.
        auto const componentOf = [=](Eigen::Index m) {
            return (m / stride) % values;
        };
        return updateModes(
            state,
            policy,
            result,
            [&](Eigen::Index m, auto const & updateMode) {
                return updateMode(wrap(One{measurement, noise[componentOf(m)].component}));
            },
            [&](auto & measured, Eigen::Index m) {
                auto & branch = measured.branches.front();
                branch.component = componentOf(m);
                branch.logWeight = Scalar(0);
            }
        );
    }
 
public:

    template <Measurement M, typename Space, typename P, typename Value, typename Policy = reductions::Collapse>
        requires(!IsForm)
    [[nodiscard]] GivenUpdateType<M, Space, P>
    update(
        StateType & state,
        M const & measurement,
        GivenBlock<Space, P, Value> const & pinned,
        Policy const & policy = {}
    )
    {
        auto result = emptyGivenUpdate<M, Space, P>(get_allocator(), state.size());
        auto const updated = update(state, measurement, pinned, policy, result);
        if (!updated)
        {
            return std::unexpected{updated.error()};
        }
        return result;
    }

    template <
        Measurement M,
        typename Space,
        typename P,
        typename Value,
        typename Policy,
        typename ModeUpdate,
        typename ResultAllocator>
        requires(!IsForm)
    [[nodiscard]] std::expected<void, NumericError>
    update(
        StateType & state,
        M const & measurement,
        GivenBlock<Space, P, Value> const & pinned,
        Policy const & policy,
        ImmFilterUpdate<ModeUpdate, Scalar, ResultAllocator> & result
    )
    {
        if constexpr (internal::isByMode<M>)
        {
            // Each mode's measurement holds its own noise component, so each resolves its own.
            return updateByAxis(state, measurement, policy, result, [&]<typename One>(One one) {
                return internal::GivenComponent<Space, P, One, Value>{std::move(one), pinned.space, pinned.value};
            });
        }
        else
        {
            auto const f = internal::PinnedMeasurement<Space, P, M>{pinned.space, measurement};
            return updateGiven(state, f, pinned.value, policy, result);
        }
    }

    template <
        typename Space,
        typename P,
        typename M,
        typename Value,
        typename Policy,
        typename ModeUpdate,
        typename ResultAllocator>
        requires(!IsForm && !internal::isByMode<M>)
    [[nodiscard]] std::expected<void, NumericError>
    updateGiven(
        StateType & state,
        internal::PinnedMeasurement<Space, P, M> const & f,
        Value const & value,
        Policy const & policy,
        ImmFilterUpdate<ModeUpdate, Scalar, ResultAllocator> & result
    )
    {
        using Given = internal::GivenMeasurement<internal::PinnedMeasurement<Space, P, M> const &, Value>;
        return updateModes(
            state,
            policy,
            result,
            [&](Eigen::Index, auto const & updateMode) {
                return updateMode(Given{f, value});
            },
            [](auto &, Eigen::Index) {}
        );
    }

    template <Measurement M>
    [[nodiscard]] ResultType<M>
    emptyUpdate(Allocator_ const & allocator, Eigen::Index modes) const
    {
        using Result = ResultType<M>;
        auto result = Result{allocator};
        result.modes.reserve(std::size_t(modes));
        return result;
    }

    template <Measurement M, typename Space, typename P>
    [[nodiscard]] GivenResultType<M, Space, P>
    emptyGivenUpdate(Allocator_ const & allocator, Eigen::Index modes) const
    {
        auto result = GivenResultType<M, Space, P>{allocator};
        result.modes.reserve(std::size_t(modes));
        return result;
    }

    [[nodiscard]] JointType
    emptyJoint(StateType const & state) const
    {
        auto joint = JointType{
            state.modes(),
            typename JointType::JointsType(state.get_allocator()),
            m_model.chain().matrix(),
            typename JointType::PredictionsType(state.get_allocator())
        };
        joint.joints.reserve(std::size_t(state.size()));
        joint.throughOtherModels.reserve(std::size_t(state.size() * (state.size() - 1)));
        return joint;
    }
 
private:
 
    using ConditionalsType = typename StateType::ConditionalsType;         
    using ComponentAllocator = typename ConditionalsType::allocator_type;  

    void
    reserveScratch()
    {
        auto const modes = std::size_t(m_model.modeSpace().cardinality());
        m_predicted.reserve(modes);
        if constexpr (!IsForm)
        {
            m_posterior.reserve(modes);
        }
    }

    template <typename PredictMode>
    [[nodiscard]] auto
    moveModes(StateType & state, PredictMode const & predictMode)
    {
        if constexpr (IsForm)
        {
            auto failure = std::expected<void, NumericError>{};
            auto const keepFirst = [&](std::expected<void, NumericError> const & moved) {
                if (!moved && failure)
                {
                    failure = moved;
                }
            };
            moveModesBy(
                state,
                [&](Eigen::Index j, auto const & model, auto const & method) {
                    keepFirst(state.updateConditional(j, [&](ComponentType & own) {
                        return predictMode(own, model, method);
                    }));
                },
                [&](ComponentType && mixed, auto const & model, auto const & method) {
                    m_predicted.push_back(std::move(mixed));
                    keepFirst(predictMode(m_predicted.back(), model, method));
                }
            );
            return failure;
        }
        else
        {
            moveModesBy(
                state,
                [&](Eigen::Index j, auto const & model, auto const & method) {
                    m_predicted.push_back(predictMode(state.conditional(j), model, method));
                },
                [&](ComponentType && mixed, auto const & model, auto const & method) {
                    m_predicted.push_back(predictMode(mixed, model, method));
                }
            );
        }
    }

    template <typename MoveOwn, typename MoveMixed>
    void
    moveModesBy(StateType & state, MoveOwn const & moveOwn, MoveMixed const & moveMixed)
    {
        Eigen::Index const count = state.size();
        auto const & logPrior = state.modes().logWeights();
        auto const & transitions = m_model.chain().matrix();
        m_predicted.clear();
        if (transitions.isIdentity(Scalar(0)))
        {
            // The autonomous multiple model estimator: each mode's mixing puts all its weight on
            // its own component, so each is predicted as it is, and the probabilities carry over.
            for (Eigen::Index j = 0; j < count; ++j)
            {
                m_model.visit(j, m_method, [&](auto const & model, auto const & method) {
                    moveOwn(j, model, method);
                });
            }
            if constexpr (!IsForm)
            {
                state.swapConditionals(m_predicted);
            }
            return;
        }
        CategoricalParams<ModeSpaceType, Scalar> const predictedModes = Emrys::predict(state.modes(), m_model.chain());
 
        auto const conditional = [&](Eigen::Index i) -> ComponentType const & {
            return state.conditional(i);
        };
        auto logMixing = typename StateType::ModesType::ArrayType{};
        logMixing.resize(count);
        for (Eigen::Index j = 0; j < count; ++j)
        {
            // A mode the chain can't reach starts from its own component, which is never weighted.
            bool const reachable = predictedModes.logWeight(j) > -Eigen::NumTraits<Scalar>::infinity();
            for (Eigen::Index i = 0; i < count; ++i)
            {
                logMixing(i) = reachable ? Eigen::numext::log(transitions(i, j)) + logPrior(i)
                                         : (i == j ? Scalar(0) : -Eigen::NumTraits<Scalar>::infinity());
            }
            ComponentType mixed = ComponentTraits<ComponentType>::collapse(count, conditional, [&](Eigen::Index i) {
                return logMixing(i);
            });
            m_model.visit(j, m_method, [&](auto const & model, auto const & method) {
                moveMixed(std::move(mixed), model, method);
            });
        }
        state.swapConditionals(m_predicted);
        state.setModes(predictedModes);
    }

    template <typename JointAllocator, typename PredictMode, typename PredictThrough>
    void
    recordJoint(
        StateType const & state,
        HybridPredictedJoint<SpaceType, ModeSpaceType, Scalar, JointAllocator> & joint,
        PredictMode const & predictMode,
        PredictThrough const & predictThrough
    ) const
    {
        Eigen::Index const count = state.size();
        joint.modes = state.modes();
        joint.transitions = m_model.chain().matrix();
        joint.joints.clear();
        joint.joints.reserve(std::size_t(count));
        joint.throughOtherModels.clear();
        joint.throughOtherModels.reserve(std::size_t(count * (count - 1)));
        for (Eigen::Index j = 0; j < count; ++j)
        {
            m_model.visit(j, m_method, [&](auto const & model, auto const & method) {
                joint.joints.push_back(predictMode(state.conditional(j), model, method));
            });
            for (Eigen::Index i = 0; i < count; ++i)
            {
                if (i != j)
                {
                    m_model.visit(i, m_method, [&](auto const & model, auto const & method) {
                        joint.throughOtherModels.push_back(predictThrough(state.conditional(j), model, method));
                    });
                }
            }
        }
    }

    template <
        typename Policy,
        typename ModeUpdate,
        typename ResultAllocator,
        typename WithMeasurement,
        typename Relabel>
    [[nodiscard]] std::expected<void, NumericError>
    updateModes(
        StateType & state,
        Policy const & policy,
        ImmFilterUpdate<ModeUpdate, Scalar, ResultAllocator> & result,
        WithMeasurement const & withMeasurement,
        Relabel const & relabel
    )
    {
        // Mode probabilities the caller left undefined fail the update before any mode is written.
        auto const logTotal = state.modes().logTotalWeight();
        if (!logTotal)
        {
            return std::unexpected{logTotal.error()};
        }
        Eigen::Index const count = state.size();
        Eigen::Index const held = std::min(Eigen::Index(result.modes.size()), count);
        result.modes.erase(result.modes.begin() + held, result.modes.end());
        internal::UpdateAccess::logLikelihood(result) = Scalar(0);
        result.accepted = false;
        m_posterior.clear();
        auto logLikelihoods = typename StateType::ModesType::ArrayType{};
        logLikelihoods.resize(count);
        for (Eigen::Index m = 0; m < count; ++m)
        {
            auto const updateMode = [&](auto const & measurement) -> std::expected<void, NumericError> {
                if constexpr (IsForm)
                {
                    static_assert(
                        internal::isCollapsePolicy<Policy>,
                        "a square-root form has one branch and takes the Collapse policy alone; gating and selection "
                        "among noise components are the covariance form's (decisions.md [gaussian-forms])"
                    );
                    auto updated = state.updateConditional(m, [&](ComponentType & conditional) {
                        return internal::updateInPlace<true>(conditional, measurement);
                    });
                    if (!updated)
                    {
                        result.accepted = false;
                        return std::unexpected{updated.error()};
                    }
                    relabel(*updated, m);
                    updated->branches.front().mode = m;
                    logLikelihoods(m) = updated->logLikelihood();
                    result.accepted = true;
                    if (std::size_t(m) == result.modes.size())
                    {
                        result.modes.push_back(std::move(*updated));
                    }
                    else
                    {
                        result.modes[std::size_t(m)] = std::move(*updated);
                    }
                }
                else
                {
                    auto const updated = internal::updateSlot<true>(
                        result.modes,
                        std::size_t(m),
                        ResultAllocator(result.modes.get_allocator()),
                        state.conditional(m),
                        measurement,
                        [&](auto & measured) {
                            relabel(measured, m);
                            for (auto & branch : measured.branches)
                            {
                                branch.mode = m;
                            }
                            static_assert(
                                ReductionPolicy<Policy, std::remove_cvref_t<decltype(measured)>, ComponentType>,
                                "an IMM filter keeps one component per mode, so its policy must keep one: Collapse, "
                                "Select, or a Gate over them"
                            );
                            auto kept = std::optional<ComponentType>{};
                            auto out =
                                internal::OneComponentOutput<ComponentType, Scalar, std::optional<ComponentType>>{kept};
                            internal::reduceInto(policy, std::move(measured.branches), out);
                            // The mode is weighted by the likelihood of the branches what it kept stands for.
                            logLikelihoods(m) = out.added ? measured.logLikelihood() + out.logWeight
                                                          : -Eigen::NumTraits<Scalar>::infinity();
                            m_posterior.push_back(out.added ? std::move(*kept) : state.conditional(m));
                            result.accepted = result.accepted || out.added;
                            return out.added;
                        }
                    );
                    if (!updated)
                    {
                        result.accepted = false;
                        return updated;
                    }
                }
                return {};
            };
            if (auto const updated = withMeasurement(m, updateMode); !updated)
            {
                return updated;
            }
        }
        auto evidence = LogSumExp<Scalar>{};
        for (Eigen::Index m = 0; m < count; ++m)
        {
            evidence.add(state.modes().logWeight(m) + result.modes[std::size_t(m)].logLikelihood());
        }
        internal::UpdateAccess::logLikelihood(result) = evidence.value() - *logTotal;
        if (!result.accepted)
        {
            return {};
        }
 
        CategoricalParams<ModeSpaceType, Scalar> modes = state.modes();
        if (!modes.update(logLikelihoods))
        {
            // Every accepting mode has probability zero: the measurement has no density under the
            // modes that could hold it, so it is rejected [update-outcomes].
            result.accepted = false;
            for (auto & mode : result.modes)
            {
                mode.accepted = false;
            }
            return {};
        }
        if constexpr (!IsForm)
        {
            state.swapConditionals(m_posterior);
        }
        state.setModes(std::move(modes));
        return {};
    }
 
    Model_ m_model;                                   
    EMRYS_FILTER_NO_UNIQUE_ADDRESS Method_ m_method;  
 
    // Scratch, exchanged with the state's components: the predicted ones and the posterior ones.
    ConditionalsType m_predicted;  
 
    ConditionalsType m_posterior;  
};
 
namespace internal
{

template <
    Component Component_,
    DiscreteSpace ModeSpace_,
    typename HybridAllocator,
    typename Model,
    typename Method,
    typename Allocator>
struct RecursionFor<Hybrid<Component_, ModeSpace_, HybridAllocator>, Model, Method, Allocator>
{
    using type = ImmRecursion<Component_, Model, Method, HybridAllocator>;
};

 
}  // namespace internal

template <
    Component Component_,
    typename Model_,
    typename Method_ = DefaultMethodFor<Model_>,
    typename Allocator_ = DefaultAllocator,
    typename Upkeep_ = upkeep::None>
class ImmFilter : public internal::HeldUpkeep<ImmFilter<Component_, Model_, Method_, Allocator_, Upkeep_>, Upkeep_>
{
    using UpkeepBase = internal::HeldUpkeep<ImmFilter, Upkeep_>;  
 
public:

    using RecursionType = ImmRecursion<Component_, Model_, Method_, Allocator_>;
 
    using Scalar = typename RecursionType::Scalar;                
    using SpaceType = typename RecursionType::SpaceType;          
    using ModelType = Model_;                                     
    using MethodType = Method_;                                   
    using ModeSpaceType = typename RecursionType::ModeSpaceType;  
    using StateType = typename RecursionType::StateType;          
    using ComponentType = Params<Component_>;                     
    using allocator_type = Allocator_;                            
    using JointType = typename RecursionType::JointType;          

    ImmFilter(
        Model_ model,
        StateType state,
        MethodFor<Model_, Method_> method = {},
        std::type_identity_t<Upkeep_> upkeep = {}
    )
        : UpkeepBase{std::move(upkeep)}
        , m_state{std::move(state)}
        , m_recursion{std::move(model), std::move(method), m_state.get_allocator()}
    {
        EMRYS_FILTER_ASSERT(
            m_recursion.modeSpace() == m_state.modeSpace() && "the model and the state have different modes"
        );
        internal::assertPartitions(
            m_recursion.model(),
            m_state.space(),
            "every process model must write every coordinate of the state space once"
        );
    }

    ImmFilter(Model_ model, StateType state, std::type_identity_t<Upkeep_> upkeep)
        requires(!std::same_as<Upkeep_, Emrys::upkeep::None>)
        : ImmFilter{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)
    {
        EMRYS_FILTER_ASSERT(state.modeSpace() == m_recursion.modeSpace() && "the state has different modes");
        internal::assertPartitions(
            m_recursion.model(),
            state.space(),
            "every process model must write every coordinate of the state space once"
        );
        m_state = std::move(state);
        if (m_state.get_allocator() != m_recursion.get_allocator())
        {
            // The assignment carried the new state's allocator in, so the scratch follows it.
            m_recursion.resetScratch(m_state.get_allocator());
        }
    }

    [[nodiscard]] RecursionType const &
    recursion() const
    {
        return m_recursion;
    }

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

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

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

    template <typename... Inputs>
        requires(!isFixedStep<Model_>)
    [[nodiscard]] auto
    predict(TimeStep<Scalar> auto const & dt, Inputs const &... inputs)
    {
        return m_recursion.predict(m_state, internal::toScalarTime<Scalar>(dt), inputs...);
    }

    template <typename... Inputs>
        requires(isFixedStep<Model_> && !internal::leadsWithSteps<Inputs...>)
    [[nodiscard]] auto
    predict(Inputs const &... inputs)
    {
        internal::rejectTimeStep<Inputs...>();
        return m_recursion.predict(m_state, inputs...);
    }

    template <typename... Inputs>
        requires isFixedStep<Model_>
    [[nodiscard]] auto
    predict(Steps steps, Inputs const &... inputs)
    {
        EMRYS_FILTER_ASSERT(steps.count >= 0 && "the number of steps must be nonnegative");
        using Result = decltype(m_recursion.predict(m_state, inputs...));
        if constexpr (std::is_void_v<Result>)
        {
            for (Eigen::Index i = 0; i < steps.count; ++i)
            {
                m_recursion.predict(m_state, inputs...);
            }
        }
        else
        {
            for (Eigen::Index i = 0; i < steps.count; ++i)
            {
                if (Result result = m_recursion.predict(m_state, inputs...); !result)
                {
                    return result;
                }
            }
            return Result{};
        }
    }

    template <typename... Inputs>
        requires(!isFixedStep<Model_>)
    [[nodiscard]] JointType
    predictJoint(TimeStep<Scalar> auto const & dt, Inputs const &... inputs)
    {
        return m_recursion.predictJoint(m_state, internal::toScalarTime<Scalar>(dt), inputs...);
    }

    template <typename JointAllocator, typename... Inputs>
        requires(!isFixedStep<Model_>)
    void
    predictJoint(
        TimeStep<Scalar> auto const & dt,
        HybridPredictedJoint<SpaceType, ModeSpaceType, Scalar, JointAllocator> & joint,
        Inputs const &... inputs
    )
    {
        m_recursion.predictJoint(m_state, internal::toScalarTime<Scalar>(dt), joint, inputs...);
    }

    template <typename... Inputs>
        requires(isFixedStep<Model_> && !internal::leadsWithSteps<Inputs...>)
    [[nodiscard]] JointType
    predictJoint(Inputs const &... inputs)
    {
        internal::rejectTimeStep<Inputs...>();
        return m_recursion.predictJoint(m_state, inputs...);
    }

    template <typename JointAllocator, typename... Inputs>
        requires isFixedStep<Model_>
    void
    predictJoint(
        HybridPredictedJoint<SpaceType, ModeSpaceType, Scalar, JointAllocator> & joint,
        Inputs const &... inputs
    )
    {
        m_recursion.predictJoint(m_state, joint, inputs...);
    }

    template <Measurement M, typename Policy = reductions::Collapse>
    [[nodiscard]] auto
    update(M const & measurement, Policy const & policy = {})
    {
        return m_recursion.update(m_state, measurement, policy);
    }

    template <Measurement M, typename Policy, typename ModeUpdate, typename ResultAllocator>
    [[nodiscard]] std::expected<void, NumericError>
    update(M const & measurement, Policy const & policy, ImmFilterUpdate<ModeUpdate, Scalar, ResultAllocator> & result)
    {
        return m_recursion.update(m_state, measurement, policy, result);
    }

    template <typename P, Measurement... Ms, typename Policy = reductions::Collapse>
    [[nodiscard]] auto
    update(SwitchingMeasurement<P, Ms...> const & measurement, Policy const & policy = {})
    {
        return m_recursion.update(m_state, measurement, policy);
    }

    template <typename P, Measurement... Ms, typename Policy, typename ModeUpdate, typename ResultAllocator>
    [[nodiscard]] std::expected<void, NumericError>
    update(
        SwitchingMeasurement<P, Ms...> const & measurement,
        Policy const & policy,
        ImmFilterUpdate<ModeUpdate, Scalar, ResultAllocator> & result
    )
    {
        return m_recursion.update(m_state, measurement, policy, result);
    }
 
private:
 
    StateType m_state;          
    RecursionType m_recursion;  
};

// One guide with a defaulted method, its parameters in the class's order.
template <
    Component C,
    typename Model,
    typename Method = DefaultMethodFor<Model>,
    typename Allocator,
    typename ModeSpace>
    requires(!internal::UpkeepArgument<Method>)
ImmFilter(Model, HybridParams<C, ModeSpace, Allocator>, Method = {}) -> ImmFilter<
    C,
    internal::BoundStep<Model, typename Params<C>::SpaceType, typename Params<C>::Scalar>,
    Method,
    Allocator>;
 
template <Component C, typename Model, typename Allocator, typename ModeSpace, typename U>
    requires internal::UpkeepArgument<U>
ImmFilter(Model, HybridParams<C, ModeSpace, Allocator>, U) -> ImmFilter<
    C,
    internal::BoundStep<Model, typename Params<C>::SpaceType, typename Params<C>::Scalar>,
    DefaultMethodFor<Model>,
    Allocator,
    U>;
 
template <Component C, typename Model, typename Method, typename Allocator, typename ModeSpace, typename U>
ImmFilter(Model, HybridParams<C, ModeSpace, Allocator>, Method, U) -> ImmFilter<
    C,
    internal::BoundStep<Model, typename Params<C>::SpaceType, typename Params<C>::Scalar>,
    Method,
    Allocator,
    U>;
 
}  // namespace Emrys
 
#ifndef EMRYS_FILTER_NO_IO
 
namespace Emrys
{
 
namespace internal
{

template <typename Component_, typename Model_, typename Method_, typename Allocator_, typename Upkeep_>
void
describe(
    std::ostream & stream,
    ImmFilter<Component_, Model_, Method_, Allocator_, Upkeep_> const & filter,
    Indent indent
)
{
    describeFilter(stream, "interacting multiple model filter", filter.state(), indent);
}
 
}  // namespace internal
 
}  // namespace Emrys
 
#endif  // EMRYS_FILTER_NO_IO