Measurement.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/IteratedUpdate.h>
#include <EmrysFilter/Models/Likelihood.h>
#include <EmrysFilter/Models/MeasurementModel.h>
#include <EmrysFilter/Models/MeasurementReads.h>
#include <EmrysFilter/Models/MeasurementUpdate.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
{

template <bool Statistics, UncertainInput Prior, typename R, typename Sink, typename Reference = NoReference>
[[nodiscard]] decltype(auto)
updateResolved(Prior const & prior, R const & resolved, Sink sink, Reference const & reference = {})
{
    using Scalar = typename Prior::Scalar;
    using Space = typename Prior::SpaceType;
    using Reads = typename R::Reads;
    constexpr int N = valueDimensionAtCompileTime<DifferenceSpace<Space>>;
    constexpr int NMax = maxValueDimensionAtCompileTime<DifferenceSpace<Space>>;
 
    static_assert(
        R::HasMethod,
        "a Kalman update pushes the prior through the model by a method: observe(method, model, z)"
    );
    static_assert(
        R::HasFunction,
        "a Kalman update needs the model's function; a model known by its likelihood alone serves the filters that "
        "weight points"
    );
    auto const & reads = resolved.reads();
    auto const read = GaussianMarginal<Prior, typename Reads::Selection, typename Reads::JointSpace>{
        prior,
        reads.selection(),
        reads.space()
    };
    auto const f = ResolvedFunction{resolved};
    auto const & method = resolved.method();
    using Method = std::remove_cvref_t<decltype(method)>;
    auto const all = internal::consecutiveIndices<N, NMax>(0, prior.differenceDimension());
    // The covariance of the whole state with the blocks read, P_xS.
    using StateReadMatrix = internal::MatrixOf<Scalar, N, Reads::DifferenceSize, NMax, Reads::DifferenceMaxSize>;
    StateReadMatrix const stateRead =
        internal::lowerSymmetricBlock(covarianceLowerOf(prior), all, reads.differenceIndices());
 
    if constexpr (isLinearizedAbout<Reference>)
    {
        // One linearization about the reference serves every noise component, which conditionOnPrediction()
        // then branches on as it does for one at the prior.
        static_assert(checkReference<Method, Reference, Space>());
        using Vector = typename GaussianParams<Space, Scalar>::VectorType;
        using Difference = typename GaussianParams<Space, Scalar>::DifferenceVectorType;
        using ReadVector = typename MovedInput<std::remove_cvref_t<decltype(read)>>::VectorType;
        using ReadDifference = typename GaussianParams<typename Reads::JointSpace, Scalar>::DifferenceVectorType;
        Vector const point = reference.mean();
        EMRYS_FILTER_ASSERT(
            point.size() == prior.valueDimension() && "a point to linearize about must have the prior's value dimension"
        );
        Difference const step = Emrys::minus(prior.space(), point, prior.mean());
        ReadVector const readMean = read.mean();
        ReadDifference const readStep = step(reads.differenceIndices());
        auto const condition = [&](auto const & y, auto const & cross, auto const & readJacobian) -> decltype(auto) {
            return conditionOnPrediction<Statistics>(prior, resolved, y, cross, readJacobian, sink);
        };
        if constexpr (isLinearMethod<Method>)
        {
            return linearizeReadsAt<Space, Scalar>(
                resolved,
                method,
                reads,
                read,
                f,
                stateRead,
                readMean,
                true,
                readStep,
                std::monostate{},
                condition
            );
        }
        else if constexpr (Reference::IsDistribution)
        {
            using ReadMatrix = internal::MatrixOf<
                Scalar,
                Reads::DifferenceSize,
                Reads::DifferenceSize,
                Reads::DifferenceMaxSize,
                Reads::DifferenceMaxSize>;
            // The reference's spread over the blocks read, in its own chart, as an iterate's is in the iterate's.
            ReadMatrix const spreadCov = internal::lowerSymmetricBlock(
                reference.reference.covarianceLower(),
                reads.differenceIndices(),
                reads.differenceIndices()
            );
            return linearizeReadsAt<Space, Scalar>(
                resolved,
                method,
                reads,
                read,
                f,
                stateRead,
                readMean,
                true,
                readStep,
                spreadCov,
                condition
            );
        }
    }
    else if constexpr (isIterated<Method>)
    {
        if constexpr (R::MixtureNoise)
        {
            // Each component's branch is its own Iterated update, weighted as conditionOnPrediction()
            // weights the branches of one linearization.
            static_assert(!isIntoPrior<Sink>, "conditioning in place takes noise of one component");
            auto const & noise = resolved.noise().distribution();
            using Component = std::remove_cvref_t<decltype(noise.begin()->component)>;
            using One = WithNoiseComponent<typename R::ObservedType, Component>;
            using Single =
                typename decltype(updateInto<true>(prior, std::declval<One const &>(), NewPosteriors{}))::value_type;
            using MeasurementSpace = typename Single::MeasurementSpaceType;
            auto const total = noise.logTotalWeight();
            EMRYS_FILTER_ASSERT(total && "the noise mixture's weights must be valid");
            decltype(auto) result = mixtureUpdateFor<Space, MeasurementSpace, Scalar>(sink);
            using Outcome = MixtureOutcome<Sink, std::remove_cvref_t<decltype(result)>>;
            result.branches.reserve(std::size_t(noise.size()));
            auto evidence = LogSumExp<Scalar>{};
            Eigen::Index k = 0;
            for (auto const & entry : noise)
            {
                auto const one = One{resolved.observed(), entry.component};
                auto single = updateInto<true>(prior, one, NewPosteriors{});
                if (!single)
                {
                    return Outcome{std::unexpected{single.error()}};
                }
                auto & branch = single->branches.front();
                branch.component = k++;
                branch.logWeight = entry.logWeight - *total + branch.logLikelihood();
                evidence.add(branch.logWeight);
                result.branches.push_back(std::move(branch));
            }
            auto const logEvidence = evidence.value();
            MixtureEvidenceAccess::setMixtureLogLikelihood(result, logEvidence);
            for (auto & branch : result.branches)
            {
                branch.logWeight -= logEvidence;
            }
            return mixtureOutcome<Sink>(result);
        }
        else
        {
            return iteratedUpdate<Statistics>(prior, resolved, reads, read, f, stateRead, sink);
        }
    }
    else if constexpr (internal::isLinearMethod<Method>)
    {
        // The method sees the measurement as affine in the blocks read, J_S (S - mu_S) plus terms
        // independent of the state, so its cross-covariance with the whole state is P_xS J_S^T,
        // with no solve against P_SS.
        auto const condition = [&](auto const & problem, auto const & linearized) -> decltype(auto) {
            using Problem = std::remove_cvref_t<decltype(problem)>;
            using Predicted = OutputWithCross<Space, typename Problem::OutputSpaceType, JacobianForm, Scalar>;
            auto const & jacobian = linearized.jacobian;
            Eigen::Index const readSize = reads.differenceIndices().size();
            auto const readJacobian = ReadLinearization{
                jacobian.template middleCols<Reads::DifferenceSize>(problem.differenceOffset(0), readSize),
                reads.differenceIndices(),
                [&] {
                    using Error = typename Problem::OutputMatrix;
                    return internal::inputsCovariance<Error>(resolved, problem, jacobian);
                }
            };
            auto cross = typename Predicted::CrossMatrix{};
            cross.noalias() = internal::multiply(stateRead, readJacobian.jacobian.transpose());
            if constexpr (R::InputCount > 0)
            {
                typename Problem::CrossMatrix const joint =
                    internal::multiply(problem.covarianceLower(), jacobian.transpose());
                auto const y = internal::PredictionMoments{problem.outputSpace(), linearized.mean, jacobian, joint};
                return internal::conditionOnPrediction<Statistics>(prior, resolved, y, cross, readJacobian, sink);
            }
            else
            {
                // The blocks read are the only uncertain input, so P_SS J^T is the rows of
                // P_xS J^T at them, borrowed rather than formed again (decisions.md [borrowed-intermediates]).
                auto const joint = std::as_const(cross)(reads.differenceIndices(), Eigen::placeholders::all);
                auto const y = internal::PredictionMoments{problem.outputSpace(), linearized.mean, jacobian, joint};
                return internal::conditionOnPrediction<Statistics>(prior, resolved, y, cross, readJacobian, sink);
            }
        };
        return internal::linearizeMeasurement(resolved, method, f, read, condition);
    }
    else
    {
        // Lift the cross-covariance from the blocks read to the full state: P_xS P_SS^-1 Sigma_Sy, which
        // is P_xS S^-T Psi with S the root that spread the points, so P_SS is not factorized again
        // (decisions.md [linearization-forms]).
        auto const predicted = internal::transformMeasurement(resolved, method, f, read);
        using Predicted = std::remove_cvref_t<decltype(predicted)>;
        using Solved = typename Predicted::CrossMatrix;
        Solved const solved = internal::solveWithRoot<Solved>(predicted.root(), predicted.whitenedCross());
        using MeasurementSpace = std::remove_cvref_t<decltype(predicted.output().space())>;
        typename OutputWithCross<Space, MeasurementSpace, JacobianForm, Scalar>::CrossMatrix const cross =
            internal::multiply(stateRead, solved);
        // The measurement's Jacobian in the blocks read, Sigma_Sy^T P_SS^-1, and N + Omega, for updates::Joseph.
        auto const readJacobian =
            internal::ReadLinearization{solved.transpose(), reads.differenceIndices(), [&] {
                                            using Error = typename Predicted::OutputMatrix;
                                            return Error{
                                                predicted.inputsCovariance() + predicted.unexplainedCovariance()
                                            };
                                        }};
        return internal::conditionOnPrediction<Statistics>(
            prior,
            resolved,
            predicted.output(),
            cross,
            readJacobian,
            sink
        );
    }
}

template <bool Statistics, UncertainInput Prior, Measurement M, typename Sink, typename Reference = NoReference>
[[nodiscard]] decltype(auto)
updateInto(Prior const & prior, M const & measurement, Sink sink, Reference const & reference = {})
{
    auto const resolved = resolveMeasurement(measurement, prior.space());
    return updateResolved<Statistics>(prior, resolved, std::move(sink), reference);
}

template <bool Statistics, UncertainInput Prior, Measurement M, typename Reference = NoReference>
[[nodiscard]] auto
updateBranches(Prior const & prior, M const & measurement, Reference const & reference = {})
{
    return updateInto<Statistics>(prior, measurement, NewPosteriors{}, reference);
}

template <UncertainInput Prior, Measurement M>
using MeasuredUpdate =
    typename decltype(updateBranches<false>(std::declval<Prior const &>(), std::declval<M const &>()))::value_type;

template <
    bool Statistics,
    UncertainInput Prior,
    Measurement M,
    typename Space,
    typename MeasurementSpace,
    typename Scalar,
    typename Allocator,
    typename Reference = NoReference>
[[nodiscard]] std::expected<void, GaussianError>
updateBranchesInto(
    Prior const & prior,
    M const & measurement,
    MeasurementUpdate<Space, MeasurementSpace, std::dynamic_extent, Scalar, Allocator> & into,
    Reference const & reference = {}
)
{
    using Update = MeasurementUpdate<Space, MeasurementSpace, std::dynamic_extent, Scalar, Allocator>;
    return updateInto<Statistics>(prior, measurement, ReusedUpdate<Update>{&into}, reference);
}

template <
    bool Statistics,
    typename Space,
    typename Scalar,
    Measurement M,
    typename Reference = NoReference,
    typename Accept = AcceptEvery>
[[nodiscard]] auto
updateInPlace(
    GaussianParams<Space, Scalar> & state,
    M const & measurement,
    Reference const & reference = {},
    Accept const & accept = {}
)
{
    return updateInto<Statistics>(
        std::as_const(state),
        measurement,
        IntoPrior<GaussianParams<Space, Scalar>, Accept>{state, accept},
        reference
    );
}
 
}  // namespace internal

