initial implementation

CMakeLists.txt.old

@@ -0,0 +1,37 @@
+# filter/CMakeLists.txt
+
+set(SELF_LIBRARY_NAME filter)
+
+set(SELF_SOURCE_FILES KalmanFilterSvc.cpp KalmanFilter.cpp KalmanFilterState.cpp KalmanFilterTransition.cpp KalmanFilterObservable.cpp KalmanFilterInput.cpp KalmanFilterStep.cpp KalmanFilterSpec.cpp KalmanFilterEngine.cpp KalmanFilterInputToConsole.cpp KalmanFilterStateToConsole.cpp EigenUtil.cpp init_filter.cpp)
+
+# build shared liburary 'filter'
+add_library(${SELF_LIBRARY_NAME} SHARED ${SELF_SOURCE_FILES})
+
+set_target_properties(${SELF_LIBRARY_NAME}
+              PROPERTIES
+              VERSION ${PROJECT_VERSION}
+              SOVERSION 1
+              PUBLIC_HEADER init_filter.hpp)
+
+# ----------------------------------------------------------------
+# all the warnings!
+#
+xo_compile_options(${SELF_LIBRARY_NAME})
+xo_include_options(${SELF_LIBRARY_NAME})
+
+# ----------------------------------------------------------------
+# internal dependencies: reactor, ...
+
+#target_link_libraries(${SELF_LIBRARY_NAME} PUBLIC process)
+target_link_libraries(${SELF_LIBRARY_NAME} PUBLIC reactor)
+target_link_libraries(${SELF_LIBRARY_NAME} PUBLIC printjson)
+target_link_libraries(${SELF_LIBRARY_NAME} PUBLIC refcnt)
+
+# ----------------------------------------------------------------
+# 3rd party dependency: eigen:
+
+xo_eigen_dependency(${SELF_LIBRARY_NAME})
+
+xo_install_library(${SELF_LIBRARY_NAME})
+
+# end CMakeLists.txt

EXAMPLES

