HybridGaussianHistory.h

Go to the documentation of this file.

#pragma once

 
#include <cstddef>
#include <list>
#include <optional>
#include <utility>
 
#include <EmrysFilter/Core/Allocation.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
#include <EmrysFilter/Core/Weights.h>
#include <EmrysFilter/Distributions/Gaussian.h>
#include <EmrysFilter/Distributions/Hybrid.h>
#include <EmrysFilter/Filters/ImmFilter.h>
#include <EmrysFilter/Smoothers/GaussianHistory.h>
#include <EmrysFilter/Smoothers/Lag.h>
#include <EmrysFilter/Spaces/Discrete.h>
#include <EmrysFilter/Spaces/Space.h>
 
namespace Emrys
{
 
namespace internal
{

template <typename Joint, typename Gains, typename Next, typename Smoothed>
void
smoothHybridStep(Joint const & joint, Gains const & gains, Next const & next, Smoothed & smoothed)
{
    using Scalar = typename Joint::Scalar;
    using ModesType = typename Joint::ModesType;
    using ArrayType = typename ModesType::ArrayType;
    using GaussianType = GaussianParams<typename Joint::SpaceType, Scalar>;
    Scalar const minusInfinity = -Eigen::NumTraits<Scalar>::infinity();
    Eigen::Index const count = joint.modes.cardinality();
    EMRYS_FILTER_ASSERT(
        Eigen::Index(joint.joints.size()) == count && Eigen::Index(gains.size()) == count && next.size() == count
        && smoothed.size() == count
        && "the joint, its gains, the next state, and the smoothed state must have one Gaussian per mode"
    );
    auto const & pi = joint.transitions;
    auto const logFiltered = logProbabilitiesOf(joint.modes);
    auto const logNext = logProbabilitiesOf(next.modes());
    auto const logTransition = [&](Eigen::Index from, Eigen::Index to) {
        return pi(from, to) > Scalar(0) ? Scalar(Eigen::numext::log(pi(from, to))) : minusInfinity;
    };
 
    // e_i, the predicted probability of mode i at t + 1, normalizes the backward transition
    // b_ij = p_ji mu_t^j / e_i (8.11).
    auto logPredicted = ArrayType{};
    logPredicted.resize(count);
    for (Eigen::Index i = 0; i < count; ++i)
    {
        auto total = LogSumExp<Scalar>{};
        for (Eigen::Index l = 0; l < count; ++l)
        {
            total.add(logTransition(l, i) + logFiltered(l));
        }
        logPredicted(i) = total.value();
    }
 
    auto logMixing = ArrayType{};
    logMixing.resize(count);
    auto logModes = ArrayType{};
    logModes.resize(count);
    for (Eigen::Index j = 0; j < count; ++j)
    {
        // The backward mixing weights mu^{i|j} of (8.12), unnormalized: b_ij mu_{t+1|N}^i.
        for (Eigen::Index i = 0; i < count; ++i)
        {
            logMixing(i) = logPredicted(i) == minusInfinity
                             ? minusInfinity
                             : logTransition(j, i) + logFiltered(j) - logPredicted(i) + logNext(i);
        }
        auto const & modeJoint = joint.joints[std::size_t(j)];
        if (logTotalWeight<Scalar>(logMixing))
        {
            // Mixing (8.13, 8.14) and the mode-matched RTS step (8.15 to 8.17).
            GaussianType const mixed = ComponentTraits<GaussianType>::collapse(
                count,
                [&](Eigen::Index i) -> GaussianType const & {
                    return next.conditional(i);
                },
                [&](Eigen::Index i) {
                    return logMixing(i);
                }
            );
            smoothed.setConditional(j, smoothStep(modeJoint, gains[std::size_t(j)], mixed));
        }
        else
        {
            // Nothing smoothed at t + 1 comes from mode j, so it keeps its filtered Gaussian.
            smoothed.setConditional(j, modeJoint.filtered);
        }
    }
 
    // The mode probabilities by the expectation correction of Barber (2006), (12) and (13) under
    // its mean approximation: p(r_t = j | r_{t+1} = i, z_{1:N}) is b_ij reweighted by the density
    // of mode i's smoothed mean at t + 1 under mode j's filtered Gaussian moved through mode i's
    // model, normalized over j, and mu_{t|N}^j is its sum over i weighted by mu_{t+1|N}^i. Where no
    // mode's prediction gives that mean a density, b_ij is left as it is.
    auto const logAdd = [](Scalar a, Scalar b) {
        auto sum = LogSumExp<Scalar>{};
        sum.add(a);
        sum.add(b);
        return sum.value();
    };
    logModes.setConstant(minusInfinity);
    auto logPair = ArrayType{};
    logPair.resize(count);
    auto logBackward = ArrayType{};
    logBackward.resize(count);
    for (Eigen::Index i = 0; i < count; ++i)
    {
        if (logNext(i) == minusInfinity || logPredicted(i) == minusInfinity)
        {
            continue;
        }
        auto const & smoothedMean = next.conditional(i).mean();
        for (Eigen::Index j = 0; j < count; ++j)
        {
            logBackward(j) = logTransition(j, i) + logFiltered(j) - logPredicted(i);
            logPair(j) = minusInfinity;
            if (logBackward(j) > minusInfinity)
            {
                if (auto const density = joint.predictedThrough(j, i).toDensity())
                {
                    logPair(j) = logBackward(j) + density->logDensity(smoothedMean);
                }
            }
        }
        auto const logNormalizer = logTotalWeight<Scalar>(logPair);
        for (Eigen::Index j = 0; j < count; ++j)
        {
            Scalar const logGiven = logNormalizer ? logPair(j) - *logNormalizer : logBackward(j);
            logModes(j) = logAdd(logModes(j), logNext(i) + logGiven);
        }
    }
    auto modes = ModesType{joint.modes.space(), logModes};
    [[maybe_unused]] auto const normalized = modes.normalize();
    EMRYS_FILTER_ASSERT(normalized && "no mode smoothed at the step's end came from a mode at its start");
    smoothed.setModes(std::move(modes));
}
 
}  // namespace internal

template <Space Space_, DiscreteSpace ModeSpace_, RealScalar Scalar_ = double, typename Allocator_ = DefaultAllocator>
class HybridGaussianHistory
{
public:
 
