CramerRao.h

Go to the documentation of this file.

#pragma once

 
#include <expected>
#include <type_traits>
#include <utility>
 
#include <Eigen/Cholesky>
 
#include <EmrysFilter/Core/Execution.h>
#include <EmrysFilter/Core/Forwarding.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
#include <EmrysFilter/Core/Multiply.h>
#include <EmrysFilter/Core/Solve.h>
#include <EmrysFilter/Core/Time.h>
#include <EmrysFilter/Distributions/Gaussian.h>
#include <EmrysFilter/Distributions/Moments.h>
#include <EmrysFilter/Models/FormSteps.h>
#include <EmrysFilter/Models/Measurement.h>
#include <EmrysFilter/Models/ProcessModel.h>
#include <EmrysFilter/Models/Transform.h>
#include <EmrysFilter/Spaces/Indexing.h>
#include <EmrysFilter/Spaces/Space.h>
 
namespace Emrys
{
 
namespace internal
{

template <typename Matrix>
struct InfoSums
{
    Matrix first;   
    Matrix second;  
    bool valid;     
};

template <typename Matrix>
[[nodiscard]] InfoSums<Matrix>
addInfoSums(InfoSums<Matrix> a, InfoSums<Matrix> const & b)
{
    a.first += b.first;
    a.second += b.second;
    a.valid = a.valid && b.valid;
    return a;
}
 
}  // namespace internal

template <Space Space_, RealScalar Scalar_ = double>
class PosteriorCramerRaoBound
{
public:
 
    using Scalar = Scalar_;                                            
    using SpaceType = Space_;                                          
    using VectorType = typename Moments<Space_, Scalar_>::VectorType;  
    using MatrixType = typename Moments<Space_, Scalar_>::MatrixType;  

    template <MatrixExpression Info>
    PosteriorCramerRaoBound(Space_ space, Info && info)
        : m_space{std::move(space)}
        , m_info{std::forward<Info>(info)}
    {
        EMRYS_FILTER_ASSERT(
            m_info.rows() == differenceDimension(m_space) && m_info.cols() == m_info.rows()
            && "the information matrix must have a row and a column per difference coordinate"
        );
    }

    [[nodiscard]] static std::expected<PosteriorCramerRaoBound, GaussianError>
    create(GaussianParams<Space_, Scalar_> const & prior)
    {
        auto const llt = Eigen::LLT<MatrixType>{internal::symmetricFromLower<MatrixType>(prior.covarianceLower())};
        if (llt.info() != Eigen::Success)
        {
            return std::unexpected{GaussianError::NotPositiveDefinite};
        }
        MatrixType info = internal::inverseFrom<MatrixType>(llt);
        return PosteriorCramerRaoBound{prior.space(), symmetric(std::move(info))};
    }

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

    [[nodiscard]] MatrixType const &
    info() const
    {
        return m_info;
    }

    [[nodiscard]] std::expected<MatrixType, GaussianError>
    covariance() const
    {
        auto const llt = Eigen::LLT<MatrixType>{m_info};
        if (llt.info() != Eigen::Success)
        {
            return std::unexpected{GaussianError::Singular};
        }
        MatrixType result = internal::inverseFrom<MatrixType>(llt);
        return symmetric(std::move(result));
    }

    template <ProcessModel Model, typename Truths, typename... Inputs>
        requires(!ExecutionPolicy<Model>)
    [[nodiscard]] std::expected<void, GaussianError>
    predict(
        Model const & model,
        Eigen::MatrixBase<Truths> const & truths,
        TimeStep<Scalar_> auto const & dt,
        Inputs const &... inputs
    )
    {
        return predict(execution::sequential, model, truths, dt, inputs...);
    }