template <UncertainInput Prior, Measurement M>
[[nodiscard]] auto
update(Prior const & prior, M const & measurement)
{
    return internal::updateBranches<false>(internal::withCovariance(prior), measurement);
}

template <UncertainInput Prior, Measurement M, typename Reference>
[[nodiscard]] auto
update(Prior const & prior, M const & measurement, LinearizedAbout<Reference> const & about)
{
    return internal::updateBranches<false>(internal::withCovariance(prior), measurement, about);
}
 
namespace internal
{

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,
    [[maybe_unused]] Jacobian const & readJacobian,
    Sink && sink
)
{
    using Scalar = typename Prior::Scalar;
    using Space = typename Prior::SpaceType;
    using MeasurementSpace = std::remove_cvref_t<decltype(y.space())>;
    using MeasurementVector = typename GaussianParams<MeasurementSpace, Scalar>::VectorType;
    using InnovationVector = typename GaussianParams<MeasurementSpace, Scalar>::DifferenceVectorType;
    using InnovationMatrix = typename GaussianParams<MeasurementSpace, Scalar>::MatrixType;
    MeasurementVector const observed = observedValue<Scalar>(resolved, y.space());
 
    // updates::Joseph's H and linearization error, once for every noise component: of the measurement
    // before additive noise, which each component then adds back as its own R. H is the measurement's
    // Jacobian in the columns its ReadLinearization names, and the error the N + Omega it gives, a
    // product of the prediction's, never Sigma_yy less H P H^T (decisions.md [linearization-forms]).
    auto const & form = resolved.covarianceUpdate();
    using Form = std::remove_cvref_t<decltype(form)>;
    auto const linearization = [&] {
        if constexpr (std::is_same_v<Form, updates::Joseph>)
        {
            using Result = StatisticalLinearization<Space, MeasurementSpace, Scalar>;
            typename Result::JacobianMatrix jacobian =
                Result::JacobianMatrix::Zero(y.differenceDimension(), prior.differenceDimension());
            jacobian(Eigen::placeholders::all, readJacobian.indices) = readJacobian.jacobian;
            typename Result::ErrorMatrix errorCov = readJacobian.error();
            return Result{std::move(jacobian), std::move(errorCov)};
        }
        else
        {
            return std::tuple<>{};
        }
    }();
 
    constexpr bool MixtureParams = R::MixtureNoise;
    static_assert(
        !(MixtureParams && isIntoPrior<std::remove_cvref_t<Sink>>),
        "conditioning in place takes noise of one component"
    );
    using Branch = UpdateBranch<Space, MeasurementSpace, Scalar>;
    constexpr bool InPlace = isIntoPrior<std::remove_cvref_t<Sink>>;
    constexpr bool BranchStatistics = Statistics || MixtureParams;
    using BranchRepair = typename R::InnovationRepair;
    // The blocks considered, which every branch keeps as the prior has them [consider-states].
    auto const considered = consideredRows<R>(prior.space());
 
    // What the updates take to make updates::Joseph's terms for a noise component whose covariance is
    // noise: a callable that makes them when the form asks, and nothing for the forms that never
    // ask. noise must outlive the callable.
    auto const termsFor = [&]([[maybe_unused]] InnovationMatrix const & noise) {
        if constexpr (std::is_same_v<Form, updates::Joseph>)
        {
            return [&] {
                // The Jacobian is the same for every component, so the terms borrow it.
                using Linearization = StatisticalLinearization<Space, MeasurementSpace, Scalar>;
                return JosephTerms<typename Linearization::JacobianMatrix const &, InnovationMatrix>{
                    linearization.jacobian,
                    InnovationMatrix{linearization.errorCov + noise}
                };
            };
        }
        else
        {
            return std::tuple<>{};
        }
    };
 
    // The branch whose predicted measurement has mean mean and the covariance makeCov() returns,
    // conditioned on the residual z - mean, with its own posterior, or the error of a covariance
    // that is not positive definite (decisions.md [innovation-cholesky]). Its NIS and log-likelihood come from the
    // gain's factorization when they will be read, as weighting a mixture's branches reads them.
    auto const condition = [&](MeasurementVector const & mean,
                               auto const & makeCov,
                               InnovationMatrix const & noise,
                               Eigen::Index component) -> std::expected<Branch, GaussianError> {
        InnovationVector residual = Emrys::minus(y.space(), observed, mean);
        InnovationMatrix cov = makeCov();
        auto conditioned = kalmanCondition<BranchStatistics, BranchRepair>(
            prior,
            residual,
            cov,
            cross,
            form,
            termsFor(noise),
            considered
        );
        if (!conditioned)
        {
            return std::unexpected{conditioned.error()};
        }
        return Branch{
            Scalar(0),
            std::move(conditioned->posterior),
            {std::move(residual), std::move(cov), conditioned->statistics},
            0,
            component
        };
    };
 
    if constexpr (MixtureParams)
    {
        auto const & noise = resolved.noise().distribution();
        auto const total = noise.logTotalWeight();
        EMRYS_FILTER_ASSERT(total && "the noise mixture's weights must be valid");
        decltype(auto) result = mixtureUpdateFor<Space, MeasurementSpace, Scalar>(sink);
        using Outcome = MixtureOutcome<std::remove_cvref_t<Sink>, std::remove_cvref_t<decltype(result)>>;
        result.branches.reserve(std::size_t(noise.size()));
        auto evidence = LogSumExp<Scalar>{};
        Eigen::Index k = 0;
        // A component whose covariance is not positive definite fails the whole update [innovation-cholesky].
        for (auto const & entry : noise)
        {
            auto branch = condition(
                Emrys::plus(y.space(), y.mean(), entry.component.mean()),
                [&] {
                    return innovationCovariancePlus<InnovationMatrix>(y, entry.component.covarianceLower());
                },
                entry.component.covarianceLower(),
                k++
            );
            if (!branch)
            {
                return Outcome{std::unexpected{branch.error()}};
            }
            branch->logWeight = entry.logWeight - *total + branch->logLikelihood();
            evidence.add(branch->logWeight);
            result.branches.push_back(std::move(*branch));
        }
        auto const logEvidence = evidence.value();
        MixtureEvidenceAccess::setMixtureLogLikelihood(result, logEvidence);
        for (auto & branch : result.branches)
        {
            branch.logWeight -= logEvidence;
        }
        return mixtureOutcome<std::remove_cvref_t<Sink>>(result);
    }
    else
    {
        // The one branch conditioned in place: S is made in the Innovation that keeps it, and the
        // state conditioned on it there; a failed conditioning, or one the sink's acceptor refuses
        // on its NIS, leaves the state as it was (decisions.md [update-outcomes]). The update is
        // built where it is returned, since each move of the branch would copy the innovation's
        // fixed-size residual and S: transform() initializes its value from the prvalue, and the
        // one return lets the compiler construct it in the caller's object, which Clang does in a
        // generic lambda only when its return type is written out. into is the sink, an IntoPrior.
        using InPlaceUpdate = FilterUpdate<MeasurementSpace, 1, Scalar>;
        auto const inPlace = [&](auto & into,
                                 MeasurementVector const & mean,
                                 auto const & makeCov,
                                 InnovationMatrix const & noise) -> std::expected<InPlaceUpdate, GaussianError> {
            auto failure = std::optional<GaussianError>{};
            bool accepted = true;
            auto const terms = termsFor(noise);
            auto result = std::expected<void, GaussianError>{}.transform([&] {
                return InPlaceUpdate{
                    {FilterBranch<MeasurementSpace, Scalar>{
                        Scalar(0),
                        Innovation<MeasurementSpace, Scalar>{
                            InnovationInPlace{},
                            InnovationVector{Emrys::minus(y.space(), observed, mean)},
                            makeCov,
                            [&](auto const & residual, auto & cov) {
                                auto conditioned = kalmanConditionInPlace<Statistics, BranchRepair>(
                                    into.state,
                                    residual,
                                    cov,
                                    cross,
                                    form,
                                    terms,
                                    considered,
                                    into.accept
                                );
                                if (!conditioned)
                                {
                                    failure = conditioned.error();
                                    return std::optional<InnovationStatistics<Scalar>>{};
                                }
                                accepted = conditioned->accepted;
                                return conditioned->statistics;
                            }
                        },
                        0,
                        0
                    }},
                    Scalar(0),
                    true
                };
            });
            if (failure)
            {
                result = std::unexpected{*failure};
            }
            else
            {
                result->accepted = accepted;
            }
            return result;
        };
 
        // The one branch, or the error of its conditioning.
        auto const single = [&](MeasurementVector const & mean, auto const & makeCov, InnovationMatrix const & noise) {
            if constexpr (InPlace)
            {
                return inPlace(sink, mean, makeCov, noise);
            }
            else
            {
                using One = MeasurementUpdate<Space, MeasurementSpace, 1, Scalar>;
                auto branch = condition(mean, makeCov, noise, 0);
                if (!branch)
                {
                    return std::expected<One, GaussianError>{std::unexpected{branch.error()}};
                }
                return std::expected<One, GaussianError>{One{{std::move(*branch)}}};
            }
        };
        if constexpr (!R::HasAdditiveNoise)
        {
            return single(
                y.mean(),
                [&] {
                    return InnovationMatrix{y.covarianceLower()};
                },
                InnovationMatrix::Zero(y.differenceDimension(), y.differenceDimension())
            );
        }
        else
        {
            auto const & noise = resolved.noise().distribution();
            return single(
                Emrys::plus(y.space(), y.mean(), noise.mean()),
                [&] {
                    return innovationCovariancePlus<InnovationMatrix>(y, noise.covarianceLower());
                },
                noise.covarianceLower()
            );
        }
    }
}
 
}  // namespace internal

