xo-alloc/KalmanFilterState.cpp

/* @file KalmanFilterState.cpp */

#include "KalmanFilterState.hpp"
#include "print_eigen.hpp"
#include "reflect/StructReflector.hpp"
#include "reflect/TaggedPtr.hpp"
#include "indentlog/scope.hpp"
#include "Eigen/src/Core/Matrix.h"
#include <ostream>
#include <string>

namespace xo {
  using xo::reflect::Reflect;
  using xo::reflect::TaggedRcptr;
  using xo::reflect::StructReflector;
  using xo::time::utc_nanos;
  using xo::ref::rp;
  using logutil::matrix;
  using logutil::vector;
  //using xo::scope;
  using xo::xtag;
  using xo::tostr;
  //using Eigen::LDLT;
  using Eigen::MatrixXd;
  using Eigen::VectorXd;

  namespace kalman {
    // ----- KalmanFilterState -----

    rp<KalmanFilterState>
    KalmanFilterState::make()
    {
      return new KalmanFilterState();
    } /*make*/

    rp<KalmanFilterState>
    KalmanFilterState::make(uint32_t k,
                utc_nanos tk,
                VectorXd x,
                MatrixXd P,
                KalmanFilterTransition transition)
    {
      return new KalmanFilterState(k, tk,
                   std::move(x),
                   std::move(P),
                   std::move(transition));
    } /*make*/

    void
    KalmanFilterState::reflect_self()
    {
      StructReflector<KalmanFilterState> sr;

      if (sr.is_incomplete()) {
    REFLECT_MEMBER(sr, k);
    REFLECT_MEMBER(sr, tk);
    REFLECT_MEMBER(sr, x);
    REFLECT_MEMBER(sr, P);
      }
    } /*reflect_self*/

    KalmanFilterState::KalmanFilterState() = default;

    KalmanFilterState::KalmanFilterState(uint32_t k,
                     utc_nanos tk,
                     VectorXd x,
                     MatrixXd P,
                     KalmanFilterTransition transition)
      : k_{k}, tk_{tk},
    x_{std::move(x)}, P_{std::move(P)},
    transition_{std::move(transition)}
    {}

    TaggedRcptr
    KalmanFilterState::self_tp()
    {
      return Reflect::make_rctp(this);
    } /*self_tp*/

    // ----- KalmanFilterExt -----

    rp<KalmanFilterStateExt>
    KalmanFilterStateExt::make()
    {
      return new KalmanFilterStateExt();
    } /*make*/

    rp<KalmanFilterStateExt>
    KalmanFilterStateExt::make(uint32_t k,
                   utc_nanos tk,
                   VectorXd x,
                   MatrixXd P,
                   KalmanFilterTransition transition,
                   MatrixXd K,
                   int32_t j,
                   rp<KalmanFilterInput> zk)
    {
      return new KalmanFilterStateExt(k,
                      tk,
                      std::move(x),
                      std::move(P),
                      std::move(transition),
                      std::move(K),
                      j,
                      std::move(zk));
    } /*make*/

    void
    KalmanFilterStateExt::reflect_self()
    {
      StructReflector<KalmanFilterStateExt> sr;

      if (sr.is_incomplete()) {
    /* TODO: use sr.adopt_ancestors<KalmanFilterState>() */

    REFLECT_EXPLICIT_MEMBER(sr, "k", &KalmanFilterState::k_);
    REFLECT_EXPLICIT_MEMBER(sr, "tk", &KalmanFilterState::tk_);
    REFLECT_EXPLICIT_MEMBER(sr, "x", &KalmanFilterState::x_);
    REFLECT_EXPLICIT_MEMBER(sr, "P", &KalmanFilterState::P_);
    REFLECT_EXPLICIT_MEMBER(sr, "transition", &KalmanFilterState::transition_);
    REFLECT_MEMBER(sr, j);
    REFLECT_MEMBER(sr, K);
    REFLECT_MEMBER(sr, zk);
      }
    } /*reflect_self*/

    KalmanFilterStateExt::KalmanFilterStateExt(uint32_t k,
                           utc_nanos tk,
                           VectorXd x,
                           MatrixXd P,
                           KalmanFilterTransition transition,
                           MatrixXd K,
                           int32_t j,
                           rp<KalmanFilterInput> zk)
      : KalmanFilterState(k, tk,
              std::move(x),
              std::move(P),
              std::move(transition)),
    j_{j},
    K_{std::move(K)},
    zk_{std::move(zk)}
    {
      uint32_t n = x.size();

      if (n != P.rows() || n != P.cols()) {
    std::string err_msg
      = tostr("with n=x.size expect [n x n] covar matrix P",
          xtag("n", x.size()),
          xtag("P.rows", P.rows()),
          xtag("P.cols", P.cols()));

      throw std::runtime_error(err_msg);
      }

      if ((K.rows() > 0) && (K.rows() > 0)) {
    if (n != K.rows()) {
      std::string err_msg
        = tostr("with n=x.size expect [m x n] gain matrix K",
            xtag("n", x.size()),
            xtag("K.rows", K.rows()),
            xtag("K.cols", K.cols()));

      throw std::runtime_error(err_msg);
    }
      } else {
    /* bypass test with [0 x 0] matrix K;
     * normal for initial filter state
     */
      }
    } /*ctor*/

    void
    KalmanFilterState::display(std::ostream & os) const
    {
      os << "<KalmanFilterState"
     << xtag("k", k_)
     << xtag("tk", tk_)
     << xtag("x", matrix(x_))
     << xtag("P", matrix(P_))
     << ">";
    } /*display*/

    std::string
    KalmanFilterState::display_string() const
    {
      std::stringstream ss;
      ss << *this;
      return ss.str();
    } /*display_string*/

    // ----- KalmanFilterStateExt -----

    ref::rp<KalmanFilterStateExt>
    KalmanFilterStateExt::initial(utc_nanos t0,
                  VectorXd x0,
                  MatrixXd P0)
    {
      return KalmanFilterStateExt::make
    (0 /*k*/,
     t0,
     std::move(x0),
     std::move(P0),
     KalmanFilterTransition(MatrixXd() /*F - not used for initial step*/,
                MatrixXd() /*Q - not used for initial step*/),
     MatrixXd() /*K - not used for initial step*/,
     -1 /*j - not used for initial step*/,
     nullptr /*zk - not defined for initial step*/);
    } /*initial*/

    void
    KalmanFilterStateExt::display(std::ostream & os) const
    {
      os << "<KalmanFilterStateExt"
     << xtag("k", this->step_no())
     << xtag("tk", this->tm())
     << xtag("x", matrix(this->state_v()))
     << xtag("P", matrix(this->state_cov()))
     << xtag("K", matrix(K_))
     << xtag("j", j_)
     << ">";
    } /*display*/

    TaggedRcptr
    KalmanFilterStateExt::self_tp()
    {
      return Reflect::make_rctp(this);
    } /*self_tp*/
  } /*namespace filter*/
} /*namespace xo*/

/* end KalmanFilterState.cpp */