RecursiveLeastSquaresTransform.hpp

Go to the documentation of this file.

/* This file is part of OpenKalman, a header-only C++ library for
 * Kalman filters and other recursive filters.
 *
 * Copyright (c) 2017-2021 Christopher Lee Ogden <ogden@gatech.edu>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at https://mozilla.org/MPL/2.0/.
 */

#ifndef OPENKALMAN_RLSTRANSFORM_HPP
#define OPENKALMAN_RLSTRANSFORM_HPP

namespace OpenKalman
{
  namespace oin = OpenKalman::internal;

  template<typename Scalar = double>
  struct RecursiveLeastSquaresTransform : oin::TransformBase<RecursiveLeastSquaresTransform<Scalar>>
  {
  private:

    using Base = oin::TransformBase<RecursiveLeastSquaresTransform<Scalar>>;

  public:

    explicit RecursiveLeastSquaresTransform(const Scalar lambda = 0.9995) : inv_lambda {1/lambda} {}


    using Base::operator();


#ifdef __cpp_concepts
    template<distribution InputDist, distribution ... NoiseDists> requires
      (coordinates::compares_with<typename DistributionTraits<InputDist>::StaticDescriptor,
        typename DistributionTraits<NoiseDists>::StaticDescriptor>and ...)
#else
    template<typename InputDist, typename ... NoiseDists,
      std::enable_if_t<(distribution<InputDist> and ... and distribution<NoiseDists>) and
        (coordinates::compares_with<typename DistributionTraits<InputDist>::StaticDescriptor,
        typename DistributionTraits<NoiseDists>::StaticDescriptor>and ...), int> = 0>
#endif
    auto operator()(const InputDist& x, const NoiseDists& ...ns) const
    {
      const auto scaled_x = GaussianDistribution {mean_of(x), covariance_of(x) * inv_lambda};
      return make_self_contained((scaled_x + ... + ns));
    }


    using Base::transform_with_cross_covariance;


#ifdef __cpp_concepts
    template<distribution InputDist, distribution ... NoiseDists> requires
      (coordinates::compares_with<typename DistributionTraits<InputDist>::StaticDescriptor,
        typename DistributionTraits<NoiseDists>::StaticDescriptor> and ...)
#else
    template<typename InputDist, typename ... NoiseDists,
      std::enable_if_t<(distribution<InputDist> and ... and distribution<NoiseDists>) and
        (coordinates::compares_with<typename DistributionTraits<InputDist>::StaticDescriptor,
        typename DistributionTraits<NoiseDists>::StaticDescriptor> and ...), int> = 0>
#endif
    auto transform_with_cross_covariance(const InputDist& x, const NoiseDists& ...ns) const
    {
      auto scaled_cov = make_self_contained(covariance_of(x) * inv_lambda);
      const auto scaled_x = GaussianDistribution {mean_of(x), scaled_cov};
      auto y = make_self_contained((scaled_x + ... + ns));
      auto cross = Matrix {scaled_cov};
      return std::tuple {std::move(y), std::move(cross)};
    }

  private:

    const Scalar inv_lambda;

  };

}


#endif //OPENKALMAN_RLSTRANSFORM_HPP