template <typename Space, typename P, typename Value>
struct GivenBlock
{
    Space space;          
    Value const & value;  
};
 
namespace internal
{

template <typename Space, typename P, typename M>
class PinnedMeasurement
{
public:

    using ResolvedType = ResolvedMeasurement<M, Space>;
 
private:
 
    using Reads = typename ResolvedType::Reads;                         
    static constexpr int N = valueDimensionAtCompileTime<Space>;        
    static constexpr int NMax = maxValueDimensionAtCompileTime<Space>;  
    static constexpr int D =
        valueDimensionAtCompileTime<DifferenceSpace<Space>>;  
    static constexpr int DMax = maxValueDimensionAtCompileTime<DifferenceSpace<Space>>;  

    static constexpr int PinnedD = valueDimensionAtCompileTime<DifferenceSpace<PathTag<P>>>;

    static constexpr int RestD = D == Eigen::Dynamic || PinnedD == Eigen::Dynamic ? Eigen::Dynamic : D - PinnedD;
 
    static constexpr int RestDMax = RestD == Eigen::Dynamic ? DMax : RestD;  

    static constexpr int PinnedN = valueDimensionAtCompileTime<PathTag<P>>;

    static constexpr int RestN = N == Eigen::Dynamic || PinnedN == Eigen::Dynamic ? Eigen::Dynamic : N - PinnedN;
 
