MeasurementReads.h

Go to the documentation of this file.

#pragma once

 
#include <array>
#include <concepts>
#include <cstddef>
#include <expected>
#include <optional>
#include <span>
#include <tuple>
#include <type_traits>
#include <utility>
#include <variant>
#include <vector>
 
#include <Eigen/Cholesky>
 
#include <EmrysFilter/Core/Allocation.h>
#include <EmrysFilter/Core/Execution.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Multiply.h>
#include <EmrysFilter/Core/Solve.h>
#include <EmrysFilter/Core/Weights.h>
#include <EmrysFilter/Distributions/Gaussian.h>
#include <EmrysFilter/Distributions/Mixture.h>
#include <EmrysFilter/Distributions/Product.h>
#include <EmrysFilter/Models/Batched.h>
#include <EmrysFilter/Models/MeasurementModel.h>
#include <EmrysFilter/Models/MeasurementUpdate.h>
#include <EmrysFilter/Models/Policies.h>
#include <EmrysFilter/Models/ResolvedReads.h>
#include <EmrysFilter/Models/Transform.h>
#include <EmrysFilter/Spaces/Composite.h>
#include <EmrysFilter/Spaces/Coordinates.h>
#include <EmrysFilter/Spaces/Indexing.h>
#include <EmrysFilter/Spaces/Space.h>
#include <EmrysFilter/Spaces/Subspace.h>
#include <EmrysFilter/Spaces/TagPath.h>
 
namespace Emrys
{
 
namespace internal
{

struct NoNoise
{};

template <typename M, typename Space>
class ResolvedMeasurement : public ResolvedReads<typename M::ModelType, typename M::ArgumentsType, Space>
{
    using Base = ResolvedReads<typename M::ModelType, typename M::ArgumentsType, Space>;
 
public:
 
    using ObservedType = M;                         
    using Model = typename M::ModelType;            
    using Function = typename Model::FunctionType;  
    using Method = typename M::MethodType;          
 
    static constexpr bool HasFunction = Model::HasFunction;      
    static constexpr bool HasMethod = M::HasMethod;              
    static constexpr bool HasObservation = M::HasObservation;    
    static constexpr bool OwnNoise = requires { M::OwnNoise; };  

    static constexpr bool HasAdditiveNoise = OwnNoise || Model::template has<PolicyKind::additiveNoise>;

    static constexpr bool HasLogLikelihood = Model::template has<PolicyKind::logLikelihood>;

    static constexpr bool HasSample = Model::template has<PolicyKind::sample>;

    static constexpr bool BatchedLikelihood = [] {
        if constexpr (HasLogLikelihood)
        {
            return isBatched<decltype(Model::template Policy<PolicyKind::logLikelihood>::function)>;
        }
        else
        {
            return false;
        }
    }();
 
    using Base::ArgumentCount;
    using Base::InputCount;
    using Base::ItemCount;
    using Base::StateCount;
    using typename Base::In;
    using typename Base::Reads;
    using typename Base::StatePaths;

    template <std::size_t I>
    using Item = typename Base::template Item<I>;

    template <std::size_t J>
    using Argument = typename Base::template Argument<J>;

    template <typename T>
    using InVector = typename Base::template InVector<T>;
 
private:
 
    using Base::argumentIsUncertain;
    using Base::argumentOf;
    using Base::inputIndexOf;
    using Base::inputItems;
    using Base::inState;
    using Base::m_differenceRows;
    using Base::m_in;
    using Base::m_reads;
    using Base::m_stateDifferenceSize;
    using Base::m_valueRows;
    using Base::stateIndexOf;
    using Base::stateItems;

    using ProbeScalar = double;

    static constexpr bool OutputFromNoise = [] {
        if constexpr (Model::template has<PolicyKind::outputSpace> || Model::template has<PolicyKind::writes>)
        {
            return false;
        }
        else if constexpr (Model::template has<PolicyKind::additiveNoise>)
        {
            return Model::template Policy<PolicyKind::additiveNoise>::HasOwnSpace;
        }
        else
        {
            return false;
        }
    }();

