initial implementation

KalmanFilterInput.cpp

@@ -0,0 +1,118 @@
+/* @file KalmanFilterInput.cpp */
+
+#include "KalmanFilterInput.hpp"
+#include "reflect/StructReflector.hpp"
+#include "Eigen/src/Core/Matrix.h"
+#include "print_eigen.hpp"
+#include "indentlog/scope.hpp"
+#include "reflect/TaggedRcptr.hpp"
+
+namespace xo {
+  using xo::reflect::Reflect;
+  using xo::reflect::TaggedRcptr;
+  using xo::reflect::StructReflector;
+  using xo::scope;
+  using logutil::matrix;
+  using xo::xtag;
+  using Eigen::MatrixXd;
+  using Eigen::VectorXi;
+  using std::uint32_t;
+
+  namespace kalman {
+    ref::rp<KalmanFilterInput>
+    KalmanFilterInput::make(utc_nanos tkp1,
+                VectorXb const & presence,
+                VectorXd const & z,
+                VectorXd const & Rd)
+    {
+      return new KalmanFilterInput(tkp1, presence, z, Rd);
+    } /*make*/
+
+    ref::rp<KalmanFilterInput>
+    KalmanFilterInput::make_present(utc_nanos tkp1,
+                    VectorXd const & z)
+    {
+      VectorXb presence = VectorXb::Ones(z.cols());
+
+      return new KalmanFilterInput(tkp1,
+                   presence,
+                   z,
+                   VectorXd(0) /*Rd - not using*/);
+    } /*make*/
+
+    VectorXi
+    KalmanFilterInput::make_kept_index() const
+    {
+        scope log(XO_DEBUG(false /*!debug_flag*/));
+
+        log && log(xtag("presence", matrix(presence_)));
+
+        /* count truth values */
+        uint32_t mstar = 0;
+
+        for (uint32_t j = 0, m = this->presence_.rows(); j<m; ++j) {
+            if (this->presence_[j])
+                ++mstar;
+        }
+
+        log && log(xtag("m*", mstar));
+
+        VectorXi keep(mstar);
+
+        /* 2nd pass,  populate keep[] */
+
+        uint32_t jstar = 0;
+
+        for (uint32_t j = 0, m = this->presence_.rows(); j<m; ++j) {
+            if (this->presence_[j]) {
+                keep[jstar] = j;
+                ++jstar;
+            }
+        }
+
+        log && log("keep", matrix(keep));
+
+        return keep;
+    } /*make_kept_index*/
+
+    void
+    KalmanFilterInput::reflect_self()
+    {
+      StructReflector<KalmanFilterInput> sr;
+
+      if (sr.is_incomplete()) {
+    REFLECT_MEMBER(sr, tkp1);
+    REFLECT_MEMBER(sr, presence);
+    REFLECT_MEMBER(sr, z);
+    REFLECT_MEMBER(sr, Rd);
+      }
+    } /*reflect_self*/
+
+    reflect::TaggedRcptr
+    KalmanFilterInput::self_tp()
+    {
+      return Reflect::make_rctp(this);
+    } /*self_tp*/
+
+    void
+    KalmanFilterInput::display(std::ostream & os) const
+    {
+      os << "<KalmanFilterInput"
+     << xtag("tkp1", tkp1_)
+     << xtag("z", matrix(z_))
+     << xtag("presence", matrix(presence_))
+     << xtag("Rd", matrix(Rd_))
+     << ">";
+    } /*display*/
+
+    std::string
+    KalmanFilterInput::display_string() const
+    {
+      std::stringstream ss;
+      this->display(ss);
+      return ss.str();
+    } /*display_string*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterInput.cpp */