    static constexpr int RestNMax = RestN == Eigen::Dynamic ? NMax : RestN;  
 
public:

    PinnedMeasurement(Space const & space, M const & measurement)
        : m_resolved{measurement, space}
        , m_rows{blockRows<P>(space)}
        , m_differenceRows{differenceBlockRows<P>(space)}
        , m_size{valueDimension(space)}
        , m_differenceSize{differenceDimension(space)}
    {
        m_rest.resize(m_size - m_rows.size);
        Eigen::Index k = 0;
        for (Eigen::Index i = 0; i < m_size; ++i)
        {
            if (i < m_rows.start || i >= m_rows.start + m_rows.size)
            {
                m_rest(k++) = i;
            }
        }
    }

    [[nodiscard]] ResolvedType const &
    resolved() const
    {
        return m_resolved;
    }

    [[nodiscard]] Eigen::Index
    restSize() const
    {
        return m_rest.size();
    }

    template <typename X, typename Pinned, typename... Inputs>
    [[nodiscard]] auto
    operator()(X const & x, Pinned const & p, Inputs const &... inputs) const
    {
        using T = ScalarOf<X>;
        EMRYS_FILTER_ASSERT(x.size() == m_rest.size() && p.size() == m_rows.size && "the blocks do not fit the space");
        auto state = Eigen::Matrix<T, N, 1, Eigen::ColMajor, NMax, 1>{};
        state.resize(m_size);
        state(m_rest) = x;
        middleRowsOf<PinnedN>(state, m_rows.start, m_rows.size) = p.template cast<T>();
        using Returned = std::remove_cvref_t<decltype(m_resolved.evaluateAtState(state, inputs...))>;
        if constexpr (!isWithJacobian<Returned>)
        {
            return m_resolved.evaluateAtState(state, inputs...);
        }
        else
        {
            auto returned = m_resolved.evaluateAtState(state, inputs...);
            return withJacobian(
                std::move(returned.value),
                splitJacobian(returned.jacobian, std::integral_constant<std::size_t, sizeof...(Inputs)>{})
            );
        }
    }

