initial implementation

KalmanFilter.cpp

@@ -0,0 +1,78 @@
+/* @file KalmanFilter.cpp */
+
+#include "KalmanFilter.hpp"
+#include "KalmanFilterEngine.hpp"
+#include "print_eigen.hpp"
+#include "indentlog/scope.hpp"
+#include "Eigen/src/Core/Matrix.h"
+
+namespace xo {
+  using xo::time::utc_nanos;
+  //using logutil::matrix;
+  using xo::scope;
+  using xo::tostr;
+  using xo::xtag;
+  using Eigen::MatrixXd;
+  using Eigen::VectorXd;
+
+  namespace kalman {
+    // ----- KalmanFilter -----
+
+    KalmanFilter::KalmanFilter(KalmanFilterSpec spec)
+      : filter_spec_{std::move(spec)},
+    state_ext_{filter_spec_.start_ext()}
+    {} /*ctor*/
+
+    void
+    KalmanFilter::notify_input(ref::rp<KalmanFilterInput> const & input_kp1)
+    {
+        scope log(XO_ENTER0(info));
+
+      /* on entry:
+       *    .state_ext refers to t(k)
+       * on exit:
+       *    .step refers to t(k+1)
+       *    .state_ext refers to t(k+1)
+       */
+
+        log && log(xtag("step_dt",
+                        input_kp1->tkp1() - this->state_ext_->tm()));
+
+      /* establish step inputs for this filter step:
+       *   F(k+1)   (system transition matrix)
+       *   Q(k+1)   (system noise covariance matrix)
+       *   H(k+1)   (observation coupling matrix)
+       *   R(k+1)   (observation noise covariance matrix)
+       *   z(k+1)   (observation vector)
+       */
+      this->step_ = this->filter_spec_.make_step(this->state_ext_, input_kp1);
+
+      //if (lscope.enabled()) { lscope.log(xtag("step", this->step_)); }
+
+      /* extrapolate filter state to t(k+1),
+       * and correct based on z(k+1)
+       */
+      this->state_ext_ = KalmanFilterEngine::step(this->step_);
+
+      //if (lscope.enabled()) { lscope.log(xtag("state_ext", this->state_ext_)); }
+    } /*notify_input*/
+
+    void
+    KalmanFilter::display(std::ostream & os) const
+    {
+      os << "<KalmanFilter";
+      os << xtag("filter_spec", filter_spec_);
+      //os << xtag("step", step_);
+      os << xtag("state_ext", state_ext_);
+      os << ">";
+    } /*display*/
+
+    std::string
+    KalmanFilter::display_string() const
+    {
+      return tostr(*this);
+    } /*display_string*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilter.cpp */