    using Scalar = Scalar_;                                                              
    using SpaceType = Space_;                                                            
    using ModeSpaceType = ModeSpace_;                                                    
    using JointType = HybridPredictedJoint<Space_, ModeSpace_, Scalar_, Allocator_>;     
    using SmoothedType = HybridGaussianParams<Space_, ModeSpace_, Scalar_, Allocator_>;  
    using allocator_type = Allocator_;  
    using Index = Eigen::Index;         

    class Step
    {
    public:
 
        using allocator_type = Allocator_;  

        using GainType = typename GaussianParams<Space_, Scalar_>::MatrixType;

        using GainAllocator = internal::ReBound<Allocator_, GainType>;

        explicit Step(JointType const & recorded, Allocator_ const & allocator = {})
            : joint{copyJoint(recorded, allocator)}
            , smoothed{recorded.modes, recorded.joints.front().filtered, allocator}
            , gains(GainAllocator(allocator))
        {
            resetSmoothed();
            computeGains();
        }

        Step(Step const & other, Allocator_ const & allocator)
            : joint{copyJoint(other.joint, allocator)}
            , smoothed{other.smoothed, allocator}
            , gains(other.gains, GainAllocator(allocator))
        {}

        Step(Step && other, Allocator_ const & allocator)
            : joint{
                  std::move(other.joint.modes),
                  typename JointType::JointsType(std::move(other.joint.joints), allocator),
                  std::move(other.joint.transitions),
                  typename JointType::PredictionsType(std::move(other.joint.throughOtherModels), allocator)
              }
            , smoothed{std::move(other.smoothed), allocator}
            , gains(std::move(other.gains), GainAllocator(allocator))
        {}
 
        Step(Step const &) = default;  
        Step(Step &&) = default;       

        Step &
        operator=(Step const &) = default;

        Step &
        operator=(Step &&) = default;
 
        ~Step() = default;

        void
        resetSmoothed()
        {
            smoothed.setModes(joint.modes);
            for (Index j = 0; j < smoothed.size(); ++j)
            {
                smoothed.setConditional(j, joint.joints[std::size_t(j)].filtered);
            }
        }

        void
        computeGains()
        {
            gains.resize(joint.joints.size());
            for (std::size_t j = 0; j < gains.size(); ++j)
            {
                gains[j] = internal::rtsGain(joint.joints[j]);
            }
        }
 