    [[nodiscard]] static consteval auto
    outputType()
    {
        if constexpr (Model::template has<PolicyKind::outputSpace>)
        {
            using Policy = typename Model::template Policy<PolicyKind::outputSpace>;
            return std::type_identity<decltype(std::declval<Policy const &>().in(std::declval<Space const &>()))>{};
        }
        else if constexpr (Model::template has<PolicyKind::writes>)
        {
            return std::type_identity<typename Model::template Policy<PolicyKind::writes>::OutputSpace>{};
        }
        else if constexpr (OutputFromNoise)
        {
            return std::type_identity<
                typename Model::template Policy<PolicyKind::additiveNoise>::Distribution::SpaceType>{};
        }
        else if constexpr (requires { typename Function::OutputSpaceType; })
        {
            return std::type_identity<typename Function::OutputSpaceType>{};
        }
        else if constexpr (HasFunction && functionForm<Function, In, NoOutputSpace, InVector<ProbeScalar>> == 0)
        {
            // A function in no form: instantiating callForm() gives its message, and a space of one
            // coordinate stands in for the output's, so that what follows adds no errors about it.
            using Refused [[maybe_unused]] = decltype(callForm(
                std::declval<Function const &>(),
                std::declval<In const &>(),
                std::declval<NoOutputSpace const &>(),
                std::declval<InVector<ProbeScalar> const &>()
            ));
            return std::type_identity<Linear<1>>{};
        }
        else if constexpr (HasFunction)
        {
            using Returned = decltype(vectorReturned(callForm(
                std::declval<Function const &>(),
                std::declval<In const &>(),
                std::declval<NoOutputSpace const &>(),
                std::declval<InVector<ProbeScalar> const &>()
            )));
            using Evaluated = std::remove_cvref_t<decltype(valueOf(std::declval<Returned>()).eval())>;
            static_assert(Evaluated::ColsAtCompileTime == 1, "a measurement model's function returns a column vector");
            return std::type_identity<Linear<Evaluated::RowsAtCompileTime, Evaluated::MaxRowsAtCompileTime>>{};
        }
        else
        {
            return std::type_identity<NoOutputSpace>{};
        }
    }
 
public:

    using Out = typename decltype(outputType())::type;
 
    static constexpr bool HasOutputSpace = !std::is_same_v<Out, NoOutputSpace>;  

    using NoiseType = typename decltype([] {
        if constexpr (OwnNoise)
        {
            return std::type_identity<typename M::NoiseType>{};
        }
        else if constexpr (HasAdditiveNoise)
        {
            static_assert(
                HasOutputSpace,
                "additive noise needs the model's output space, from writes(), outputSpace(), or its function's result"
            );
            return std::type_identity<decltype(placeNoise(
                std::declval<Out const &>(),
                std::declval<Model const &>().template policy<PolicyKind::additiveNoise>()
            ))>{};
        }
        else
        {
            return std::type_identity<NoNoise>{};
        }
    }())::type;

    static constexpr bool MixtureNoise = [] {
        if constexpr (HasAdditiveNoise)
        {
            return isGaussianMixture<typename NoiseType::Distribution>;
        }
        else
        {
            return false;
        }
    }();

    using InnovationRepair = typename M::InnovationRepair;

    using ConsiderType = typename M::ConsiderType;

    ResolvedMeasurement(M const & measurement, Space const & space)
        : Base{measurement.arguments(), space}
        , m_measurement{measurement}
        , m_out{makeOut(measurement, space)}
        , m_noise{makeNoise(measurement)}
    {}

    [[nodiscard]] M const &
    observed() const
    {
        return m_measurement;
    }

    [[nodiscard]] Model const &
    model() const
    {
        return m_measurement.model();
    }

    [[nodiscard]] Method const &
    method() const
    {
        return m_measurement.method();
    }

    [[nodiscard]] decltype(auto)
    observation() const
        requires HasObservation
    {
        return m_measurement.observation();
    }

    [[nodiscard]] decltype(auto)
    covarianceUpdate() const
    {
        return m_measurement.covarianceUpdate();
    }