    static constexpr bool EvaluatesBatches = ResolvedType::BatchedFunction;

    template <typename Y, typename Xs, typename Pinned, typename... Inputs>
    void
    evaluateBatch(Y & y, Xs const & xs, Pinned const & p, Inputs const &... inputs) const
        requires EvaluatesBatches
    {
        using T = typename Xs::Scalar;
        using States = MatrixOf<T, N, Y::ColsAtCompileTime, NMax, Y::MaxColsAtCompileTime>;
        Eigen::Index const count = xs.cols();
        EMRYS_FILTER_ASSERT(
            xs.rows() == m_rest.size() && p.rows() == m_rows.size && (p.cols() == 1 || p.cols() == count)
            && "the blocks do not fit the space"
        );
        auto states = States{};
        states.resize(m_size, count);
        for (Eigen::Index j = 0; j < count; ++j)
        {
            auto state = states.col(j);
            state(m_rest) = xs.col(j);
            middleRowsOf<PinnedN>(state, m_rows.start, m_rows.size) = p.col(p.cols() == 1 ? 0 : j).template cast<T>();
        }
        m_resolved.evaluateStateColumns(y, states, inputs...);
    }
 
private:

    template <typename Blocks, std::size_t InputCount>
    [[nodiscard]] auto
    splitJacobian(Blocks const & blocks, std::integral_constant<std::size_t, InputCount>) const
    {
        auto const & whole = jacobianBlock<0>(blocks);
        using Whole = std::remove_cvref_t<decltype(whole)>;
        using Scalar = typename Whole::Scalar;
        constexpr int Rows = Whole::RowsAtCompileTime;
        constexpr int MaxRows = Whole::MaxRowsAtCompileTime;
        using RestJacobian = MatrixOf<Scalar, Rows, RestD, MaxRows, RestDMax>;
        using GivenJacobian =
            MatrixOf<Scalar, Rows, PinnedD, MaxRows, maxValueDimensionAtCompileTime<DifferenceSpace<PathTag<P>>>>;
        Eigen::Index const before = m_differenceRows.start;
        Eigen::Index const after = m_differenceSize - m_differenceRows.start - m_differenceRows.size;
        RestJacobian rest(whole.rows(), before + after);
        rest.leftCols(before) = whole.leftCols(before);
        rest.rightCols(after) = whole.rightCols(after);
        GivenJacobian given = middleColsOf<PinnedD>(whole, m_differenceRows.start, m_differenceRows.size);
        return [&]<std::size_t... K>(std::index_sequence<K...>) {
            using Tuple =
                std::tuple<RestJacobian, GivenJacobian, std::remove_cvref_t<decltype(jacobianBlock<1 + K>(blocks))>...>;
            return ArgumentJacobians<Tuple>{Tuple{std::move(rest), std::move(given), jacobianBlock<1 + K>(blocks)...}};
        }(std::make_index_sequence<InputCount>{});
    }
 
