GaussianHistory.h

Go to the documentation of this file.

#pragma once

 
#include <list>
#include <optional>
#include <utility>
 
#include <Eigen/Cholesky>
 
#include <EmrysFilter/Core/Allocation.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
#include <EmrysFilter/Core/Multiply.h>
#include <EmrysFilter/Core/Solve.h>
#include <EmrysFilter/Distributions/Gaussian.h>
#include <EmrysFilter/Models/ProcessModel.h>
#include <EmrysFilter/Smoothers/Lag.h>
#include <EmrysFilter/Spaces/Reset.h>
#include <EmrysFilter/Spaces/Space.h>
 
namespace Emrys
{

template <typename Space_, typename Scalar_>
[[nodiscard]] GaussianParams<Space_, Scalar_>
smoothStep(
    PredictedJoint<Space_, Scalar_> const & joint,
    typename GaussianParams<Space_, Scalar_>::MatrixType const & gain,
    GaussianParams<Space_, Scalar_> const & smoothedNext
)
{
    using GaussianType = GaussianParams<Space_, Scalar_>;
    using MatrixType = typename GaussianType::MatrixType;
    auto const & space = joint.filtered.space();
    EMRYS_FILTER_ASSERT(
        joint.cross.rows() == joint.filtered.differenceDimension()
        && joint.cross.cols() == joint.predicted.differenceDimension()
        && "the joint's cross-covariance has the wrong shape"
    );
    EMRYS_FILTER_ASSERT(
        gain.rows() == joint.filtered.differenceDimension() && gain.cols() == joint.predicted.differenceDimension()
        && "the gain has the wrong shape"
    );
    EMRYS_FILTER_ASSERT(
        smoothedNext.differenceDimension() == joint.predicted.differenceDimension()
        && "the smoothed Gaussian must be over the space the joint predicted"
    );
    MatrixType const predictedCov = joint.predicted.covarianceLower().template selfadjointView<Eigen::Lower>();
    MatrixType nextCov = smoothedNext.covarianceLower().template selfadjointView<Eigen::Lower>();
    typename GaussianType::DifferenceVectorType const correction =
        Emrys::minus(joint.predicted.space(), smoothedNext.mean(), joint.predicted.mean());
    internal::resetCovarianceAt<true>(
        joint.predicted.space(),
        joint.predicted.mean(),
        correction,
        smoothedNext.mean(),
        nextCov
    );
    typename GaussianType::DifferenceVectorType const step = internal::multiply(gain, correction);
    typename GaussianType::VectorType mean = Emrys::plus(space, joint.filtered.mean(), step);
    MatrixType cov = joint.filtered.covarianceLower().template selfadjointView<Eigen::Lower>();
    cov.noalias() += internal::multiply(internal::multiply(gain, nextCov - predictedCov), gain.transpose());
    cov = (Scalar_(0.5) * (cov + cov.transpose())).eval();
    internal::resetCovarianceAt(space, joint.filtered.mean(), step, mean, cov);
    return {space, std::move(mean), std::move(cov)};
}
 
namespace internal
{

template <typename Space_, typename Scalar_>
[[nodiscard]] typename GaussianParams<Space_, Scalar_>::MatrixType
rtsGain(PredictedJoint<Space_, Scalar_> const & joint)
{
    using MatrixType = typename GaussianParams<Space_, Scalar_>::MatrixType;
    EMRYS_FILTER_ASSERT(
        joint.cross.rows() == joint.filtered.differenceDimension()
        && joint.cross.cols() == joint.predicted.differenceDimension()
        && "the joint's cross-covariance has the wrong shape"
    );
    MatrixType const predictedCov = joint.predicted.covarianceLower().template selfadjointView<Eigen::Lower>();
    auto const ldlt = Eigen::LDLT<MatrixType>{predictedCov};
    return internal::transposedSolve<MatrixType>(ldlt, joint.cross.transpose());
}
 
}  // namespace internal

template <typename Space_, typename Scalar_>
[[nodiscard]] GaussianParams<Space_, Scalar_>
smoothStep(PredictedJoint<Space_, Scalar_> const & joint, GaussianParams<Space_, Scalar_> const & smoothedNext)
{
    return smoothStep(joint, internal::rtsGain(joint), smoothedNext);
}

template <Space Space_, RealScalar Scalar_ = double, typename Allocator_ = DefaultAllocator>
class GaussianHistory
{
public:
 
    using Scalar = Scalar_;                                
    using SpaceType = Space_;                              
    using GaussianType = GaussianParams<Space_, Scalar_>;  
    using JointType = PredictedJoint<Space_, Scalar_>;     
    using allocator_type = Allocator_;                     
    using Index = Eigen::Index;                            

    struct Step
    {
        JointType joint;  

        GaussianType smoothed;

        typename GaussianType::MatrixType gain;
    };
 
private:
 
    using StepAllocator = internal::ReBound<Allocator_, Step>;  
    using ListType = std::list<Step, StepAllocator>;            
 
public:
 
    using const_iterator = typename ListType::const_iterator;  

    GaussianHistory() = default;

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

    explicit GaussianHistory(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");
    }

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

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

    GaussianHistory(GaussianHistory && other) = default;

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

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

    GaussianHistory &
    operator=(GaussianHistory && other) = default;
 
    ~GaussianHistory() = 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 joint)
    {
        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 = std::move(joint);
            step.smoothed = step.joint.filtered;
            step.gain = internal::rtsGain(step.joint);
        }
        else
        {
            GaussianType smoothed = joint.filtered;
            typename GaussianType::MatrixType gain = internal::rtsGain(joint);
            m_steps.push_back(Step{std::move(joint), std::move(smoothed), std::move(gain)});
        }
    }

    void
    smooth(GaussianType const & latest)
    {
        GaussianType const * next{&latest};
        for (auto step = m_steps.rbegin(); step != m_steps.rend(); ++step)
        {
            step->smoothed = smoothStep(step->joint, step->gain, *next);
            next = &step->smoothed;
        }
    }

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