    template <ExecutionPolicy P, ProcessModel Model, typename Truths, typename... Inputs>
    [[nodiscard]] std::expected<void, GaussianError>
    predict(
        P && policy,
        Model const & model,
        Eigen::MatrixBase<Truths> const & truths,
        TimeStep<Scalar_> auto const & dt,
        Inputs const &... inputs
    )
    {
        static_assert(
            (!UncertainInput<Inputs> && ...),
            "the bound takes a model's true inputs, as simulate() does; an uncertain input adds noise that the "
            "transition density of the bound does not model (decisions.md [cramer-rao-bound])"
        );
        internal::assertPartitions(model, m_space);
        Scalar_ const step = internal::toScalarTime<Scalar_>(dt);
        auto const call = internal::modelWith(std::cref(model), inputs...);
        auto const resolved = internal::resolveProcess(call, m_space);
        using R = std::remove_cvref_t<decltype(resolved)>;
        auto const noise = internal::symmetricFromLower<MatrixType>(resolved.template noise<Scalar_>(step));
        if constexpr (R::Affine && R::ReturnsJacobian && R::InputCount == 0)
        {
            MatrixType const transition = resolved.template transition<Scalar_>(step);
            return predictFrom(noise, [&](auto const & solveNoise) {
                internal::InfoSums<MatrixType> sums{transition, MatrixType{}, true};
                MatrixType const whitened = solveNoise(transition);
                sums.second.noalias() = internal::multiply(whitened.transpose(), whitened);
                return sums;
            });
        }
        else
        {
            assertTruths(truths);
            auto const f = [&](auto const & x) {
                return resolved.propagate(x, step);
            };
            using Input = internal::PointInput<Space_, Scalar_>;
            using Problem = internal::Transform<decltype(f) const, Space_, Input>;
            Eigen::Index const count = truths.cols();
            return predictFrom(noise, [&](auto const & solveNoise) {
                return averageOver(
                    std::forward<P>(policy),
                    count,
                    [&](Eigen::Index i, internal::InfoSums<MatrixType> & sums) {
                        // The problem holds its inputs by reference, so the point is named first.
                        auto const point = Input{m_space, VectorType{truths.col(i)}};
                        auto const problem = Problem{f, m_space, point};
                        auto const linearized = internal::linearizeAt(methods::linearized(), problem);
                        MatrixType const jacobian = linearized.jacobian.leftCols(differenceDimension(m_space));
                        MatrixType const whitened = solveNoise(jacobian);
                        sums.first += jacobian;
                        sums.second.noalias() += internal::multiply(whitened.transpose(), whitened);
                    }
                );
            });
        }
    }

    template <typename Model, typename Truths, typename... Inputs>
        requires(
            !ExecutionPolicy<Model> && (isFixedStep<Model> || isUnboundStep<Model>)
            && requires(Model const & m) { m.model(); }
        )
    [[nodiscard]] std::expected<void, GaussianError>
    predict(Model const & model, Eigen::MatrixBase<Truths> const & truths, Inputs const &... inputs)
    {
        return predict(execution::sequential, model.model(), truths, model.timeStep(), inputs...);
    }

    template <ExecutionPolicy P, typename Model, typename Truths, typename... Inputs>
        requires((isFixedStep<Model> || isUnboundStep<Model>) && requires(Model const & m) { m.model(); })
    [[nodiscard]] std::expected<void, GaussianError>
    predict(P && policy, Model const & model, Eigen::MatrixBase<Truths> const & truths, Inputs const &... inputs)
    {
        return predict(std::forward<P>(policy), model.model(), truths, model.timeStep(), inputs...);
    }

    template <typename Truths, MeasurementModel M, typename... Args>
    [[nodiscard]] std::expected<void, GaussianError>
    update(Eigen::MatrixBase<Truths> const & truths, M const & model, Args const &... args)
    {
        return update(execution::sequential, truths, model, args...);
    }