@@ -0,0 +1,164 @@
+scaffold kalman filter implementation here
+
+notation:
+
+x_(k) : n x 1 : true system state that we want to estimate
+F(k)  : n x n : state transition matrix at time t(k)
+w_(k) : n x 1 : system noise,  with mean 0, covariance Q(k)
+Q(k)  : n x n : covariance of systen noise
+
+v_(k) : m x 1 : observation errors at time t(k)
+R(k)  : m x m : observation error covariance matrix at time t(k)
+
+x(k)  : n x 1 : state vector estimate for time t(k)
+P(k)  : n x n : covariance matrix for x(k)
+z(k)  : m x 1 : observation vector at time t(k)
+H(k)  : m x n : observation matrix at time t(k)
+
+K(k)  :       : kalman gain - measures information gain from observation z(k)
+                relative to prior P(k | k-1)
+
+use shorthand:
+  xT, F(k)T   for transpose(x), transpose(F(k))
+  x(k+1 | k)  for "estimate x(k+1) given information known at t(k)"
+  
+A. Linear Kalman Filter
+-----------------------
+
+1. system model:
+     x_(k+1) = F(k).x_(k) + w_(k),    w_(k) ~ N(0,Q)
+
+   i.e. expected behavior of system from t(k) -> t(k+1),
+   absent system noise, is given by linear transformation F(k)
+
+   ofc model is not directly observable,
+   since we don't know x_(k) or w_(k),
+   instead we will be estimating it.
+
+2. measurement model:
+     z(k+1) = H(k+1).x_(k+1) + v_(k),   v_(k) ~ N(0, R)
+
+3. prior:
+     x(0), P(0)
+
+   must be supplied as initial input
+
+4. pre-estimate for t(k+1) system state:
+   (before incorporating z(k+1) and accounting for system noise)
+     x(k+1|k) := F(k).x(k)
+
+   in other words propagate t(k) estimate to t(k+1),
+   using F(k)
+   
+5. pre-estimate for t(k+1) estimate covariance
+   (before incorporating z(k+1) and accounting for system noise)
+     P(k+1|k) := F(k).P(k).F(k)T + Q(k)
+
+6. kalman gain matrix
+     K(k+1) := P(k+1|k).H(k)T.inverse(H(k).P(k+1|k).H(k)T + R(k))
+
+   note that the matrix-to-be-inverted in the gain expression
+   is symmetric and positive-definite (so can use cholesky decomposition)
+
+7. innovation: difference between actual observation vector and
+   observation predicted from state estimate x(k+1|k):
+     z(k+1) - H(k+1).x(k+1|k)
+
+8. corrected state estimate for t(k+1):
+     x(k+1) := x(k+1|k) + K(k+1)[z(k+1) - H(k+1).x(k+1|k)]
+
+9. correct state covariance for t(k+1):
+     P(k+1) := [I - K(k+1).H(k+1)].P(k+1|k)
+     
+
+B. Extended Kalman Filter
+-------------------------
+Must write in continuous form,  to deal with non-linearities
+
+notation:
+
+x_(t)      : n x 1 : true system state that we want to estimate,
+                     continuously evolves with t
+f(x_(t),t) : n x 1 : expected derivative (d/dt)(x_(t))
+h(x_(t),t) : m x 1 : observation function
+x(k)       : n x 1 : state vector estimate for x(t) at time t=t(k)
+F(x(t),t)  : n x n : jacobian of f(x_(t),t) evaluated at x_(t)=x(t)
+H(x(t),t)  : m x n : jacobian of h(x_(t),t) evaluated at x_(t)=x(t)
+
+1. system model:
+    (d/dt)x_(t) = f(x_(t),t) + w(t),
+   where
+    w(t) is a white noise with
+
+      / t2
+      |
+      |   w(t).dt   ~   N(0, Q(t1,t2))
+      |
+      / t1
+
+   editorial:
+   note that we exclude more general model
+     (d/dt)x_(t) = f(x_(t),t)) + G(x_(t),t).w(t)
+   because would be unable to assume RHS mean-square integrable.
+   The form chosen effectively band-limits the frequencies at which
+   w(t) acts,   since it's independent of x_(t).  This allows mean-square
+   rules to be applied as in linear kalman filter
+
+2. measurement model:
+     z(k+1) = h(x_(t(k+1)), t(k+1)) + v(k),  v(k) ~ N(0, Rk)
+
+3. prior:
+     x(0), P(0)
+
+    (same as for linear kalman filter)
+
+(3a). state estimate propagation model:
+    (d/dt)x(t) = f(x(t),t)
+
+   editorial:
+   this step is *sketchy*:  x(t) is intended to be minimum-variance
+   estimate for system state,  but that property is lost when solving
+   differential equation (d/dt)x(t) = f(x(t),t) with non-linear f.
+
+(3b). covariance estimate propagation model:
+    (d/dt)P(t) = F(x(t),t).P(t) + P(t).F(x(t),t)T + Q(t)
+
+4. pre-estimate (extrapolation) for t(k+1) system state:
+
+   can use first order approx:
+    x(k+1|k) = x(k) + F(x(tk),tk).(t(k+1) - t(k))
+   or multiple timesteps if t(k+1) - t(k) is too large
+
+   i.e. approximately solving (3a)
+
+5. pre-estimate (extrapolation) for t(k+1) error covariance:
+
+    P(k+1|k) = P(k) + [F(x(tk),tk).P(k) + P(k)F(x(tk),tk)T + Q(tk)].(t(k+1) - t(k))
+   or multiple timesteps if t(k+1) - t(k) is too large
+
+   i.e. approximately solving (3b)
+
+6. kalman gain matrix
+    K(k+1) = P(k+1|k)
+              .H(k+1)T(x(k+1|k))
+	      .inverse[H(k+1)(x(k+1|k)).P(k+1|k).H(k+1)T(x(k+1|k)) + R(k+1)]
+
+7. innovation:
+    z(k+1) - h(x(k+1|k), t(k+1))
+
+8. corrected state estimate for t(k+1):
+    x(k+1) = x(k+1|k) + K(k+1).(z(k+1) - h(k+1)(x(k+1|k))
+
+9. corrected error covariance for t(k+1):
+    P(k+1) = [I - K(k+1).H(k+1)(x(k+1|k))].P(k+1|k)
+
+   editorial:
+   This step is *sketchy*, to the extend h() is non-linear,  linearizing around
+   x(k+1|k) may give poor estimate of state covariance
+
+10. linearization (1st order term for taylor series of f() around x(t))
+  jacobian:
+    F(x(t),t) = (df/dx_) evaluated at x_(t)=x(t)
+
+    H(k+1)(x(k+1|k)) = (dh/dx_) evaluated at x_(t)=x(k+1|k)
+

EigenUtil.cpp

@@ -0,0 +1,157 @@
+/* file EigenUtil.cpp
+ *
+ * author: Roland Conybeare, Sep 2022
+ */
+
+#include "EigenUtil.hpp"
+#include "printjson/PrintJson.hpp"
+#include "reflect/Reflect.hpp"
+#include <Eigen/Dense>
+#include <memory>
+#include <cstdint>
+#include <cassert>
+
+namespace xo {
+  using xo::json::PrintJson;
+  using xo::json::JsonPrinter;
+  using xo::reflect::Reflect;
+  using xo::reflect::TypeDescr;
+  using VectorXb = Eigen::Array<bool, Eigen::Dynamic, 1>;
+  using Eigen::VectorXd;
+  using Eigen::MatrixXd;
+
+#ifdef NOT_YET
+  namespace reflect {
+    template<typename ElementType>
+    using EigenVectorX_Tdx = xo::reflect::StlVectorTdx<Eigen::Array<ElementType,
+								    Eigen::Dynamic,
+								    1>>;
+    
+    /* probably need this to appear before decl for class xo::reflect::Reflect */
+    template<typename ElementType>
+    class EstablishTdx<Eigen::Array<ElementType, Eigen::Dynamic, 1>> {
+    public:
+      static std::unique_ptr<TypeDescrExtra> make() {
+	return EigenVectorX_Tdx<ElementType>::make();
+      } /*make*/
+    }; /*EstablishTdx*/
+  } /*reflect*/
+#endif
+  
+  namespace eigen {
+
+    namespace {
+      /* prints a VectorXd as json,  in the obvious format, e.g.
+       *   [1,2,3]
+       */
+      template<typename EigenVectorType>
+      class EigenVectorJsonPrinter : public JsonPrinter {
+      public:
+	EigenVectorJsonPrinter(PrintJson const * pjson) : JsonPrinter(pjson) {}
+
+	virtual void print_json(TaggedPtr tp,
+				std::ostream * p_os) const override
+	{
+	  EigenVectorType * pv = this->check_recover_native<EigenVectorType>(tp, p_os);
+
+	  if (pv) {
+            /* EigenVectorType (VectorXb, VectorXd, ..)
+	     * is reflected as atomic for now, out of expedience.
+	     *
+             * as soon as we reflect as mt_vector,  will not need this helper.
+             */
+            *p_os << "[";
+
+            for (std::uint32_t i = 0, n = pv->size(); i < n; ++i) {
+              if (i > 0)
+                *p_os << ",";
+
+		/* note: need to dispatch via json printer for vector elements,
+		 *       to get special treatment for non-finite values
+		 */
+	      this->pjson()->print((*pv)[i], p_os);
+	      //*p_os << jsonp((*pv)[i], this->pjson());
+            }
+
+            *p_os << "]";
+	  }
+	} /*print_json*/
+      }; /*EigenVectorJsonPrinter*/
+
+      /* prints a MatrixXd as json,   in row-major format, e.g.
+       *   [[1,2,3], [4,5,6], [7,8,9]]
+       */
+      class MatrixXdJsonPrinter : public JsonPrinter {
+      public:
+	MatrixXdJsonPrinter(PrintJson const * pjson) : JsonPrinter(pjson) {}
+
+	virtual void print_json(TaggedPtr tp,
+				std::ostream * p_os) const override
+	{
+	  MatrixXd * pm = this->check_recover_native<MatrixXd>(tp, p_os);
+
+	  if (pm) {
+	    /* MatrixXd is reflected as atomic for now, out of expedience */
+	    *p_os << "[";
+
+	    for(std::uint32_t i=0, m=pm->rows(); i<m; ++i) {
+	      if (i > 0)
+		*p_os << ", ";
+	      *p_os << "[";
+	      for(std::uint32_t j=0, n=pm->cols(); j<n; ++j) {
+		if (j > 0)
+		  *p_os << ",";
+
+		/* note: need to dispatch via json printer for matrix elements,
+		 *       to get special treatment for non-finite values
+		 */
+		this->pjson()->print((*pm)(i, j), p_os);
+		//*p_os << jsonp((*pm)(i, j), this->pjson());
+	      }
+	      *p_os << "]";
+	    }
+
+	    *p_os << "]";
+	  }
+	} /*print_json*/
+      }; /*MatrixXdJsonPrinter*/
+
+      template<typename EigenVectorType>
+      void
+      provide_eigen_vector_printer(PrintJson * p_pjson)
+      {
+	TypeDescr td = Reflect::require<EigenVectorType>();
+	std::unique_ptr<JsonPrinter> pr(new EigenVectorJsonPrinter<EigenVectorType>(p_pjson));
+
+	p_pjson->provide_printer(td, std::move(pr));
+      } /*provide_eigen_vector_printer*/
+    } /*namespace*/
+
+    void
+    EigenUtil::reflect_eigen()
+    {
+#ifdef NOT_YET
+      Reflect::require<VectorXb>();
+      Reflect::require<VectorXd>();
+#endif
+    } /*reflect_eigen*/
+
+    void
+    EigenUtil::provide_json_printers(PrintJson * p_pjson)
+    {
+      assert(p_pjson);
+      
+      provide_eigen_vector_printer<VectorXb>(p_pjson);
+      provide_eigen_vector_printer<VectorXd>(p_pjson);
+
+      {
+	TypeDescr td = Reflect::require<MatrixXd>();
+	std::unique_ptr<JsonPrinter> pr(new MatrixXdJsonPrinter(p_pjson));
+
+	p_pjson->provide_printer(td, std::move(pr));
+      }
+    } /*provide_json_printers*/
+  } /*namespace eigen*/
+} /*namespace xo*/
+
+/* end EigenUtil.cpp */

EigenUtil.hpp

@@ -0,0 +1,29 @@
+/* file EigenUtil.hpp
+ *
+ * author: Roland Conybeare, Sep 2022
+ */
+
+#pragma once
+
+namespace xo {
+  namespace json { class PrintJson; }
+
+  namespace eigen {
+    class EigenUtil {
+    public:
+      /* reflection for
+       *   Eigen::VectorXd
+       *   VectorXb (= Eigen::Array<bool, Eigen::Dynamic, 1>; by analogy with Eigen::VectorXd)
+       */
+      static void reflect_eigen();
+
+      /* json printers for:
+       *   Eigen::VectorXd
+       *   Eigen::MatrixXd
+       */
+      static void provide_json_printers(json::PrintJson * pjson);
+    }; /*EigenUtil*/
+  } /*namespace eigen*/
+} /*namespace xo*/
+
+/* end EigenUtil.hpp */

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 */

KalmanFilter.hpp

@@ -0,0 +1,128 @@
+/* @file KalmanFilter.hpp */
+
+#pragma once
+
+#include "filter/KalmanFilterSpec.hpp"
+
+namespace xo {
+  namespace kalman {
+    /* Specification for an ordinary discrete linear kalman filter.
+     *
+     * The filter generates estimates for a process observed at a discrete
+     * set of times tk in {t0, t1, .., tn}
+     *
+     * At each time tk we have the following:
+     *
+     * 0. x(0) initial estimate at t(0)
+     *    P(0) initial priors: error covariance matrix for x(0)
+     *
+     * 1. x_(k), [n x 1] vector:
+     *    system state, denoted by vector.
+     *    (state is not directly observable,
+     *     filter will attempt to estimate it)
+     *
+     * 2. w_(k), [n x 1] vector
+     *    Q(k),  [n x n] matrix
+     *
+     *    w_(k) denotes system noise,
+     *    gaussian with covariance Q(k).
+     *    noise w_(k) is not directly observable.
+     *
+     * 3. z(k), [m x 1] vector:
+     *
+     *    observation vector for time tk
+     *
+     * 4. v_(k), [m x 1] vector
+     *    R(k), [m x m] matrix
+     *
+     *    v_(k) denotes observation errors,
+     *    gaussian with covariance R(k).
+     *    noise v_(k) is not directly observable.
+     *
+     * 5. F(k), [n x n] matrix
+     *    state transition matrix
+     *    model system evolves according to:
+     *
+     *    x_(k+1) = F(x).x_(k) + w_(k)
+     *
+     * 6. observations z(k) depend on system state:
+     *
+     *    z(k) = H(k).x_(k) + v_(k)    
+     *     
+     * 7. Kalman filter outputs:
+     *    x(k), [n x 1] vector
+     *    Q(k), [n x n] matrix
+     *
+     *    x(k) is optimal estimate for system state x_(k)
+     *    P(k) is covariance matrix specifying confidence intervals
+     *         for pairs (x(k)[i], x(k)[j])
+     *
+     * filter specification consists of:
+     *    n, m, x(0), P(0), F(k), Q(k), H(k), R(k)
+     * The cardinality of observations z(k) can vary over time,
+     * so to be precise,  m can vary with tk;   write as m(k)
+     *
+     * More details:
+     * - avoid having to specify t(k) in advance;
+     *   instead defer until observation available
+     *   so t(k) can be taken from polling timestamp
+     */
+
+    /* encapsulate a (linear) kalman filter
+     * together with event publishing
+     */
+    class KalmanFilter {
+    public:
+      using MatrixXd = Eigen::MatrixXd;
+      using VectorXd = Eigen::VectorXd;
+      using utc_nanos = xo::time::utc_nanos;
+
+    public:
+      /* create filter with specification given by spec,  and initial state s0 */
+      explicit KalmanFilter(KalmanFilterSpec spec);
+
+      uint32_t step_no() const { return state_ext_->step_no(); }
+      utc_nanos tm() const { return state_ext_->tm(); }
+      KalmanFilterSpec const & filter_spec() const { return filter_spec_; }
+      KalmanFilterStep const & step() const { return step_; }
+      ref::rp<KalmanFilterStateExt> const & state_ext() const { return state_ext_; }
+
+      /* notify kalman filter with input for time t(k+1) = input_kp1.tkp1()
+       * Require: input.tkp1() >= .current_tm()
+       * Promise:
+       * - .tm() = input_kp1.tkp1()
+       * - .step_no() = old .step_no() + 1
+       * - .filter_spec_k, .step_k, .state_k updated
+       *   for observations in input_kp1
+       */
+      void notify_input(ref::rp<KalmanFilterInput> const & input_kp1);
+
+      void display(std::ostream & os) const;
+      std::string display_string() const;
+
+    private:
+      /* specification for kalman filter;
+       * produces process/observation matrices on demand
+       */
+      KalmanFilterSpec filter_spec_;
+
+      /* filter step for most recent observation */
+      KalmanFilterStep step_;
+
+      /* filter state as of most recent observation;
+       * result of applying KalmanFilterEngine::step() to contents of .step
+       */
+      ref::rp<KalmanFilterStateExt> state_ext_;
+    }; /*KalmanFilter*/
+
+    inline std::ostream &
+    operator<<(std::ostream & os, KalmanFilter const & x) {
+      x.display(os);
+      return os;
+    } /*operator<<*/
+
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+
+/* end KalmanFilter.hpp */

KalmanFilterEngine.cpp

@@ -0,0 +1,360 @@
+/* @file KalmanFilterEngine.cpp
+ *
+ */
+
+#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::xtag;
+  using Eigen::LDLT;
+  using Eigen::MatrixXd;
+  using Eigen::VectorXd;
+
+  namespace kalman {
+    // ----- KalmanFilterEngine -----
+
+    ref::rp<KalmanFilterState>
+    KalmanFilterEngine::extrapolate(utc_nanos tkp1,
+                                    ref::rp<KalmanFilterState> const & s,
+                                    KalmanFilterTransition const & f)
+    {
+        //constexpr char const * c_self_name
+        //    = "KalmanFilterEngine::extrapolate";
+
+        /* prior estimates at t(k) */
+        VectorXd const & x = s->state_v();
+        MatrixXd const & P = s->state_cov();
+
+        /* model change from t(k) -> t(k+1) */
+        MatrixXd const & F = f.transition_mat();
+        MatrixXd const & Q = f.transition_cov();
+
+        if(F.cols() != x.rows()) {
+            scope log(XO_DEBUG(true /*debug_flag*/));
+
+            log("error: F*x: expected F.cols=x.rows",
+                xtag("F.cols", F.cols()), xtag("x.rows", x.rows()));
+        }
+
+        /* x(k+1|k) */
+        VectorXd x_ext = F * x;
+
+        /* P(k+1|k) */
+        MatrixXd P_ext = (F * P * F.transpose()) + Q;
+
+        /* creating new state object here
+         * allows KalmanFilterSvc.is_volatile()=false
+         */
+
+        return KalmanFilterState::make(s->step_no() + 1,
+                                       tkp1,
+                                       std::move(x_ext),
+                                       std::move(P_ext),
+                                       f);
+    } /*extrapolate*/
+
+    VectorXd
+    KalmanFilterEngine::kalman_gain1(ref::rp<KalmanFilterState> const & skp1_ext,
+                                     KalmanFilterObservable const & h,
+                                     uint32_t j)
+    {
+        constexpr bool c_debug_enabled = false;
+
+        scope log(XO_DEBUG(c_debug_enabled));
+
+        /* P(k+1|k) :: [n x n] */
+        MatrixXd const & P_ext = skp1_ext->state_cov();
+
+        /* H(k) :: [m x n] */
+        MatrixXd const & H = h.observable();
+        /* R(k) :: [m x m] */
+        MatrixXd const & R = h.observable_cov();
+
+        /* i'th col of H couples element #i of filter state to each member of input z(k);
+         * j'th row of H couples filter state to j'th observable
+         *
+         * Hj :: [1 x n]    Hj is a row-vector
+         */
+        auto Hj = H.row(j);
+
+        /* Rjj is the j'th diagonal element of R */
+        double Rjj = R(j, j);
+
+        /*                            T
+         *   M(k) = Hj * P(k+1|k) * Hj  + Rjj
+         *
+         * M(k) is a [1 x 1] matrix
+         */
+        double m = Hj * (P_ext * Hj.transpose()) + Rjj;
+
+        /*     -1
+         * M(k)      trivial,  since M is [1 x 1]
+         */
+        double m_inv = 1.0 / m;
+
+        /* K :: [n x 1] */
+        VectorXd K = P_ext * Hj.transpose() * m_inv;
+
+        log && log("result",
+                   xtag("P(k+1|k)", matrix(P_ext)),
+                   xtag("R", matrix(R)),
+                   xtag("m", m));
+
+        return K;
+    } /*kalman_gain1*/
+
+    MatrixXd
+    KalmanFilterEngine::kalman_gain(ref::rp<KalmanFilterState> const & skp1_ext,
+                                    KalmanFilterObservable const & h)
+    {
+        scope log(XO_DEBUG(false /*debug_enabled*/));
+
+        /* P(k+1|k) */
+        MatrixXd const & P_ext = skp1_ext->state_cov();
+
+        MatrixXd const & H = h.observable();
+        MatrixXd const & R = h.observable_cov();
+
+        uint32_t m = H.rows();
+        uint32_t n = H.cols();
+
+        if ((P_ext.rows() != n) || (P_ext.cols() != n)) {
+            std::string err_msg
+                = tostr("kalman_gain: with dim(H) = [m x n] expect dim(P) = [n x n]",
+                        xtag("m", m), xtag("n", n),
+                        xtag("P.rows", P_ext.rows()),
+                        xtag("P.cols", P_ext.cols()));
+
+            throw std::runtime_error(err_msg);
+        }
+
+        if ((R.rows() != m) || (R.cols() != m)) {
+            std::string err_msg
+                = tostr("kalman_gain: with dim(H) = [m x n] expect dim(R) = [m x m]",
+                        xtag("m", m), xtag("n", n),
+                        xtag("R.rows", R.rows()), xtag("R.cols", R.cols()));
+
+            throw std::runtime_error(err_msg);
+        }
+
+        /* kalman gain:
+         *                         T  -1
+         *   K(k+1) = P(k+1|k).H(k) .M
+         *
+         *                         T /                   T       \ -1
+         *          = P(k+1|k).H(k) .| H(k).P(k+1|k).H(k) + R(k) |
+         *                           \                           /
+         *
+         * Notes:
+         * 1. the matrix M being inverted is symmetric,  since represents covariances.
+         * 2. if diagonal of R(k) has no zeroes (i.e. all measurements are subject to error),
+         *    then it must be non-negative definite
+         * 3. unless observation errors are perfectly correlated, M(k)
+         *    is positive definite.
+         * 4. even though 3. holds,  there may be a nearby non-positive-definite matrix M+dM,
+         *    Factoring M with finite-precision arithmetic solves for M+dM instead of M;
+         *    which may run into difficulty if M is only 'slighlty' +ve definite.
+         *    If necessary add small diagonal correction D to M,
+         *    sufficient to make M+D positive definite.
+         *    This is equivalent to introducing additional
+         *    uncorrelated observation error,   so benign from a robustness perspective
+         * 5. In generally we usually want to avoid fully realizing a matrix inverse.
+         *    In this case need to explicitly compute K as ingredient used to
+         *    correct state covariance later.
+         * 6. However,  if R is diagonal (which is in practice quite likely),
+         *    then it's easy to decompose a suite of vector observations z(k+1) = [z1, ..zm]T
+         *    into separate zi, with dt=0 separating them.
+         *    Can use this to avoid computing the inverse.
+         *    See .kalman_gain1(), .correct1()
+         * 7. .kalman_gain() works unaltered when H, R have been reindexed
+         *    to exclude outliers/errors;   this is true because .kalman_gain() does not
+         *    use the observation vector z[], i.e. operates entirely in the reduced
+         *    reindexed space.
+         */
+
+        MatrixXd M = H * P_ext * H.transpose() + R;
+
+        /* will use to write M as:
+         *
+         *        T      T
+         *   M = P .L.D.L .P
+         *
+         * where:
+         *   P is a permutation matrix
+         *   L is lower triangular,  with unit diagonal
+         *   D is diagonal
+         */
+        LDLT<MatrixXd> ldlt = M.ldlt();
+
+        /* solve for the identity matrix to realize the inverse this way */
+        MatrixXd I = MatrixXd::Identity(M.rows(), M.cols());
+
+        /*  -1
+         * M
+         */
+        MatrixXd M_inv = ldlt.solve(I);
+
+        /* K(k+1) */
+        MatrixXd K = P_ext * H.transpose() * M_inv;
+
+        log && log("result",
+                   xtag("k", skp1_ext->step_no()),
+                   xtag("P(k+1|k)", matrix(P_ext)),
+                   xtag("H", matrix(H)),
+                   xtag("R", matrix(R)),
+                   xtag("M", matrix(M)),
+                   xtag("K", matrix(K)));
+
+        return K;
+    } /*kalman_gain*/
+
+    ref::rp<KalmanFilterStateExt>
+    KalmanFilterEngine::correct1(ref::rp<KalmanFilterState> const & skp1_ext,
+                 KalmanFilterObservable const & h,
+                 ref::rp<KalmanFilterInput> const & zkp1,
+                 uint32_t j)
+    {
+      uint32_t n = skp1_ext->n_state();
+      /* Kj :: [n x 1] */
+      VectorXd Kj = kalman_gain1(skp1_ext, h, j);
+      /* H :: [m x n] */
+      MatrixXd const & H = h.observable();
+      VectorXd const & z = zkp1->z();
+
+      /* Hj :: [1 x n]  the j'th row of H */
+      auto const & Hj = H.row(j);
+
+
+      /* x(k+1|x) :: [n x 1] */
+      VectorXd const & x_ext = skp1_ext->state_v();
+
+      /* P(k+1|k) :: [n x n] */
+      MatrixXd const & P_ext = skp1_ext->state_cov();
+
+      /* innovj : difference between jth 'actual observation'
+       *          and jth 'predicted observation'
+       */
+      double innovj = z[j] - (Hj * x_ext);
+
+      /* x(k+1) */
+      VectorXd xkp1 = x_ext + (Kj * innovj);
+
+      MatrixXd I = MatrixXd::Identity(n, n);
+      /* note: Kj [n x 1], Hj [1 x n],
+       *       so Kj * Hj [n x n],  with rank 1
+       */
+      MatrixXd Pkp1 = (I - (Kj * Hj)) * P_ext;
+
+      return KalmanFilterStateExt::make(skp1_ext->step_no(),
+                    skp1_ext->tm(),
+                    xkp1,
+                    Pkp1,
+                    skp1_ext->transition(),
+                    Kj,
+                    j,
+                    zkp1);
+    } /*correct1*/
+
+    ref::rp<KalmanFilterStateExt>
+    KalmanFilterEngine::correct(ref::rp<KalmanFilterState> const & skp1_ext,
+                KalmanFilterObservable const & h,
+                ref::rp<KalmanFilterInput> const & zkp1)
+    {
+      uint32_t n = skp1_ext->n_state();
+      /* K :: [n x m] */
+      MatrixXd K = kalman_gain(skp1_ext, h);
+      MatrixXd const & H = h.observable();
+      /* z_orig[] is original observation vector before reindexing */
+      VectorXd const & z_orig = zkp1->z();
+      /* reindex z_orig, keeping only elements that appear in
+       */
+      VectorXd z = z_orig(h.keep());
+
+      /* 'ext' short for 'extrapolated' */
+      VectorXd const & x_ext = skp1_ext->state_v();
+      MatrixXd const & P_ext = skp1_ext->state_cov();
+
+      /* innov: difference between 'actual observations'
+       * and 'predicted observations'
+       */
+      VectorXd innov = z - (H * x_ext);
+
+      /* x(k+1) :: [n x 1] */
+      VectorXd xkp1 = x_ext + K * innov;
+      MatrixXd I = MatrixXd::Identity(n, n);
+      MatrixXd Pkp1 = (I - K * H) * P_ext;
+
+      return KalmanFilterStateExt::make(skp1_ext->step_no(),
+                    skp1_ext->tm(),
+                    xkp1,
+                    Pkp1,
+                    skp1_ext->transition(),
+                    K,
+                    -1 /*j: not used*/,
+                    zkp1);
+    } /*correct*/
+
+    ref::rp<KalmanFilterStateExt>
+    KalmanFilterEngine::step(utc_nanos tkp1,
+                 ref::rp<KalmanFilterState> const & sk,
+                 KalmanFilterTransition const & Fk,
+                 KalmanFilterObservable const & Hkp1,
+                 ref::rp<KalmanFilterInput> const & zkp1)
+    {
+      ref::rp<KalmanFilterState> skp1_ext
+    = KalmanFilterEngine::extrapolate(tkp1, sk, Fk);
+
+      ref::rp<KalmanFilterStateExt> skp1
+    = KalmanFilterEngine::correct(skp1_ext, Hkp1, zkp1);
+
+      return skp1;
+    } /*step*/
+
+    ref::rp<KalmanFilterStateExt>
+    KalmanFilterEngine::step(KalmanFilterStep const & step_spec)
+    {
+      return step(step_spec.tkp1(),
+          step_spec.state(),
+          step_spec.model(),
+          step_spec.obs(),
+          step_spec.input());
+    } /*step*/
+
+    ref::rp<KalmanFilterStateExt>
+    KalmanFilterEngine::step1(utc_nanos tkp1,
+                  ref::rp<KalmanFilterState> const & sk,
+                  KalmanFilterTransition const & Fk,
+                  KalmanFilterObservable const & Hkp1,
+                  ref::rp<KalmanFilterInput> const & zkp1,
+                  uint32_t j)
+    {
+      ref::rp<KalmanFilterState> skp1_ext
+    = KalmanFilterEngine::extrapolate(tkp1, sk, Fk);
+
+      ref::rp<KalmanFilterStateExt> skp1
+    = KalmanFilterEngine::correct1(skp1_ext, Hkp1, zkp1, j);
+
+      return skp1;
+    } /*step1*/
+
+    ref::rp<KalmanFilterStateExt>
+    KalmanFilterEngine::step1(KalmanFilterStep const & step_spec,
+                  uint32_t j)
+    {
+      return step1(step_spec.tkp1(),
+           step_spec.state(),
+           step_spec.model(),
+           step_spec.obs(),
+           step_spec.input(),
+           j);
+    } /*step1*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterEngine.cpp */

KalmanFilterEngine.hpp

@@ -0,0 +1,185 @@
+/* @file KalmanFilterEngine.hpp */
+
+#pragma once
+
+#include "filter/KalmanFilterStep.hpp"
+#include "filter/KalmanFilterState.hpp"
+#include "filter/KalmanFilterTransition.hpp"
+#include "filter/KalmanFilterObservable.hpp"
+#include "filter/KalmanFilterInput.hpp"
+
+namespace xo {
+  namespace kalman {
+    class KalmanFilterEngine {
+    public:
+      using MatrixXd = Eigen::MatrixXd;
+      using VectorXd = Eigen::VectorXd;
+      using utc_nanos = xo::time::utc_nanos;
+
+    public:
+      /* evolution of system state + account for system noise,
+       * before incorporating effect of observations z(k+1)
+       *   x(k) --> x(k+1|k)
+       *   P(k) --> P(k+1|k)
+       *
+       * tkp1.  time t(k+1) assoc'd with step k+1
+       * sk.    filter state at time tk:
+       *    sk.k  = k       step # (starts from 0)
+       *    sk.tk = t(k)    time t(k) assoc'd with step #k
+       *    sk.x = x(k)     estimated state at time tk
+       *    sk.P = P(k)     quality of state estimate x(k)
+       *                    (error covariance matrix)
+       * Fk. state transition:
+       *    Fk.F = F(k)     state transition matrix
+       *    Fk.Q = Q(k)     covariance matrix for system noise
+       *
+       * returns propagated state estimate for t(k+1):
+       *    retval.k = k+1
+       *    retval.tk = t(k+1) = tkp1
+       *    retval.x = x(k+1|k)
+       *    retval.P = P(k+1|k)
+       */
+      static ref::rp<KalmanFilterState> extrapolate(utc_nanos tkp1,
+						    ref::rp<KalmanFilterState> const & sk,
+						    KalmanFilterTransition const & Fk);
+
+      /* compute kalman gain matrix for filter step t(k) -> t(k+1)
+       * Expensive implementation using matrix inversion
+       *
+       *                              T
+       *   M(k+1) = H(k).P(k+1|k).H(k) + R(k)
+       *
+       *                         T       -1
+       *   K(k+1) = P(k+1|k).H(k) .M(k+1)
+       *
+       * Require:
+       * - skp1_ext.n_state() = Hkp1.n_state()
+       *
+       * skp1_ext.  extrapolated filter state at t(k+1)
+       * Hkp1.      relates model state to observable variables,
+       *            for step t(k) -> t(k+1)
+       */
+      static MatrixXd kalman_gain(ref::rp<KalmanFilterState> const & skp1_ext,
+				  KalmanFilterObservable const & Hkp1);
+
+      /* compute kalman gain for a single observation z(k)[j].
+       * This is useful iff the observation error matrix R is diagonal.
+       * For diagonal R we can present a set of observations z(k) serially
+       * instead of all at once,  with lower time complexity
+       *
+       * Kalman Filter specifies some space with m observables.
+       * j identifies one of those observables, indexing from 0.
+       * This corresponds to row #j of H(k), and element R[j,j] of R.
+       *
+       * Effectively,  we are projecting the kalman filter assoc'd with
+       * {skp1_ext, Hkp1} to a filter with a single observable variable z(k)[j],
+       * then computing the (scalar) kalman gain for this 1-variable filter
+       *
+       * The gain vector tells us for each member of filter state,
+       * how much to adjust our optimal estimate for that member for a unit
+       * amount of innovation in observable #j,  i.e. for difference between
+       * expected and actual value for that observable.
+       */
+      static VectorXd kalman_gain1(ref::rp<KalmanFilterState> const & skp1_ext,
+				   KalmanFilterObservable const & Hkp1,
+				   uint32_t j);
+
+      /* correct extrapolated state+cov estimate;
+       * also computes kalman gain
+       *
+       * Require:
+       * - skp1_ext.n_state() = Hkp1.n_state()
+       * - zkp1.n_obs() == Hkp1.n_observable()
+       *
+       * skp1_ext.      extrapolated kalman state + covaraince at t(k+1)
+       * Hkp1.          relates model state to observable variables
+       *                for step t(k) -> t(k+1)
+       * zkp1.          observations for time t(k+1)
+       *
+       * return new filter state+cov
+       */
+      static ref::rp<KalmanFilterStateExt> correct(ref::rp<KalmanFilterState> const & skp1_ext,
+						   KalmanFilterObservable const & Hkp1,
+						   ref::rp<KalmanFilterInput> const & zkp1);
+
+      /* correct extrapolated filter state for observation
+       * of j'th filter observable z(k+1)[j]
+       *
+       * Can use this when observation errors are uncorrelated
+       * (i.e. observation error matrix R is diagonal)
+       */
+      static ref::rp<KalmanFilterStateExt> correct1(ref::rp<KalmanFilterState> const & skp1_ext,
+						    KalmanFilterObservable const & Hkp1,
+						    ref::rp<KalmanFilterInput> const & zkp1,
+						    uint32_t j);
+
+      /* step filter from t(k) -> t(k+1)
+       *
+       * sk.        filter state from previous step:
+       *            x (state vector), P (state covar matrix)
+       * Fk.        transition-related params:
+       *            F (transition matrix), Q (system noise covar matrix)
+       * Hkp1.      observation-related params:
+       *            H (coupling matrix), R (error covar matrix)
+       * zkp1.      observations z(k+1) for time t(k+1)
+       */
+      static ref::rp<KalmanFilterStateExt> step(utc_nanos tkp1,
+						ref::rp<KalmanFilterState> const & sk,
+						KalmanFilterTransition const & Fk,
+						KalmanFilterObservable const & Hkp1,
+						ref::rp<KalmanFilterInput> const & zkp1);
+
+      /* step filter from t(k) -> tk(k+1)
+       * same as
+       *   .step(tkp1, sk, step_spec.model(), step_spec.obs(), zkp1);
+       *
+       * step_spec.  encapsulates Fk (transition-related params)
+       *             and Q (system noise covar matrix)
+       */
+      static ref::rp<KalmanFilterStateExt> step(KalmanFilterStep const & step_spec);
+
+      /* step filter from t(k) -> t(k+1)
+       *
+       * sk.    filter state from previous step:
+       *        x (state vector), P (state covar matrix)
+       * Fk.    transition-related params:
+       *        F (transition matrix), Q (system noise covar matrix)
+       * Hkp1.  observation-related params:
+       *        H (coupling matrix), R (error covar matrix)
+       * zkp1.  observations z(k+1) for time t(k+1)
+       * j.     identifies a single filter observable --
+       *        step will only consume observation z(k+1)[j]
+       */
+      static ref::rp<KalmanFilterStateExt> step1(utc_nanos tkp1,
+						 ref::rp<KalmanFilterState> const & sk,
+						 KalmanFilterTransition const & Fk,
+						 KalmanFilterObservable const & Hkp1,
+						 ref::rp<KalmanFilterInput> const & zkp1,
+						 uint32_t j);
+
+      /* step filter from t(k) -> t(k+1)
+       *
+       * same as
+       *   .step1(step_spec.tkp1(),
+       *          step_spec.state(),
+       *          step_spec.model(),
+       *          step_spec.obs(),
+       *          step_spec.input(),
+       *          j);
+       *
+       * step_spec.  encapsulates
+       *             x (state vector),
+       *             P (state covar matrix),
+       *             Fk (transition-related params),
+       *             Q (system noise covar matrix)
+       *             z (z(k+1), observations at time t(k+1))
+       * j.          identifies a single filter observable --
+       *             step will only consume observation z(k+1)[j]
+       */
+      static ref::rp<KalmanFilterStateExt> step1(KalmanFilterStep const & step_spec,
+						 uint32_t j);
+    }; /*KalmanFilterEngine*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterEngine.hpp */

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 */

KalmanFilterInput.hpp

@@ -0,0 +1,121 @@
+/* @file KalmanFilterInput.hpp */
+
+#pragma once
+
+#include "reflect/SelfTagging.hpp"
+#include "time/Time.hpp"
+#include "refcnt/Refcounted.hpp"
+#include <Eigen/Dense>
+#include <cstdint>
+
+namespace xo {
+  /* FIXME.  hack here to get member access working for reflection */
+  namespace vf { class StrikesetKfinput; }
+
+  namespace kalman {
+    /* represents a single kalman filter input event
+     * comprising:
+     *   - time tkp1
+     *   - observation vector z[]
+     *   - presence bits presence[] for z
+     *   - observation errors Rd[]
+     */
+    class KalmanFilterInput : public reflect::SelfTagging {
+    public:
+      using TaggedRcptr = xo::reflect::TaggedRcptr;
+      using utc_nanos = xo::time::utc_nanos;
+      using VectorXb = Eigen::Array<bool, Eigen::Dynamic, 1>;
+      using VectorXi = Eigen::VectorXi;
+      using VectorXd = Eigen::VectorXd;
+      using uint32_t = std::uint32_t;
+
+    public:
+      KalmanFilterInput() = default;
+
+      static ref::rp<KalmanFilterInput> make(utc_nanos tkp1,
+                         VectorXb const & presence,
+                         VectorXd const & z,
+                         VectorXd const & Rd);
+
+      /* create input,  with all presence bits set + not using Rd */
+      static ref::rp<KalmanFilterInput> make_present(utc_nanos tkp1,
+                             VectorXd const & z);
+
+      /* reflect KalmanFilterInput object representation */
+      static void reflect_self();
+
+      /* alt name -- concession to reactor::DirectSource
+       *             when event type is KalmanFilterInput
+       */
+      utc_nanos tm() const { return tkp1_; }
+
+      utc_nanos tkp1() const { return tkp1_; }
+      uint32_t n_obs() const { return z_.size(); }
+      VectorXb const & presence() const { return presence_; }
+      VectorXd const & z() const { return z_; }
+      VectorXd const & Rd() const { return Rd_; }
+
+      /* computes reindexer keep[]:
+       *   .presence[keep[j*]]
+       * is the j*'th true value in .presence
+       */
+      VectorXi make_kept_index() const;
+
+      virtual void display(std::ostream & os) const;
+      std::string display_string() const;
+
+      // ----- inherited from SelfTagging -----
+
+      virtual TaggedRcptr self_tp() override;
+
+    protected:
+      KalmanFilterInput(utc_nanos tkp1, VectorXb presence, VectorXd z, VectorXd Rd)
+    : tkp1_(tkp1),
+      presence_{std::move(presence)},
+      z_{std::move(z)},
+      Rd_{std::move(Rd)} {}
+
+      friend class xo::vf::StrikesetKfinput;
+
+    private:
+      /* t(k+1) - asof time for observations .z */
+      utc_nanos tkp1_ = xo::time::timeutil::epoch();
+      /* [m x 1] presence vector.
+       *         an observation z[j] is present iff .presence[j] is true.
+       *         rows/columns for an absent observation are removed from filter matrices
+       */
+      VectorXb presence_;
+      /* [m x 1] observation vector z(k) */
+      VectorXd z_;
+
+      /* [m x 1] observation error vector Rd(k).
+       *         This represents a side channel for passing the diagonal of
+       *         observation matrix R(k), when both:
+       *         (a) error of different observations are assumed to be uncorrelated (likely)
+       *         (b) error variance is derived from input data, e.g. because of
+       *             some input preprocessing.
+       *
+       *         It's up to KalmanFilterSpec::MkStepFn to opt-in to using this information,
+       *         which requires agreement with any input preparation step.
+       *
+       *         This variable could just as well provide observation error matrix R
+       *
+       * NOTE: perhaps-cleaner alternative would be to inherit KalmanFilterInput to
+       *       introduce additional state,  then MkStepFn can dynamic_cast
+       */
+      VectorXd Rd_;
+    }; /*KalmanFilterInput*/
+
+    using KalmanFilterInputPtr = ref::rp<KalmanFilterInput>;
+
+    inline std::ostream &
+    operator<<(std::ostream & os, KalmanFilterInput const & x)
+    {
+      x.display(os);
+      return os;
+    } /*operator<<*/
+
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterInput.hpp */

KalmanFilterInputCallback.hpp

@@ -0,0 +1,14 @@
+/* @file KalmanFilterInputCallback.hpp */
+
+#pragma once
+
+#include "refcnt/Refcounted.hpp"
+#include "filter/KalmanFilter.hpp"
+
+namespace xo {
+  namespace kalman {
+    using KalmanFilterInputCallback = reactor::Sink1<ref::rp<KalmanFilterInput>>;
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterInputCallback.hpp */

KalmanFilterInputSource.hpp

@@ -0,0 +1,28 @@
+/* @file KalmanFilterInputSource.hpp */
+
+#pragma once
+
+#include "reactor/EventSource.hpp"
+#include "filter/KalmanFilterInputCallback.hpp"
+
+namespace xo {
+  namespace kalman {
+    /* Use:
+     *    rp<KalmanFilterInputSource> src = ...;
+     *    rp<KalmanFilterInputCallback> in_cb = ...;
+     *
+     *    src->add_callback(in_cb);
+     * ...
+     *    // src invokes in_cb->notify_input(
+     *    src->remove_callback(in_cb);
+     */
+    using KalmanFilterInputSource = reactor::EventSource<
+      /*KalmanFilterInput,*/
+      KalmanFilterInputCallback
+      /*&KalmanFilterInputCallback::notify_filter*/
+      >;
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterInputSource.hpp */
+  

KalmanFilterInputToConsole.cpp

@@ -0,0 +1,25 @@
+/* @file KalmanFilterInputToConsole.cpp */
+
+#include "KalmanFilterInputToConsole.hpp"
+#include "indentlog/print/tag.hpp"
+
+namespace xo {
+  using xo::xtag;
+
+  namespace kalman {
+    ref::rp<KalmanFilterInputToConsole>
+    KalmanFilterInputToConsole::make() {
+      return new KalmanFilterInputToConsole();
+    } /*make*/
+
+    void
+    KalmanFilterInputToConsole::display(std::ostream & os) const
+    {
+      os << "<KalmanFilterInputToConsole"
+     << xtag("this", (void*)this)
+     << ">";
+    } /*display*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterInputToConsole.cpp */

KalmanFilterInputToConsole.hpp

@@ -0,0 +1,30 @@
+/* @file KalmanFilterInputToConsole.hpp */
+
+#pragma once
+
+#include "reactor/Sink.hpp"
+#include "filter/KalmanFilterInput.hpp"
+
+namespace xo {
+  namespace kalman {
+    class KalmanFilterInputToConsole
+      : public xo::reactor::SinkToConsole<ref::rp<KalmanFilterInput>>
+    {
+    public:
+      KalmanFilterInputToConsole() = default;
+
+      static ref::rp<KalmanFilterInputToConsole> make();
+
+      virtual void display(std::ostream & os) const;
+      //virtual std::string display_string() const;
+    }; /*KalmanFilterInputToConsole*/
+
+    inline std::ostream &
+    operator<<(std::ostream & os, KalmanFilterInputToConsole const & x) {
+      x.display(os);
+      return os;
+    } /*operator<<*/
+  } /*namespace option*/
+} /*namespace xo*/
+
+/* end KalmanFilterInputToConsole.hpp */

KalmanFilterObservable.cpp

@@ -0,0 +1,71 @@
+/* @file KalmanFilterObservable.cpp */
+
+#include "KalmanFilterObservable.hpp"
+#include "print_eigen.hpp"
+#include "indentlog/scope.hpp"
+
+namespace xo {
+  using xo::scope;
+  using logutil::matrix;
+  using xo::xtag;
+
+  namespace kalman {
+    KalmanFilterObservable
+    KalmanFilterObservable::keep_all(MatrixXd H,
+                     MatrixXd R)
+    {
+      VectorXi keep(H.rows());
+
+      for (uint32_t j=0; j<H.rows(); ++j)
+    keep[j] = j;
+
+      return KalmanFilterObservable(std::move(keep),
+                    std::move(H),
+                    std::move(R));
+    } /*keep_all*/
+
+    KalmanFilterObservable
+    KalmanFilterObservable::reindex(VectorXi keep,
+                                    MatrixXd H,
+                                    MatrixXd R)
+    {
+        scope log(XO_DEBUG(false /*debug_flag*/));
+
+        /* Hp:
+         *   - keep rows in H with indices that appear in keep[]
+         *   - keep all columns of H
+         */
+
+        MatrixXd Hp = H(keep, Eigen::all);
+        MatrixXd Rp = R(keep, keep);
+
+        if (log.enabled()) {
+            log(xtag("keep", matrix(keep)));
+            log(xtag("H", matrix(H)));
+            log(xtag("R", matrix(R)));
+        }
+
+        return KalmanFilterObservable(keep, Hp, Rp);
+    } /*reindex*/
+
+      void
+      KalmanFilterObservable::display(std::ostream & os) const
+      {
+          os << "<KalmanFilterObservable"
+             << xtag("H", matrix(H_))
+             << xtag("R", matrix(R_))
+             << ">";
+      } /*display*/
+
+      std::string
+      KalmanFilterObservable::display_string() const
+      {
+          std::stringstream ss;
+          this->display(ss);
+          return ss.str();
+      } /*display_string*/
+
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterObservable.cpp */

KalmanFilterObservable.hpp

@@ -0,0 +1,110 @@
+/* @file KalmanFilterObservable.hpp */
+
+#pragma once
+
+#include "time/Time.hpp"
+#include <Eigen/Dense>
+#include <cstdint>
+
+namespace xo {
+  namespace kalman {
+    class KalmanFilterObservable {
+    public:
+      using MatrixXd = Eigen::MatrixXd;
+      using VectorXi = Eigen::VectorXi;
+      
+    public:
+      KalmanFilterObservable() = default;
+
+      /* keep maps back to canonical observations z(j).
+       * H, R have been re-indexed
+       *
+       * If all m observations are included,  then keep will be:
+       *   [0, .., m-1]
+       */
+      KalmanFilterObservable(VectorXi keep, MatrixXd H, MatrixXd R)
+	: keep_{std::move(keep)}, H_{std::move(H)}, R_{std::move(R)} {
+	assert(this->check_ok());
+      } /*ctor*/
+
+      /* build KF observable object where keeping all observations */
+      static KalmanFilterObservable keep_all(MatrixXd H, MatrixXd R);
+
+      /* build KF observable object.  replace H, R with reindexed versions H', R'
+       * according to indexing vector keep[].   keep[] indexes members of
+       * observation vector z(k)[j].   Reindexed z', H', R' as follows:
+       *
+       *   z'[j*]       = z[keep[j*]]
+       *   H'[j*, i]    = H[keep[j*], i]
+       *   R'[j1*, j2*] = R[keep[j1*], keep[j2*]]
+       */
+      static KalmanFilterObservable reindex(VectorXi keep, MatrixXd H, MatrixXd R);
+
+      uint32_t n_state() const { return H_.cols(); }
+      uint32_t n_observable() const { return H_.rows(); }
+      VectorXi const & keep() const { return keep_; }
+      MatrixXd const & observable() const { return H_; }
+      MatrixXd const & observable_cov() const { return R_; }
+      
+      bool check_ok() const {
+	uint32_t m = H_.rows();
+	bool keep_is_mx1 = (keep_.rows() == m);
+	bool r_is_mxm = ((R_.cols() == m) && (R_.rows() == m));
+
+	bool keep_is_well_ordered = true;
+
+	/* members of .keep are non-negative integers,
+	 * in strictly increasing order
+	 */
+	int64_t keep_jm1 = -1;
+
+	for (uint32_t j = 0; j < keep_.rows(); ++j) {
+	  if (keep_[j] < 0)
+	    keep_is_well_ordered = false;
+	  if (keep_[j] <= keep_jm1)
+	    keep_is_well_ordered = false;
+	}
+
+	/* also would like to require: R is +ve definite */
+	
+	return keep_is_mx1 && keep_is_well_ordered && r_is_mxm;
+      } /*check_ok*/
+
+      void display(std::ostream & os) const;
+      std::string display_string() const;
+
+    private:
+
+    private:
+      /* m: #of observations that survived sanity/error checks
+       *
+       * H, R here will have been re-indexed to exclude rejected observations.
+       * observations z will also have been re-indexed.
+       *
+       * If an observation z[j] is excluded,  then also exclude:
+       * - j'th row of H
+       * - j'th row and j'th column of R
+       * - j'th element of z
+       *
+       * Given re-indexed H, R,   the j*'th row of H goes with z[keep[j*]]
+       */
+
+      /* [m x 1] maps back to indices in original observation vector */
+      VectorXi keep_;
+      /* [m x n] observation matrix */
+      MatrixXd H_;
+      /* [m x m] covariance matrix for observation noise */
+      MatrixXd R_;
+    }; /*KalmanFilterObservable*/
+
+    inline std::ostream &
+    operator<<(std::ostream & os, KalmanFilterObservable const & x)
+    {
+      x.display(os);
+      return os;
+    } /*operator<<*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterObservable.hpp */
+   

KalmanFilterOutputCallback.hpp

@@ -0,0 +1,15 @@
+/* @file KalmanFilterOutputCallback.hpp */
+
+#pragma once
+
+#include "reactor/Sink.hpp"
+#include "filter/KalmanFilter.hpp"
+
+namespace xo {
+  namespace kalman {
+    /* callback for consuming kalman filter output */
+    using KalmanFilterOutputCallback = reactor::Sink1<ref::rp<KalmanFilterStateExt>>;
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterOutputCallback.hpp */

KalmanFilterSpec.cpp

@@ -0,0 +1,27 @@
+/* @file KalmanFilterSpec.cpp */
+
+#include "KalmanFilterSpec.hpp"
+#include "indentlog/scope.hpp"
+
+namespace xo {
+  using xo::tostr;
+  using xo::xtag;
+
+  namespace kalman {
+    void
+    KalmanFilterSpec::display(std::ostream & os) const
+    {
+      os << "<KalmanFilterSpec"
+     << xtag("start_ext", start_ext_)
+     << ">";
+    } /*display*/
+
+    std::string
+    KalmanFilterSpec::display_string() const
+    {
+      return tostr(*this);
+    } /*display_string*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterSpec.cpp */

KalmanFilterSpec.hpp

@@ -0,0 +1,86 @@
+/* @file KalmanFilterSpec.hpp */
+
+#pragma once
+
+#include "filter/KalmanFilterStep.hpp"
+
+namespace xo {
+  namespace kalman {
+    /* full specification for a kalman filter.
+     *
+     * For a textbook linear filter,  expect a KalmanFilterStep
+     * instance to be independent of KalmanFilterState/KalmanFilterInput.
+     * 
+     * Relaxing this requirement for two reasons:
+     * 1. (proper) want to allow filter with variable timing between observations,
+     *    expecially if observations are event-driven.
+     *    In that case it's likely that state transition matrices are a function
+     *    of elapsed time between observations.  Providing filter state sk
+     *    allows MkStepFn to use sk.tm()
+     * 2. (sketchy) when observations represent market data,
+     *    desirable to treat an observation as giving one-sided information
+     *    about true value.   For example treat a bid price as evidence 
+     *    true value is higher than that bid,  but don't want to constrain
+     *    "how much higher".   Certainly no reason to think that
+     *    bid price is normally distributed around fair value.
+     *    Allow for hack in which we 
+     *    and modulate error distribution "as if it were normal" to assess
+     *    a non-gaussian error distribution
+     */
+    class KalmanFilterSpec {
+    public:
+      /* typical implementation will look something like:
+       *    mk_step(KalmanFilterState const & sk,
+       *            KalmanFilterInputPtr const & zkp1)
+       *    {
+       *      KalmanFilterTransition model = ...;
+       *      KalmanFilterObservable obs = ...;
+       *
+       *      return KalmanFilterStep(sk, model, obs, zkp1);
+       *    }
+       */
+      using MkStepFn = std::function<KalmanFilterStep
+				     (ref::rp<KalmanFilterState> const & sk,
+	                              KalmanFilterInputPtr const & zkp1)>;
+
+    public:
+      explicit KalmanFilterSpec(ref::rp<KalmanFilterStateExt> s0, MkStepFn mkstepfn)
+	: start_ext_{std::move(s0)},
+	  mk_step_fn_{std::move(mkstepfn)} {}
+
+      ref::rp<KalmanFilterStateExt> const & start_ext() const { return start_ext_; }
+      /* get step parameters (i.e. matrices F, Q, H, R)
+       * for step t(k) -> t(k+1).
+       *
+       * We supply t(k) state s and t(k+1) observation z(k+1):
+       * - to allow time stepping to be observation-driven
+       * - to allow for selective outlier removal
+       */
+      KalmanFilterStep make_step(ref::rp<KalmanFilterState> const & sk,
+				 ref::rp<KalmanFilterInput> const & zkp1) {
+	return this->mk_step_fn_(sk, zkp1);
+      } /*make_step*/
+
+      void display(std::ostream & os) const;
+      std::string display_string() const;
+
+    private:
+      /* starting state */
+      ref::rp<KalmanFilterStateExt> start_ext_;
+
+      /* creates kalman filter step object on demand;
+       * step object specifies inputs to 1 step in discrete
+       * linear kalman filter
+       */
+      MkStepFn mk_step_fn_;
+    }; /*KalmanFilterSpec*/
+
+    inline std::ostream &
+    operator<<(std::ostream & os, KalmanFilterSpec const & x) {
+      x.display(os);
+      return os;
+    } /*operator<<*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterSpec.hpp */

KalmanFilterState.cpp

@@ -0,0 +1,231 @@
+/* @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 */

KalmanFilterState.hpp

@@ -0,0 +1,163 @@
+/* @file KalmanFilterState.hpp */
+
+#pragma once
+
+#include "reflect/SelfTagging.hpp"
+#include "filter/KalmanFilterInput.hpp"
+#include "filter/KalmanFilterTransition.hpp"
+#include "time/Time.hpp"
+#include <Eigen/Dense>
+#include <functional>
+#include <cstdint>
+
+namespace xo {
+  namespace kalman {
+    /* encapsulate state (i.e. initial state,  and output after each step)
+     * for a kalman filter
+     */
+    class KalmanFilterState : public reflect::SelfTagging {
+    public:
+      using TaggedRcptr = reflect::TaggedRcptr;
+      using utc_nanos = xo::time::utc_nanos;
+      using VectorXd = Eigen::VectorXd;
+      using MatrixXd = Eigen::MatrixXd;
+      using uint32_t = std::uint32_t;
+
+    public:
+      static ref::rp<KalmanFilterState> make();
+      static ref::rp<KalmanFilterState> make(uint32_t k,
+					     utc_nanos tk,
+					     VectorXd x,
+					     MatrixXd P,
+					     KalmanFilterTransition transition);
+      virtual ~KalmanFilterState() = default;
+
+      /* reflect KalmanFilterState object representation */
+      static void reflect_self();
+
+      uint32_t step_no() const { return k_; }
+      utc_nanos tm() const { return tk_; }
+      /* with n = .n_state():
+       *   x_ is [n x 1] vector
+       *   P_ is [n x n] matrix,
+       */
+      uint32_t n_state() const { return x_.size(); }
+      VectorXd const & state_v() const { return x_; }
+      MatrixXd const & state_cov() const { return P_; }
+
+      KalmanFilterTransition const & transition() const { return transition_; }
+
+      virtual void display(std::ostream & os) const;
+      std::string display_string() const;
+
+      // ----- inherited from SelfTagging -----
+
+      virtual TaggedRcptr self_tp() override;
+
+    private:
+      KalmanFilterState();
+      KalmanFilterState(uint32_t k,
+			utc_nanos tk,
+			VectorXd x,
+			MatrixXd P,
+			KalmanFilterTransition transition);
+
+      friend class KalmanFilterStateExt;
+      
+    private:
+      /* step# k,  advances by +1 on each filter step */
+      uint32_t k_ = 0;
+      /* time t(k) */
+      utc_nanos tk_;
+      /* [n x 1] (estimated) system state xk = x(k) */
+      VectorXd x_;
+      /* [n x n] covariance matrix for error assoc'd with with x(k)
+       *   P(i,j) is the covariance of (ek[i], ek[j]),
+       * where ex(k) is the difference (x(k) - x_(k))
+       * between estimated state x(k)
+       * (= this->x_) and model state x_(k)
+       */
+      MatrixXd P_;
+
+      /* F, Q matrices driving .x, .P */
+      KalmanFilterTransition transition_;
+    }; /*KalmanFilterState*/
+
+    inline std::ostream & operator<<(std::ostream & os,
+				     KalmanFilterState const & s)
+    {
+      s.display(os);
+      return os;
+    } /*operator<<*/
+
+    /* KalmanFilterStateExt:
+     * adds additional details from filter step to KalmanFilterState
+     */
+    class KalmanFilterStateExt : public KalmanFilterState {
+    public:
+      using TaggedRcptr = reflect::TaggedRcptr;
+      using MatrixXd = Eigen::MatrixXd;
+      using int32_t = std::int32_t;
+
+    public:
+      static ref::rp<KalmanFilterStateExt> make();
+      static ref::rp<KalmanFilterStateExt> make(uint32_t k,
+						utc_nanos tk,
+						VectorXd x,
+						MatrixXd P,
+						KalmanFilterTransition transition,
+						MatrixXd K,
+						int32_t j,
+						ref::rp<KalmanFilterInput> zk);
+      
+      /* create state object for initial filter state */
+      static ref::rp<KalmanFilterStateExt> initial(utc_nanos t0,
+						   VectorXd x0,
+						   MatrixXd P0);
+
+      /* reflect KalmanFilterStateExt object representation */
+      static void reflect_self();
+
+      int32_t observable() const { return j_; }
+      MatrixXd const & gain() const { return K_; }
+      ref::rp<KalmanFilterInput> const & zk() const { return zk_; }
+
+      virtual void display(std::ostream & os) const override;
+
+      // ----- inherited from SelfTagging -----
+
+      virtual TaggedRcptr self_tp() override;
+
+    private:
+      KalmanFilterStateExt() = default;
+      KalmanFilterStateExt(uint32_t k,
+			   utc_nanos tk,
+			   VectorXd x,
+			   MatrixXd P,
+			   KalmanFilterTransition transition,
+			   MatrixXd K,
+			   int32_t j,
+			   ref::rp<KalmanFilterInput> zk);
+
+    private:
+      /* if -1:  not used;
+       * if >= 0: identifies j'th of m observables;
+       * gain .K applies just to information obtainable from
+       * observing that scalar variable
+       */
+      int32_t j_ = -1;
+      /* if .j is -1:
+       *   [n x n] kalman gain
+       * if .j >= 0:
+       *   [n x 1] kalman gain for observable #j
+       */
+      MatrixXd K_;
+      /* input leading to state k.
+       * empty for initial state (i.e. when .k is 0)
+       */
+      ref::rp<KalmanFilterInput> zk_;
+    }; /*KalamnFilterStateExt*/
+  } /*namespace filter*/
+} /*namespace xo*/
+
+/* end KalmanFilterState.hpp */

KalmanFilterStateToConsole.cpp

@@ -0,0 +1,25 @@
+/* @file KalmanFilterStateToConsole.cpp */
+
+#include "KalmanFilterStateToConsole.hpp"
+#include "indentlog/print/tag.hpp"
+
+namespace xo {
+  using xo::xtag;
+
+  namespace kalman {
+    ref::rp<KalmanFilterStateToConsole>
+    KalmanFilterStateToConsole::make() {
+      return new KalmanFilterStateToConsole();
+    } /*make*/
+
+    void
+    KalmanFilterStateToConsole::display(std::ostream & os) const
+    {
+      os << "<KalmanFilterStateToConsole"
+     << xtag("this", (void*)this)
+     << ">";
+    } /*display*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterStateToConsole.cpp */

KalmanFilterStateToConsole.hpp

@@ -0,0 +1,30 @@
+/* @file KalmanFilterStateToConsole.hpp */
+
+#pragma once
+
+#include "reactor/Sink.hpp"
+#include "filter/KalmanFilterState.hpp"
+
+namespace xo {
+  namespace kalman {
+    class KalmanFilterStateToConsole
+      : public xo::reactor::SinkToConsole<KalmanFilterStateExt>
+    {
+    public:
+      KalmanFilterStateToConsole() = default;
+
+      static ref::rp<KalmanFilterStateToConsole> make();
+
+      virtual void display(std::ostream & os) const;
+      //virtual std::string display_string() const;
+    }; /*KalmanFilterStateToConsole*/
+
+    inline std::ostream &
+    operator<<(std::ostream & os, KalmanFilterStateToConsole const & x) {
+      x.display(os);
+      return os;
+    } /*operator<<*/
+  } /*namespace option*/
+} /*namespace xo*/
+
+/* end KalmanFilterStateToConsole.hpp */

KalmanFilterStep.cpp

@@ -0,0 +1,84 @@
+/* @file KalmanFilterStep.cpp */
+
+#include "KalmanFilterStep.hpp"
+#include "filter/KalmanFilterEngine.hpp"
+#include "filter/KalmanFilterState.hpp"
+#include "indentlog/scope.hpp"
+
+namespace xo {
+  using xo::scope;
+  using xo::tostr;
+  using xo::xtag;
+  using Eigen::MatrixXd;
+  using Eigen::VectorXd;
+
+  namespace kalman {
+    ref::rp<KalmanFilterState>
+    KalmanFilterStep::extrapolate() const
+    {
+      return KalmanFilterEngine::extrapolate(this->tkp1(),
+                         this->state(),
+                         this->model() /*transition*/);
+    } /*extrapolate*/
+
+    MatrixXd
+    KalmanFilterStep::gain(ref::rp<KalmanFilterState> const & skp1_ext) const
+    {
+      return KalmanFilterEngine::kalman_gain(skp1_ext,
+                         this->obs());
+    } /*gain*/
+
+    VectorXd
+    KalmanFilterStep::gain1(ref::rp<KalmanFilterState> const & skp1_ext,
+                uint32_t j) const
+    {
+      return KalmanFilterEngine::kalman_gain1(skp1_ext,
+                          this->obs(),
+                          j);
+
+    } /*gain1*/
+
+    ref::rp<KalmanFilterStateExt>
+    KalmanFilterStep::correct(ref::rp<KalmanFilterState> const & skp1_ext)
+    {
+      return KalmanFilterEngine::correct(skp1_ext,
+                     this->obs(),
+                     this->input());
+    } /*correct*/
+
+    ref::rp<KalmanFilterStateExt>
+    KalmanFilterStep::correct1(ref::rp<KalmanFilterState> const & skp1_ext,
+                   uint32_t j)
+    {
+      return KalmanFilterEngine::correct1(skp1_ext,
+                      this->obs(),
+                      this->input(),
+                      j);
+    } /*correct1*/
+
+    void
+    KalmanFilterStep::display(std::ostream & os) const
+    {
+      //scope lscope("KalmanFilterStep::display");
+
+      os << "<KalmanFilterStep";
+      //lscope.log("state:");
+      os << xtag("state", state_);
+      //lscope.log("model:");
+      os << xtag("model", this->model());
+      //lscope.log("obs:");
+      os << xtag("obs", this->obs());
+      //lscope.log("input:");
+      os << xtag("input", this->input());
+      os << ">";
+    } /*display*/
+
+    std::string
+    KalmanFilterStep::display_string() const
+    {
+      return tostr(*this);
+    } /*display_string*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterStep.cpp */

KalmanFilterStep.hpp

@@ -0,0 +1,128 @@
+/* @file KalmanFilterStep.hpp */
+
+#pragma once
+
+#include "KalmanFilterState.hpp"
+#include "KalmanFilterInput.hpp"
+#include "KalmanFilterTransition.hpp"
+#include "KalmanFilterObservable.hpp"
+
+namespace xo {
+  namespace kalman {
+    /* encapsulate {state + observation} models for a single time step t(k).
+     * Emitted by KalmanFilterSpec, q.v.
+     */
+    class KalmanFilterStepBase {
+    public:
+      KalmanFilterStepBase() = default;
+      KalmanFilterStepBase(KalmanFilterTransition model,
+			   KalmanFilterObservable obs)
+	: model_{std::move(model)},
+	  obs_{std::move(obs)} {}
+
+      /* aka system_model() */
+      KalmanFilterTransition const & model() const { return model_; }
+      KalmanFilterObservable const & obs() const { return obs_; }
+
+    private:
+      /* model for process being observed (state transition + noise) */
+      KalmanFilterTransition model_;
+      /* what can be observed (observables + noise) */
+      KalmanFilterObservable obs_;
+    }; /*KalmanFilterStepBase*/
+
+    /* encapsulate {state + observation} models for a single time step t(k).
+     * Emitted by KalmanFilterSpec, q.v.
+     *
+     * holds:
+     *   x(k)
+     *   P(k)
+     *   F(k)
+     *   H(k+1)
+     *   z(k+1)
+     *
+     * contains all the inputs needed to compute:
+     *   x(k+1)
+     *   P(k+1)
+     *
+     * does not provide that result
+     */
+    class KalmanFilterStep : public KalmanFilterStepBase {
+    public:
+      using utc_nanos = xo::time::utc_nanos;
+      using MatrixXd = Eigen::MatrixXd;
+      using VectorXd = Eigen::VectorXd;
+
+    public:
+      KalmanFilterStep() = default;
+      KalmanFilterStep(ref::rp<KalmanFilterState> state,
+		       KalmanFilterTransition model,
+		       KalmanFilterObservable obs,
+		       ref::rp<KalmanFilterInput> zkp1)
+	: KalmanFilterStepBase(model, obs),
+	  state_{std::move(state)},
+	  input_{std::move(zkp1)} {}
+      
+      ref::rp<KalmanFilterState> const & state() const { return state_; }
+      ref::rp<KalmanFilterInput> const & input() const { return input_; }
+
+      utc_nanos tkp1() const { return input_->tkp1(); }
+
+      /* extrapolate kalman filter state forward to time
+       * .tkp1() (i.e. to t(k+1));  computes
+       *   x(k+1|k)
+       *   P(k+1|k)
+       * does not use the t(k+1) observations .input.z
+       */
+      ref::rp<KalmanFilterState> extrapolate() const;
+
+      /* compute kalman gain matrix K(k+1)
+       * given extrapolated t(k+1) state skp1_ext = {x(k+1|k), P(k+1|k)}
+       *
+       * note that .state() != skp1_ext;  .state() reports {x(k), P(k)}
+       */
+      MatrixXd gain(ref::rp<KalmanFilterState> const & skp1_ext) const;
+
+      /* compute kalman gain vector K(k+1)
+       * given extrapolated t(k+1) state skp1_ext = {x(k+1|k), P(k+1|k)},
+       * on behalf of a single observation z[j].
+       * actual observation z[j] is not given here,
+       * just computing the gain vector.   i'th member of gain vector
+       * gives effect of innovation on i'th member of kalman filter state.
+       */
+      VectorXd gain1(ref::rp<KalmanFilterState> const & skp1_ext,
+		     uint32_t j) const;
+
+      /* compute correction to extrapolated filter state {x(k+1|k), P(k+1|k)},
+       * for observation z(k+1) = .input.z()
+       */
+      ref::rp<KalmanFilterStateExt> correct(ref::rp<KalmanFilterState> const & skp1_ext);
+
+      /* compute correction to extrapolated filter state skp1_ext = {x(k+1|k), P(k+1|k)},
+       * for a single observation z(k+1, j) = .input.z()[j]
+       */
+      ref::rp<KalmanFilterStateExt> correct1(ref::rp<KalmanFilterState> const & skp1_ext,
+					     uint32_t j);
+
+      void display(std::ostream & os) const;
+      std::string display_string() const;
+
+    private:
+      /* system state: timestamp, estimated process state, process covariance
+       *               asof beginning of this step
+       */
+      ref::rp<KalmanFilterState> state_;
+      /* input: observations at time t(k+1) */
+      KalmanFilterInputPtr input_;
+    }; /*KalmanFilterStep*/
+
+    inline std::ostream &
+    operator<<(std::ostream & os, KalmanFilterStep const & x) {
+      x.display(os);
+      return os;
+    } /*operator<<*/
+
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterStep.hpp */

KalmanFilterSvc.cpp

@@ -0,0 +1,44 @@
+/* @file KalmanFilterSvc.cpp */
+
+#include "KalmanFilterSvc.hpp"
+
+namespace xo {
+  using xo::ref::rp;
+  using xo::scope;
+  using xo::xtag;
+
+  namespace kalman {
+    rp<KalmanFilterSvc>
+    KalmanFilterSvc::make(KalmanFilterSpec spec)
+    {
+      return new KalmanFilterSvc(std::move(spec));
+    } /*make*/
+
+    KalmanFilterSvc::KalmanFilterSvc(KalmanFilterSpec spec)
+      : filter_{std::move(spec)}
+    {}
+
+    void
+    KalmanFilterSvc::notify_ev(ref::rp<KalmanFilterInput> const & input_kp1)
+    {
+      this->filter_.notify_input(input_kp1);
+
+      ++(this->n_in_ev_);
+      this->notify_secondary_event(this->filter_.state_ext());
+    } /*notify_input*/
+
+    void
+    KalmanFilterSvc::display(std::ostream & os) const
+    {
+      os << "<KalmanFilterSvc"
+     << xtag("name", this->name())
+     << xtag("n_in_ev", this->n_in_ev())
+     << xtag("n_queued_out_ev", this->n_queued_out_ev())
+     << xtag("n_out_ev", this->n_out_ev())
+    //<< xtag("filter", this->filter_)
+     << ">";
+    } /*display*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterSvc.cpp */

KalmanFilterSvc.hpp

@@ -0,0 +1,64 @@
+/* @file KalmanFilterSvc.hpp */
+
+#include "reactor/Sink.hpp"
+#include "reactor/DirectSourcePtr.hpp"
+#include "filter/KalmanFilter.hpp"
+#include "filter/KalmanFilterInputSource.hpp"
+#include "filter/KalmanFilterOutputCallback.hpp"
+#include "callback/CallbackSet.hpp"
+
+namespace xo {
+  namespace kalman {
+    /* encapsulate a passive KalmanFilter
+     * instance as an active event consumer+producer
+     *
+     * sinks that want to consume KalmanFilterSvc events will use
+     * .attach_sink() (or .add_callback())
+     */
+    class KalmanFilterSvc : public xo::reactor::Sink1<ref::rp<KalmanFilterInput>>,
+			    public xo::reactor::DirectSourcePtr<ref::rp<KalmanFilterStateExt>> {
+    public:
+      using AbstractSource = xo::reactor::AbstractSource;
+      
+    public:
+      /* named ctor idiom */
+      static ref::rp<KalmanFilterSvc> make(KalmanFilterSpec spec);
+
+      KalmanFilter const & filter() const { return filter_; }
+
+      /* notify incoming observations;  will trigger kalman filter step */
+      void notify_ev(ref::rp<KalmanFilterInput> const & input_kp1) override;
+
+      // ----- inherited from reactor::AbstractSink -----
+
+      /* filter captures KF input pointer */
+      virtual bool allow_volatile_source() const override { return false; }
+      virtual uint32_t n_in_ev() const override { return n_in_ev_; }
+      virtual void display(std::ostream & os) const override;
+
+      // ----- inherited from reactor::AbstractSource -----
+
+      /* note: correct since KalmanFilterEngine.extrapolate()
+       *       always creates new state object
+       */
+      virtual bool is_volatile() const override { return false; }
+
+      // ----- Inherited from AbstractEventProcessor -----
+
+    private:
+      KalmanFilterSvc(KalmanFilterSpec spec);
+
+    private:
+      /* passive kalman filter */
+      KalmanFilter filter_;
+      /* receive filter input from this source; see .attach_input() */
+      ref::rp<KalmanFilterInputSource> input_src_;
+      /* counts lifetime #of input events (see .notify_ev()) */
+      uint32_t n_in_ev_ = 0;
+      /* publish filter state updates to these callbacks */
+      fn::RpCallbackSet<KalmanFilterOutputCallback> pub_;
+    }; /*KalmanFilterSvc*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* KalmanFilterSvc.hpp */

KalmanFilterTransition.cpp

@@ -0,0 +1,55 @@
+/* @file KalmanFilterTransition.cpp */
+
+#include "KalmanFilterTransition.hpp"
+#include "reflect/StructReflector.hpp"
+#include "print_eigen.hpp"
+#include "indentlog/scope.hpp"
+
+namespace xo {
+  using xo::reflect::StructReflector;
+  using logutil::matrix;
+  using xo::xtag;
+
+  namespace kalman {
+    void
+    KalmanFilterTransition::reflect_self()
+    {
+      StructReflector<KalmanFilterTransition> sr;
+
+      if (sr.is_incomplete()) {
+    REFLECT_MEMBER(sr, F);
+    REFLECT_MEMBER(sr, Q);
+      }
+    } /*reflect_self*/
+
+    uint32_t
+    KalmanFilterTransition::n_state() const
+    {
+      /* we know F.rows() == F.cols() = Q.cols() == Q.rows(),
+       * see .check_ok()
+       */
+
+      return F_.rows();
+    } /*n_state*/
+
+    void
+    KalmanFilterTransition::display(std::ostream & os) const
+    {
+      os << "<KalmanFilterTransition"
+     << xtag("F", matrix(F_))
+     << xtag("Q", matrix(Q_))
+     << ">";
+    } /*display*/
+
+    std::string
+    KalmanFilterTransition::display_string() const
+    {
+      std::stringstream ss;
+      this->display(ss);
+      return ss.str();
+    } /*display_string*/
+
+  } /*namespace kalman*/
+} /*namespace xo*/
+
+/* end KalmanFilterTransition.cpp */

KalmanFilterTransition.hpp

@@ -0,0 +1,62 @@
+/* @file KalmanFilterTransition.hpp */
+
+#pragma once
+
+#include "time/Time.hpp"
+#include <Eigen/Dense>
+#include <cstdint>
+
+namespace xo {
+  namespace kalman {
+
+    /* encapsulate transition behavior for a kalman filter
+     * before taking observations into account
+     */
+    class KalmanFilterTransition {
+    public:
+      using MatrixXd = Eigen::MatrixXd;
+      using uint32_t = std::uint32_t;
+
+    public:
+      KalmanFilterTransition() = default;
+      KalmanFilterTransition(MatrixXd F,
+			     MatrixXd Q)
+	: F_{std::move(F)}, Q_{std::move(Q)} { assert(this->check_ok()); }
+
+      static void reflect_self();
+
+      /* n:    cardinality of state vector */
+      uint32_t n_state() const;
+
+      MatrixXd const & transition_mat() const { return F_; }
+      MatrixXd const & transition_cov() const { return Q_; }
+
+      bool check_ok() const {
+	uint32_t n = F_.rows();
+	bool f_is_nxn = ((F_.rows() == n) && (F_.cols() == n));
+	bool q_is_nxn = ((Q_.rows() == n) && (Q_.cols() == n));
+
+	/* also would like to require: Q is +ve definite */
+
+	return f_is_nxn && q_is_nxn;
+      } /*check_ok*/
+
+      void display(std::ostream & os) const;
+      std::string display_string() const;
+
+    private:
+      /* [n x n] state transition matrix */
+      MatrixXd F_;
+      /* [n x n] covariance matrix for system noise */
+      MatrixXd Q_;
+    }; /*KalmanFilterTransition*/
+
+    inline std::ostream &
+    operator<<(std::ostream & os, KalmanFilterTransition const & x) {
+      x.display(os);
+      return os;
+    } /*operator<<*/
+  } /*namespace kalman*/
+} /*namespace xo*/
+    
+/* end KalmanFilterTransition.hpp */

init_filter.cpp

@@ -0,0 +1,51 @@
+/* file init_filter.cpp
+ *
+ * author: Roland Conybeare, Aug 2022
+ */
+
+#include "init_filter.hpp"
+#include "reactor/init_reactor.hpp"
+#include "KalmanFilterState.hpp"
+#include "EigenUtil.hpp"
+#include "printjson/PrintJson.hpp"
+
+namespace xo {
+  using xo::kalman::KalmanFilterInput;
+  using xo::kalman::KalmanFilterTransition;
+  using xo::kalman::KalmanFilterState;
+  using xo::kalman::KalmanFilterStateExt;
+  using xo::eigen::EigenUtil;
+  using xo::json::PrintJsonSingleton;
+  using xo::json::PrintJson;
+
+  void
+  InitSubsys<S_filter_tag>::init()
+  {
+    PrintJson * pjson = PrintJsonSingleton::instance().get();
+
+    EigenUtil::reflect_eigen();
+    EigenUtil::provide_json_printers(pjson);
+
+    KalmanFilterInput::reflect_self();
+    KalmanFilterTransition::reflect_self();
+    KalmanFilterState::reflect_self();
+    KalmanFilterStateExt::reflect_self();
+
+  } /*init*/
+
+  InitEvidence
+  InitSubsys<S_filter_tag>::require()
+  {
+    InitEvidence retval;
+
+    /* subsystem dependencies for filter/ */
+    retval ^= InitSubsys<S_reactor_tag>::require();
+
+    /* filter/'s own initialization code */
+    retval ^= Subsystem::provide<S_filter_tag>("filter", &init);
+
+    return retval;
+  } /*require*/
+} /*namespace xo*/
+
+/* end init_filter.cpp */

init_filter.hpp

@@ -0,0 +1,20 @@
+/* file init_filter.hpp
+ *
+ * author: Roland Conybeare, Aug 2022
+ */
+
+#pragma once
+
+#include "subsys/Subsystem.hpp"
+
+namespace xo {
+  enum S_filter_tag {};
+
+  template<>
+  struct InitSubsys<S_filter_tag> {
+    static void init();
+    static InitEvidence require();
+  };
+} /*namespace xo*/
+
+/* end init_filter.hpp */

print_eigen.hpp

@@ -0,0 +1,41 @@
+/* @file print_eigen.hpp */
+
+#include <Eigen/Dense>
+#include <cstdint>
+
+namespace logutil {
+  template<typename T>
+  class matrix {
+  public:
+    matrix(T x) : x_{std::move(x)} {}
+
+    /* print this value */
+    T x_;
+  }; /*matrix*/
+
+  template<typename T>
+  using vector = matrix<T>;
+
+  template<typename T>
+  inline std::ostream &
+  operator<<(std::ostream & s, matrix<T> const & mat)
+  {
+    s << "[";
+    for(std::uint32_t i = 0, m = mat.x_.rows(); i<m; ++i) {
+      if(i > 0)
+	s << "; ";
+
+      for(std::uint32_t j = 0, n = mat.x_.cols(); j<n; ++j) {
+	if(j > 0)
+	  s << ' ';
+
+	s << mat.x_(i, j);
+      }
+    }
+    s << "]";
+    
+    return s;
+  } /*operator<<*/
+} /*namespace logutil*/
+
+/* end print_eigen.hpp */