    [[nodiscard]] Out const &
    space() const
        requires HasOutputSpace
    {
        return m_out;
    }

    [[nodiscard]] NoiseType const &
    noise() const
        requires HasAdditiveNoise
    {
        return m_noise;
    }

    template <typename Joint, typename... Inputs>
    [[nodiscard]] auto
    evaluate(Joint const & joint, Inputs const &... inputs) const
    {
        static_assert(sizeof...(Inputs) == InputCount, "the function takes one vector per uncertain input");
        using T = typename Joint::Scalar;
        return split<false>(call(this->template gather<T>(joint, std::tuple<Inputs const &...>{inputs...})));
    }

    template <typename State, typename... Inputs>
    [[nodiscard]] auto
    evaluateAtState(State const & state, Inputs const &... inputs) const
    {
        static_assert(
            sizeof...(Inputs) == InputCount || sizeof...(Inputs) == 0,
            "the function takes one vector per uncertain input, or none to read each at its mean"
        );
        using T = typename State::Scalar;
        return split<true>(
            call(this->template gatherFromRows<T>(state, Eigen::Index{0}, std::tuple<Inputs const &...>{inputs...}))
        );
    }

    template <typename State>
    [[nodiscard]] auto
    logLikelihoodAt(State const & state) const
        requires(HasLogLikelihood && HasObservation)
    {
        return logLikelihoodAtRows(state, Eigen::Index{0});
    }

    template <typename Rows>
    [[nodiscard]] auto
    logLikelihoodAtRows(Rows const & rows, Eigen::Index start) const
        requires(HasLogLikelihood && HasObservation)
    {
        using T = typename Rows::Scalar;
        if constexpr (BatchedLikelihood)
        {
            // A batch of one column.
            auto x = InMatrix<T>{};
            x.resize(valueDimension(m_in), 1);
            this->template gatherColumnsFromRows<T>(
                x,
                [&](Eigen::Index) -> Rows const & {
                    return rows;
                },
                start
            );
            auto result = Eigen::Matrix<T, 1, 1>{T(0)};
            batchedLogLikelihoods(x, result);
            return result(0);
        }
        else
        {
            return callWithScalar<T>(
                model().template policy<PolicyKind::logLikelihood>().function,
                m_in,
                this->template gatherFromRows<T>(rows, start, std::tuple<>{}),
                m_measurement.observation()
            );
        }
    }

    template <typename State, typename G, typename... Inputs>
    [[nodiscard]] auto
    sampleAtState(State const & state, G & rng, Inputs const &... inputs) const
        requires HasSample
    {
        static_assert(
            sizeof...(Inputs) == InputCount || sizeof...(Inputs) == 0,
            "the draw takes one vector per uncertain input, or none to read each at its mean"
        );
        using T = typename State::Scalar;
        return callWithScalar<T>(
            model().template policy<PolicyKind::sample>().function,
            m_in,
            this->template gatherFromRows<T>(state, Eigen::Index{0}, std::tuple<Inputs const &...>{inputs...}),
            rng
        );
    }

    template <typename T>
    using InMatrix = MatrixOf<
        T,
        valueDimensionAtCompileTime<In>,
        Eigen::Dynamic,
        maxValueDimensionAtCompileTime<In>,
        Eigen::Dynamic>;

    static constexpr bool BatchedFunction = takesBatches<Function>;

    template <typename T, typename S = Out>
    using OutMatrix =
        MatrixOf<T, valueDimensionAtCompileTime<S>, Eigen::Dynamic, maxValueDimensionAtCompileTime<S>, Eigen::Dynamic>;

    template <typename X, typename Y>
    void
    evaluateBatch(X const & x, Y && y) const
        requires BatchedFunction
    {
        callBatched(m_measurement.model().function(), m_in, m_out, x, y);
    }