        JointType joint;  

        SmoothedType smoothed;

        internal::Buffer<GainType, Allocator_> gains;
 
    private:

        [[nodiscard]] static JointType
        copyJoint(JointType const & recorded, Allocator_ const & allocator)
        {
            return {
                recorded.modes,
                typename JointType::JointsType(recorded.joints, allocator),
                recorded.transitions,
                typename JointType::PredictionsType(recorded.throughOtherModels, allocator)
            };
        }
    };
 
private:
 
    using StepAllocator = internal::ReBound<Allocator_, Step>;  
    using ListType = std::list<Step, StepAllocator>;            
 
public:
 
    using const_iterator = typename ListType::const_iterator;  

    HybridGaussianHistory() = default;

    explicit HybridGaussianHistory(Allocator_ const & allocator)
        : m_steps(StepAllocator(allocator))
    {}

    explicit HybridGaussianHistory(Lag lag, Allocator_ const & allocator = {})
        : m_steps(StepAllocator(allocator))
        , m_lag{lag.steps}
    {
        EMRYS_FILTER_ASSERT(lag.steps > 0 && "a fixed-lag history keeps at least one step");
    }

    HybridGaussianHistory(HybridGaussianHistory const & other)
        : HybridGaussianHistory{other, other.get_allocator()}
    {}

    HybridGaussianHistory(HybridGaussianHistory const & other, Allocator_ const & allocator)
        : m_steps(other.m_steps, StepAllocator(allocator))
        , m_lag{other.m_lag}
    {}

    HybridGaussianHistory(HybridGaussianHistory && other) = default;

    HybridGaussianHistory(HybridGaussianHistory && other, Allocator_ const & allocator)
        : m_steps(std::move(other.m_steps), StepAllocator(allocator))
        , m_lag{other.m_lag}
    {}

    HybridGaussianHistory &
    operator=(HybridGaussianHistory const & other) = default;

    HybridGaussianHistory &
    operator=(HybridGaussianHistory && other) = default;
 
    ~HybridGaussianHistory() = default;

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

    [[nodiscard]] std::optional<Index>
    lag() const
    {
        return m_lag > 0 ? std::optional<Index>{m_lag} : std::nullopt;
    }

    [[nodiscard]] Index
    size() const
    {
        return Index(m_steps.size());
    }

    [[nodiscard]] bool
    empty() const
    {
        return m_steps.empty();
    }

    [[nodiscard]] Step const &
    front() const
    {
        EMRYS_FILTER_ASSERT(!empty() && "the history is empty");
        return m_steps.front();
    }

    [[nodiscard]] Step const &
    back() const
    {
        EMRYS_FILTER_ASSERT(!empty() && "the history is empty");
        return m_steps.back();
    }

    [[nodiscard]] const_iterator
    begin() const
    {
        return m_steps.begin();
    }

    [[nodiscard]] const_iterator
    end() const
    {
        return m_steps.end();
    }

    void
    push(JointType const & joint)
    {
        EMRYS_FILTER_ASSERT(!joint.joints.empty() && "the joint has no modes");
        if (m_lag > 0 && size() == m_lag)
        {
            m_steps.splice(m_steps.end(), m_steps, m_steps.begin());
            Step & step = m_steps.back();
            step.joint.modes = joint.modes;
            step.joint.joints = joint.joints;
            step.joint.transitions = joint.transitions;
            step.joint.throughOtherModels = joint.throughOtherModels;
            step.resetSmoothed();
            step.computeGains();
        }
        else
        {
            m_steps.emplace_back(joint);
        }
    }

    template <typename LatestAllocator>
    void
    smooth(HybridGaussianParams<Space_, ModeSpace_, Scalar_, LatestAllocator> const & latest)
    {
        auto step = m_steps.rbegin();
        if (step == m_steps.rend())
        {
            return;
        }
        internal::smoothHybridStep(step->joint, step->gains, latest, step->smoothed);
        for (auto next = step++; step != m_steps.rend(); next = step++)
        {
            internal::smoothHybridStep(step->joint, step->gains, next->smoothed, step->smoothed);
        }
    }

    void
    clear()
    {
        m_steps.clear();
    }
 
private:
 
    ListType m_steps;  
    Index m_lag = 0;   
};
 
}  // namespace Emrys