    ResolvedType m_resolved;              
    BlockRows m_rows;                     
    BlockRows m_differenceRows;           
    Eigen::Index m_size;                  
    Eigen::Index m_differenceSize;        
    IndexVector<RestN, RestNMax> m_rest;  
};
 
}  // namespace internal

template <typename P, typename Space, typename Value>
[[nodiscard]] GivenBlock<Space, P, Value>
given(Space const & space, Value const & value)
{
    return {space, value};
}

template <typename P, typename Space, typename Value>
void
given(Space const & space, Value const && value) = delete;
 
namespace internal
{

template <bool Statistics, UncertainInput Prior, typename Space, typename P, typename M, typename Value>
[[nodiscard]] auto
updateBranchesWith(Prior const & prior, PinnedMeasurement<Space, P, M> const & f, Value const & value)
{
    auto const & resolved = f.resolved();
    using R = std::remove_cvref_t<decltype(resolved)>;
    static_assert(
        R::HasMethod,
        "a Kalman update pushes the prior through the model by a method: observe(method, model, z)"
    );
    static_assert(!internal::isIterated<typename R::Method>, "an update given a block does not support Iterated yet");
    static_assert(
        !hasConsider<R>,
        "an update given a block, as a Rao-Blackwellized filter's, does not consider blocks yet (decisions.md "
        "[consider-states])"
    );
    EMRYS_FILTER_ASSERT(
        prior.valueDimension() == f.restSize() && "the prior must cover the state outside the given block"
    );
    auto const & method = resolved.method();
    auto const predicted = std::apply(
        [&](auto const &... inputs) {
            return internal::transformPrediction(resolved, method, f, withCross(prior), value, inputs...);
        },
        resolved.inputs()
    );
    // The measurement reads the whole prior, so its linearization is the prediction's own.
    constexpr int N = valueDimensionAtCompileTime<DifferenceSpace<typename Prior::SpaceType>>;
    constexpr int NMax = maxValueDimensionAtCompileTime<DifferenceSpace<typename Prior::SpaceType>>;
    auto const all = internal::consecutiveIndices<N, NMax>(0, prior.differenceDimension());
    using Predicted = std::remove_cvref_t<decltype(predicted)>;
    auto const & cross = predicted.crossCovariance();
    return internal::conditionOnPrediction<Statistics>(
        prior,
        resolved,
        predicted.output(),
        cross,
        internal::ReadLinearization{
            typename Predicted::JacobianMatrix{predicted.jacobian()},
            all,
            [&] {
                using Error = typename Predicted::OutputMatrix;
                return Error{predicted.inputsCovariance() + predicted.unexplainedCovariance()};
            }
        },
        NewPosteriors{}
    );
}

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

template <typename Space, typename P, typename Value>
inline constexpr bool isGivenBlock<GivenBlock<Space, P, Value>> = true;

template <typename F, typename Value>
struct GivenMeasurement
{
    F pinned;             
    Value const & value;  
};

template <typename G>
concept GivenMeasurementLike = requires(G const & given) {
    given.pinned.resolved();
    given.value;
};

template <bool Statistics, UncertainInput Prior, Measurement M, typename Space, typename P, typename Value>
[[nodiscard]] auto
updateBranches(Prior const & prior, M const & measurement, GivenBlock<Space, P, Value> const & pinned)
{
    auto const f = PinnedMeasurement<Space, P, M>{pinned.space, measurement};
    return updateBranchesWith<Statistics>(prior, f, pinned.value);
}
 
}  // namespace internal

template <UncertainInput Prior, Measurement M, typename Space, typename P, typename Value>
[[nodiscard]] auto
update(Prior const & prior, M const & measurement, GivenBlock<Space, P, Value> const & pinned)
{
    return internal::updateBranches<false>(internal::withCovariance(prior), measurement, pinned);
}
 
}  // namespace Emrys