    template <typename X, typename Y, typename J>
    void
    evaluateBatchWithInputJacobians(X const & x, Y & y, J & jacobians) const
        requires BatchedFunction
    {
        auto wanted = std::array<bool, std::tuple_size_v<InputBlocks<In>>>{};
        [&]<std::size_t... I>(std::index_sequence<I...>) {
            ((wanted[I] = argumentIsUncertain(I)), ...);
        }(std::make_index_sequence<ItemCount>{});
        callBatchedWithJacobians(m_measurement.model().function(), m_in, m_out, x, wanted, y, jacobians);
    }

    template <typename Y, typename Joint, typename... Inputs>
    [[nodiscard]] auto
    gatherColumns(Joint const & joint, Inputs const &... inputs) const
    {
        static_assert(sizeof...(Inputs) == InputCount);
        using T = typename Joint::Scalar;
        using X = MatrixOf<
            T,
            valueDimensionAtCompileTime<In>,
            Y::ColsAtCompileTime,
            maxValueDimensionAtCompileTime<In>,
            Y::MaxColsAtCompileTime>;
        auto x = X{};
        x.resize(valueDimension(m_in), joint.cols());
        for (Eigen::Index j = 0; j < joint.cols(); ++j)
        {
            x.col(j) = this->template gather<T>(joint.col(j), std::tuple{inputs.col(j)...});
        }
        return x;
    }

    template <typename Y, typename Joint, typename... Inputs>
    void
    evaluateColumns(Y & y, Joint const & joint, Inputs const &... inputs) const
        requires BatchedFunction
    {
        callBatched(m_measurement.model().function(), m_in, m_out, gatherColumns<Y>(joint, inputs...), y);
    }

    template <typename Y, typename States, typename... Inputs>
    void
    evaluateStateColumns(Y & y, States const & states, Inputs const &... inputs) const
        requires BatchedFunction
    {
        static_assert(sizeof...(Inputs) == InputCount);
        using T = typename States::Scalar;
        using X = MatrixOf<
            T,
            valueDimensionAtCompileTime<In>,
            Y::ColsAtCompileTime,
            maxValueDimensionAtCompileTime<In>,
            Y::MaxColsAtCompileTime>;
        auto x = X{};
        x.resize(valueDimension(m_in), states.cols());
        for (Eigen::Index j = 0; j < states.cols(); ++j)
        {
            x.col(j) = this->template gatherFromRows<T>(states.col(j), Eigen::Index{0}, std::tuple{inputs.col(j)...});
        }
        callBatched(m_measurement.model().function(), m_in, m_out, x, y);
    }

    template <typename Y, typename J, typename Joint, typename... Inputs>
    void
    evaluateColumnsWithAffineJacobians(Y & y, J & jacobians, Joint const & joint, Inputs const &... inputs) const
        requires BatchedFunction
    {
        auto wanted = std::array<bool, std::tuple_size_v<InputBlocks<In>>>{};
        [&]<std::size_t... I>(std::index_sequence<I...>) {
            ((wanted[I] = Base::template itemFactored<I>()), ...);
        }(std::make_index_sequence<ItemCount>{});
        auto const x = gatherColumns<Y>(joint, inputs...);
        callBatchedWithJacobians(m_measurement.model().function(), m_in, m_out, x, wanted, y, jacobians);
    }

    template <typename X, typename Result>
    void
    batchedLogLikelihoods(X const & x, Result && result) const
        requires(BatchedLikelihood && HasObservation)
    {
        using T = typename X::Scalar;
        using Likelihood = typename Model::template Policy<PolicyKind::logLikelihood>;
        using Output = BatchedOutput<In, Linear<1>, T, typename decltype(Likelihood::function)::MarkType>;
        EMRYS_FILTER_ASSERT(result.size() == x.cols() && "give one entry per column of the batch");
        auto y = Output{
            typename Output::ValuesType{result.data(), 1, x.cols()},
            typename Output::JacobiansType{nullptr, 1, 0},
            m_in,
            {}
        };
        callWithScalar<T>(
            model().template policy<PolicyKind::logLikelihood>().function.function,
            m_in,
            x,
            m_measurement.observation(),
            y
        );
    }
 
private:

    [[nodiscard]] static Out
    makeOut([[maybe_unused]] M const & measurement, [[maybe_unused]] Space const & space)
    {
        if constexpr (Model::template has<PolicyKind::outputSpace>)
        {
            return measurement.model().template policy<PolicyKind::outputSpace>().in(space);
        }
        else if constexpr (OutputFromNoise)
        {
            return measurement.model().template policy<PolicyKind::additiveNoise>().distribution().space();
        }
        else if constexpr (FixedSizeSpace<Out> || !HasOutputSpace)
        {
            return Out{};
        }
        else
        {
            static_assert(
                requires(Function const & f) { f.outputRows(); },
                "a function that returns a vector of run-time size names its output with writes() or outputSpace()"
            );
            return Out{measurement.model().function().outputRows()};
        }
    }

    [[nodiscard]] NoiseType
    makeNoise([[maybe_unused]] M const & measurement) const
    {
        if constexpr (OwnNoise)
        {
            return measurement.noise();
        }
        else if constexpr (HasAdditiveNoise)
        {
            return placeNoise(m_out, measurement.model().template policy<PolicyKind::additiveNoise>());
        }
        else
        {
            return {};
        }
    }

    template <typename X>
    [[nodiscard]] auto
    call(X const & x) const
    {
        static_assert(HasFunction, "a measurement model known by its likelihood alone has no function to evaluate");
        return vectorReturned(callForm(m_measurement.model().function(), m_in, m_out, x));
    }

    template <bool Whole, typename R>
    [[nodiscard]] auto
    split(R && returned) const
    {
        using Plain = std::remove_cvref_t<R>;
        if constexpr (!isWithJacobian<Plain>)
        {
            return std::forward<R>(returned);
        }
        else
        {
            using J = std::remove_cvref_t<decltype(returned.jacobian)>;
            static_assert(
                MatrixExpression<J>,
                "a measurement model's function returns its Jacobian as one matrix over its input's differences, "
                "withJacobian(y, J)"
            );
            auto const & jacobian = returned.jacobian;
            EMRYS_FILTER_ASSERT(
                jacobian.cols() == differenceDimension(m_in)
                && "a returned Jacobian has a column per difference coordinate of the model's input"
            );
            using Scalar = typename J::Scalar;
            constexpr int Rows = J::RowsAtCompileTime;
            constexpr int MaxRows = J::MaxRowsAtCompileTime;
            constexpr int Cols = Whole ? valueDimensionAtCompileTime<DifferenceSpace<Space>> : Reads::DifferenceSize;
            constexpr int MaxCols =
                Whole ? maxValueDimensionAtCompileTime<DifferenceSpace<Space>> : Reads::DifferenceMaxSize;
            using StateBlock = MatrixOf<Scalar, Rows, Cols, MaxRows, MaxCols>;
            StateBlock stateBlock =
                StateBlock::Zero(jacobian.rows(), Whole ? m_stateDifferenceSize : m_reads.differenceIndices().size());
            [&]<std::size_t... K>(std::index_sequence<K...>) {
                ((stateBlock.middleCols(
                      Whole ? m_reads.differenceRows(K).start : m_reads.jointDifferenceStart(K),
                      m_differenceRows[stateItems(K)].size
                  ) = jacobian.middleCols(m_differenceRows[stateItems(K)].start, m_differenceRows[stateItems(K)].size)),
                 ...);
            }(std::make_index_sequence<StateCount>{});
            if constexpr (InputCount == 0)
            {
                return withJacobian(std::move(returned.value), std::move(stateBlock));
            }
            else
            {
                auto blocks = [&]<std::size_t... P>(std::index_sequence<P...>) {
                    return std::tuple{std::move(stateBlock), inputBlock<P>(jacobian)...};
                }(std::make_index_sequence<InputCount>{});
                return withJacobian(std::move(returned.value), std::move(blocks));
            }
        }
    }

