Unscented.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_UNSCENTED_HPP
#define OPENKALMAN_UNSCENTED_HPP

namespace OpenKalman
{
  namespace oin = OpenKalman::internal;


  template<typename Parameters>
  struct Unscented;


  struct UnscentedParametersStateEstimation
  {
    static constexpr double alpha = 0.001;
    static constexpr double beta = 2.0;
    template<std::size_t dim>
    static constexpr double kappa = 0.0;
  };


  struct UnscentedParametersParameterEstimation
  {
    static constexpr double alpha = 0.001;
    static constexpr double beta = 2.0;
    template<std::size_t dim>
    static constexpr double kappa = 3. - double(dim);
  };


  using UnscentedSigmaPointsStateEstimation = Unscented<UnscentedParametersStateEstimation>;

  using UnscentedSigmaPointsParameterEstimation = Unscented<UnscentedParametersParameterEstimation>;

  using UnscentedSigmaPoints = Unscented<UnscentedParametersStateEstimation>;


#if __cpp_nontype_template_args >= 201911L
  template<typename Parameters = UnscentedParametersStateEstimation> // \@todo add as floating-point parameters: alpha, beta, kappa
#else
  template<typename Parameters = UnscentedParametersStateEstimation>
#endif
  struct Unscented : oin::ScaledSigmaPointsBase<Unscented<Parameters>>
  {

    template<std::size_t dim>
    static constexpr std::size_t sigma_point_count = dim * 2 + 1;


    static constexpr auto alpha = Parameters::alpha;


    static constexpr auto beta = Parameters::beta;


    template<std::size_t dim>
    static constexpr auto unscaled_W0()
    {
      return Parameters::template kappa<dim> / (dim + Parameters::template kappa<dim>);
    }


    template<std::size_t dim>
    static constexpr auto unscaled_W()
    {
      return 0.5 / (dim + Parameters::template kappa<dim>);
    }

  private:

    Unscented() {};


    /*
     * Scale and translate normalized sample points based on mean and (square root) covariance.
     * This algorithm decreases the complexity from O(n^3) to O(n^2).
     * This steps recursively through a tuple of input and noise distributions.
     */
    template<
      std::size_t dim, //< The total number of dimensions for which sigma points are assigned.
      std::size_t pos = 0, //< The writing position during this recursive step.
      typename D, //< First input or noise distribution.
      typename...Ds> //< Other input or noise distributions.
    static auto
    sigma_points_impl(const D& d, const Ds&...ds)
    {
      using Scalar = typename DistributionTraits<D>::Scalar;
      using Coeffs = typename DistributionTraits<D>::StaticDescriptor;
      using M = typename DistributionTraits<D>::Mean;
      constexpr auto points_count = sigma_point_count<dim>;
      constexpr auto dim_i = index_dimension_of_v<D, 0>;
      constexpr auto frame_size = dim_i * 2;
      constexpr Scalar gamma_L = alpha * alpha * (Parameters::template kappa<dim> + dim);
      const auto delta = make_vector_space_adapter(to_dense_object(square_root(gamma_L * covariance_of(d))), Coeffs{}, Dimensions<dim_i>{});

      if constexpr(1 + frame_size == points_count)
      {
        // | 0 | delta | -delta |
        static_assert(sizeof...(ds) == 0);
        using M0 = dense_writable_matrix_t<M, Layout::none, Scalar, std::tuple<Coeffs, Axis>>;
        const auto m0 = make_zero<M0>(Dimensions<dim_i>{}, Dimensions<1>{});
        auto ret {make_self_contained(concatenate_horizontal(std::move(m0), delta, -delta))};
        static_assert(index_dimension_of_v<decltype(ret), 1> == points_count);
        return std::tuple {std::move(ret)};
      }
      else if constexpr (pos == 0)
      {
        // | 0 | delta | -delta | 0 ... |
        using M0 = dense_writable_matrix_t<M, Layout::none, Scalar, std::tuple<Coeffs, Axis>>;
        const auto m0 = make_zero<M0>(Dimensions<dim_i>{}, Dimensions<1>{});
        constexpr auto width = points_count - (1 + frame_size);
        using Mright = dense_writable_matrix_t<M, Layout::none, Scalar, std::tuple<Coeffs, Dimensions<width>>>;
        const auto mright = make_zero<Mright>(Dimensions<dim_i>{}, Dimensions<width>{});
        auto ret {make_self_contained(concatenate_horizontal(std::move(m0), delta, -delta, std::move(mright)))};
        static_assert(index_dimension_of_v<decltype(ret), 1> == points_count);
        return std::tuple_cat(std::tuple {std::move(ret)}, sigma_points_impl<dim, 1 + frame_size>(ds...));
      }
      else if constexpr (pos + frame_size < points_count)
      {
        // | 0 | 0 ... | delta | -delta | 0 ... |
        using Mleft = dense_writable_matrix_t<M, Layout::none, Scalar, std::tuple<Coeffs, Dimensions<pos>>>;
        const auto mleft = make_zero<Mleft>(Dimensions<dim_i>{}, Dimensions<pos>{});
        constexpr auto width = points_count - (pos + frame_size);
        using Mright = dense_writable_matrix_t<M, Layout::none, Scalar, std::tuple<Coeffs, Dimensions<width>>>;
        const auto mright = make_zero<Mright>(Dimensions<dim_i>{}, Dimensions<width>{});
        auto ret {make_self_contained(concatenate_horizontal(std::move(mleft), delta, -delta, std::move(mright)))};
        static_assert(index_dimension_of_v<decltype(ret), 1> == points_count);
        return std::tuple_cat(std::tuple {std::move(ret)}, sigma_points_impl<dim, pos + frame_size>(ds...));
      }
      else
      {
        // | 0 | 0 ... | delta | -delta |
        static_assert(sizeof...(ds) == 0);
        using Mleft = dense_writable_matrix_t<M, Layout::none, Scalar, std::tuple<Coeffs, Dimensions<pos>>>;
        const auto mleft = make_zero<Mleft>(Dimensions<dim_i>{}, Dimensions<pos>{});
        auto ret {make_self_contained(concatenate_horizontal(std::move(mleft), delta, -delta))};
        static_assert(index_dimension_of_v<decltype(ret), 1> == points_count);
        return std::tuple {std::move(ret)};
      }
    }

  public:

#ifdef __cpp_concepts
    template<gaussian_distribution ... Dist> requires (sizeof...(Dist) > 0)
#else
    template<typename...Dist, std::enable_if_t<
      (gaussian_distribution<Dist> and ...) and (sizeof...(Dist) > 0), int> = 0>
#endif
    static auto
    sample_points(const Dist& ...ds)
    {
      constexpr auto dim = (index_dimension_of_v<Dist, 0> + ...);
      return sigma_points_impl<dim>(ds...);
    }
  };


}

#endif //OPENKALMAN_UNSCENTED_HPP