    template <ExecutionPolicy P, typename Truths, MeasurementModel M, typename... Args>
    [[nodiscard]] std::expected<void, GaussianError>
    update(P && policy, Eigen::MatrixBase<Truths> const & truths, M const & model, Args const &... args)
    {
        static_assert(
            (!UncertainInput<Args> && ...),
            "the bound takes a model's true inputs, as simulate() does; an uncertain input adds noise that the "
            "measurement density of the bound does not model (decisions.md [cramer-rao-bound])"
        );
        assertTruths(truths);
        auto const observed = internal::modelWith(std::cref(model), args...);
        auto const resolved = internal::resolveMeasurement(observed, m_space);
        using R = std::remove_cvref_t<decltype(resolved)>;
        static_assert(
            R::HasAdditiveNoise && !R::MixtureNoise,
            "the bound takes a measurement model with additive Gaussian noise, whose likelihood is the Gaussian of "
            "Tichavsky et al. (1998), eq. (33) (decisions.md [cramer-rao-bound])"
        );
        auto const sums = averageOver(
            std::forward<P>(policy),
            truths.cols(),
            [&](Eigen::Index i, internal::InfoSums<MatrixType> & sums) {
                auto const linearization = internal::linearizeMeasurementAtPoint<Scalar_>(
                    m_space,
                    VectorType{truths.col(i)},
                    resolved,
                    methods::linearized()
                );
                using NoiseMatrix = typename std::remove_cvref_t<decltype(linearization)>::NoiseMatrix;
                auto const llt = Eigen::LLT<NoiseMatrix>{linearization.noise};
                if (llt.info() != Eigen::Success)
                {
                    sums.valid = false;
                    return;
                }
                using JacobianMatrix = typename std::remove_cvref_t<decltype(linearization)>::JacobianMatrix;
                JacobianMatrix whitened = linearization.jacobian;
                internal::solveInPlace(llt.matrixL(), whitened);
                sums.second.noalias() += internal::multiply(whitened.transpose(), whitened);
            }
        );
        if (!sums.valid)
        {
            return std::unexpected{GaussianError::NotPositiveDefinite};
        }
        m_info += sums.second;
        m_info = symmetric(std::move(m_info));
        return {};
    }
 
private:

    [[nodiscard]] static MatrixType
    symmetric(MatrixType matrix)
    {
        MatrixType result = Scalar_(0.5) * (matrix + matrix.transpose());
        return result;
    }

    template <typename Truths>
    void
    assertTruths(Eigen::MatrixBase<Truths> const & truths) const
    {
        EMRYS_FILTER_ASSERT(truths.cols() > 0 && "the bound takes at least one true state");
        EMRYS_FILTER_ASSERT(truths.rows() == valueDimension(m_space) && "a true state does not fit the space");
    }

    template <ExecutionPolicy P, typename AddTerms>
    [[nodiscard]] internal::InfoSums<MatrixType>
    averageOver(P && policy, Eigen::Index count, AddTerms const & addTerms) const
    {
        Eigen::Index const n = differenceDimension(m_space);
        auto sums = internal::reduceRanges(
            std::forward<P>(policy),
            count,
            [&](Eigen::Index begin, Eigen::Index length) {
                auto range = internal::InfoSums<MatrixType>{MatrixType::Zero(n, n), MatrixType::Zero(n, n), true};
                for (Eigen::Index i = begin; i < begin + length; ++i)
                {
                    addTerms(i, range);
                }
                return range;
            },
            internal::addInfoSums<MatrixType>
        );
        sums.first /= Scalar_(count);
        sums.second /= Scalar_(count);
        return sums;
    }

    template <typename Average>
    [[nodiscard]] std::expected<void, GaussianError>
    predictFrom(MatrixType const & noise, Average const & average)
    {
        auto const noiseLlt = Eigen::LLT<MatrixType>{noise};
        if (noiseLlt.info() != Eigen::Success)
        {
            return std::unexpected{GaussianError::NotPositiveDefinite};
        }
        auto const solveNoise = [&](MatrixType const & a) {
            MatrixType result = a;
            internal::solveInPlace(noiseLlt.matrixL(), result);
            return result;
        };
        internal::InfoSums<MatrixType> const sums = average(solveNoise);
        auto const combinedLlt = Eigen::LLT<MatrixType>{MatrixType{m_info + sums.second}};
        if (combinedLlt.info() != Eigen::Success)
        {
            return std::unexpected{GaussianError::Singular};
        }
        MatrixType const noiseInverse = internal::inverseFrom<MatrixType>(noiseLlt);
        MatrixType gain = sums.first;
        internal::solveInPlace(noiseLlt, gain);
        MatrixType whitened = gain.transpose();
        internal::solveInPlace(combinedLlt.matrixL(), whitened);
        MatrixType info = noiseInverse;
        info.noalias() -= internal::multiply(whitened.transpose(), whitened);
        m_info = symmetric(std::move(info));
        return {};
    }
 
    EMRYS_FILTER_NO_UNIQUE_ADDRESS Space_ m_space;  
    MatrixType m_info;                              
};
 
}  // namespace Emrys