    template <std::size_t P, typename J>
    [[nodiscard]] auto
    inputBlock(J const & jacobian) const
    {
        using Input =
            std::remove_cvref_t<decltype(inputOf(std::declval<Argument<argumentOf(inputItems(P))> const &>()))>;
        using Block = MatrixOf<
            typename J::Scalar,
            J::RowsAtCompileTime,
            Input::DifferenceDimensionAtCompileTime,
            J::MaxRowsAtCompileTime,
            Input::MaxDifferenceDimensionAtCompileTime>;
        BlockRows const & rows = m_differenceRows[inputItems(P)];
        Block block = jacobian.middleCols(rows.start, rows.size);
        return block;
    }
 
    M const & m_measurement;                           
    EMRYS_FILTER_NO_UNIQUE_ADDRESS Out m_out;          
    EMRYS_FILTER_NO_UNIQUE_ADDRESS NoiseType m_noise;  
};

template <typename R>
class ResolvedFunction
{
public:

    static constexpr bool EvaluatesBatches = R::BatchedFunction;

    explicit ResolvedFunction(R const & resolved)
        : m_resolved{resolved}
    {}

    template <typename Joint, typename... Inputs>
    [[nodiscard]] auto
    operator()(Joint const & joint, Inputs const &... inputs) const
    {
        return m_resolved.evaluate(joint, inputs...);
    }

    template <typename Y, typename Joint, typename... Inputs>
    void
    evaluateBatch(Y & y, Joint const & joint, Inputs const &... inputs) const
        requires EvaluatesBatches
    {
        m_resolved.evaluateColumns(y, joint, inputs...);
    }

    template <typename Y, typename J, typename Joint, typename... Inputs>
    void
    evaluateBatchWithAffineJacobians(Y & y, J & jacobians, Joint const & joint, Inputs const &... inputs) const
        requires EvaluatesBatches
    {
        m_resolved.evaluateColumnsWithAffineJacobians(y, jacobians, joint, inputs...);
    }
 
private:
 
    R const & m_resolved;  
};

template <typename Space, typename M>
[[nodiscard]] ResolvedMeasurement<M, Space>
resolveMeasurement(M const & measurement, Space const & space)
{
    return ResolvedMeasurement<M, Space>{measurement, space};
}

template <typename Scalar, typename R, typename Space>
[[nodiscard]] typename Moments<Space, Scalar>::VectorType
observedValue(R const & resolved, Space const & space)
{
    using Vector = typename Moments<Space, Scalar>::VectorType;
    auto const & observation = resolved.observation();
    EMRYS_FILTER_ASSERT(
        observation.size() == valueDimension(space) && "the observation must be a value in the measurement space"
    );
    Vector result = observation.template cast<Scalar>();
    EMRYS_FILTER_ASSERT(result.allFinite() && "the observation must be finite");
    normalizeValues(space, result);
    return result;
}

template <typename Scalar, typename R, typename Space, typename Predicted>
[[nodiscard]] typename Moments<Space, Scalar>::DifferenceVectorType
observedResidual(R const & resolved, Space const & space, Eigen::MatrixBase<Predicted> const & predicted)
{
    using Difference = typename Moments<Space, Scalar>::DifferenceVectorType;
    return Difference{Emrys::minus(space, observedValue<Scalar>(resolved, space), predicted)};
}

template <typename R, typename Method, typename F, typename... Args>
[[nodiscard]] auto
transformPrediction(R const & resolved, Method const & method, F const & f, Args const &... args)
{
    return transformBy(method, f, resolved.space(), args...);
}

template <typename R, typename Method, typename F, typename Read>
[[nodiscard]] auto
transformMeasurement(R const & resolved, Method const & method, F const & f, Read const & read)
{
    return std::apply(
        [&](auto const &... inputs) {
            if constexpr (isSigmaPointMethod<Method> && R::factorsAffine())
            {
                auto const factored = FactoredAt<R::affineSize(), R::affineMaxSize(), Method>{
                    method,
                    resolved.template affineCoordinates<false>()
                };
                return transformPrediction(resolved, factored, f, withCross(read), inputs...);
            }
            else
            {
                return transformPrediction(resolved, method, f, withCross(read), inputs...);
            }
        },
        resolved.inputs()
    );
}

template <typename R, typename Method, typename F, typename G, typename... Args>
[[nodiscard]] decltype(auto)
linearizePrediction(R const & resolved, Method const & method, F const & f, G const & g, Args const &... args)
{
    using Problem = Transform<F const, typename R::Out, Args...>;
    auto const problem = Problem{f, resolved.space(), args...};
    return g(problem, linearizeAt(method, problem));
}

template <typename R, typename Method, typename F, typename Read, typename G>
[[nodiscard]] decltype(auto)
linearizeMeasurement(R const & resolved, Method const & method, F const & f, Read const & read, G const & g)
{
    return std::apply(
        [&](auto const &... inputs) -> decltype(auto) {
            return linearizePrediction(resolved, method, f, g, withCross(read), inputs...);
        },
        resolved.inputs()
    );
}

template <typename Space, typename Vector, typename Jacobian, typename Cross>
class PredictionMoments
{
public:

    PredictionMoments(Space const & space, Vector const & mean, Jacobian const & jacobian, Cross const & cross)
        : m_space{space}
        , m_mean{mean}
        , m_jacobian{jacobian}
        , m_cross{cross}
    {}

    [[nodiscard]] Space const &
    space() const
    {
        return m_space;
    }

    [[nodiscard]] Vector const &
    mean() const
    {
        return m_mean;
    }

    [[nodiscard]] auto
    covarianceLower() const
    {
        return multiply(m_jacobian, m_cross);
    }

    template <typename Matrix, typename Noise>
    [[nodiscard]] Matrix
    covariancePlus(Noise const & noise) const
    {
        Matrix result = noise;
        result.noalias() += multiply(m_jacobian, m_cross);
        return result;
    }

    [[nodiscard]] Eigen::Index
    valueDimension() const
    {
        return Emrys::valueDimension(m_space);
    }

    [[nodiscard]] Eigen::Index
    differenceDimension() const
    {
        return Emrys::differenceDimension(m_space);
    }
 
private:
 
    Space const & m_space;        
    Vector const & m_mean;        
    Jacobian const & m_jacobian;  
    Cross const & m_cross;        
};

template <typename Matrix, typename Output, typename Noise>
[[nodiscard]] Matrix
innovationCovariancePlus(Output const & y, Noise const & noise)
{
    if constexpr (requires { y.template covariancePlus<Matrix>(noise); })
    {
        return y.template covariancePlus<Matrix>(noise);
    }
    else
    {
        Matrix result = noise;
        result += y.covarianceLower();
        return result;
    }
}

struct NewPosteriors
{};

template <typename G, typename Accept = AcceptEvery>
struct IntoPrior
{
    G & state;      
    Accept accept;  
};

template <typename Sink>
inline constexpr bool isIntoPrior = false;

template <typename G, typename Accept>
inline constexpr bool isIntoPrior<IntoPrior<G, Accept>>{true};


template <typename Update>
struct ReusedUpdate
{
    Update * update;  
};

template <typename Sink>
inline constexpr bool isReusedUpdate = false;

template <typename Update>
inline constexpr bool isReusedUpdate<ReusedUpdate<Update>>{true};


template <typename Space, typename MeasurementSpace, typename Scalar, typename Sink>
[[nodiscard]] decltype(auto)
mixtureUpdateFor(Sink & sink)
{
    if constexpr (isReusedUpdate<Sink>)
    {
        using Update = std::remove_cvref_t<decltype(*sink.update)>;
        static_assert(
            std::is_same_v<typename Update::SpaceType, Space>
                && std::is_same_v<typename Update::MeasurementSpaceType, MeasurementSpace>
                && Update::BranchCount == std::dynamic_extent,
            "the update reused must be over the state and the innovation, with a run-time number of branches"
        );
        Update & result = *sink.update;
        result.branches.clear();
        return result;
    }
    else
    {
        return MeasurementUpdate<Space, MeasurementSpace, std::dynamic_extent, Scalar>{};
    }
}

template <typename Sink, typename Update>
using MixtureOutcome =
    std::conditional_t<isReusedUpdate<Sink>, std::expected<void, GaussianError>, std::expected<Update, GaussianError>>;

template <typename Sink, typename Update>
[[nodiscard]] MixtureOutcome<Sink, Update>
mixtureOutcome(Update & update)
{
    if constexpr (isReusedUpdate<Sink>)
    {
        return {};
    }
    else
    {
        return std::move(update);
    }
}

template <typename Jacobian, typename Indices, typename Error>
struct ReadLinearization
{
    Jacobian jacobian;        
    Indices const & indices;  
    Error error;              
};

template <typename Jacobian, typename Indices, typename Error>
ReadLinearization(Jacobian, Indices const &, Error) -> ReadLinearization<Jacobian, Indices, Error>;


template <typename Matrix, typename R, typename Problem, typename Jacobian>
[[nodiscard]] Matrix
inputsCovariance(R const & resolved, Problem const & problem, Jacobian const & jacobian)
{
    using Scalar = typename Matrix::Scalar;
    Eigen::Index const m = jacobian.rows();
    Matrix result = Matrix::Zero(m, m);
    auto const inputs = resolved.inputs();
    [&]<std::size_t... P>(std::index_sequence<P...>) {
        [[maybe_unused]] auto const add = [&]<std::size_t K>(std::integral_constant<std::size_t, K>) {
            auto const & input = inputOf(std::get<K>(inputs));
            using Input = std::remove_cvref_t<decltype(input)>;
            using NoiseMatrix = MatrixOf<
                Scalar,
                Input::DifferenceDimensionAtCompileTime,
                Input::DifferenceDimensionAtCompileTime,
                Input::MaxDifferenceDimensionAtCompileTime,
                Input::MaxDifferenceDimensionAtCompileTime>;
            using InputJacobian = MatrixOf<
                Scalar,
                Matrix::RowsAtCompileTime,
                Input::DifferenceDimensionAtCompileTime,
                Matrix::MaxRowsAtCompileTime,
                Input::MaxDifferenceDimensionAtCompileTime>;
            Eigen::Index const d = input.differenceDimension();
            InputJacobian const block = jacobian.middleCols(problem.differenceOffset(K + 1), d);
            NoiseMatrix const cov = symmetricFromLower<NoiseMatrix>(covarianceLowerOf(input));
            result.noalias() += multiply(multiply(block, cov), block.transpose());
        };
        (add(std::integral_constant<std::size_t, P>{}), ...);
    }(std::make_index_sequence<R::InputCount>{});
    if constexpr (R::InputCount > 0)
    {
        result = (Scalar(0.5) * (result + result.transpose())).eval();
    }
    return result;
}

template <typename P, typename... Qs>
inline constexpr bool liesInNoOther = ((std::is_same_v<P, Qs> || !pathLiesIn<P, Qs>) && ...);

template <typename C>
struct ConsideredRowsOf;

template <typename... Paths>
struct ConsideredRowsOf<Consider<Paths...>>
{
    template <typename Space>
    [[nodiscard]] static ConsideredRows<sizeof...(Paths)>
    resolve([[maybe_unused]] Space const & space)
    {
        static_assert(
            ([]<typename First, typename... Rest>(TagPath<First, Rest...> *) {
                return countTag<First, Space>() > 0;
            }(static_cast<Paths *>(nullptr))
             && ...),
            "a path of consider() names no block of the state"
        );
        static_assert((liesInNoOther<Paths, Paths...> && ...), "a block of consider() lies in another it names");
        return {differenceBlockRows<Paths>(space)...};
    }
};


template <typename R, typename Space>
[[nodiscard]] auto
consideredRows(Space const & space)
{
    return ConsideredRowsOf<typename R::ConsiderType>::resolve(space);
}
 
template <
    bool Statistics,
    typename Prior,
    typename R,
    typename Output,
    typename Cross,
    typename Jacobian,
    typename Sink>
[[nodiscard]] decltype(auto)
conditionOnPrediction(
    Prior const & prior,
    R const & resolved,
    Output const & y,
    Cross const & cross,
    Jacobian const & readJacobian,
    Sink && sink
);
 
}  // namespace internal
 
}  // namespace Emrys