From 8454b48956dcaf7b8884588595ef52d230067b4f Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sun, 22 Oct 2023 20:51:11 -0400 Subject: [PATCH 01/25] initial implementation --- CMakeLists.txt.old | 37 ++++ EXAMPLES | 164 +++++++++++++++ EigenUtil.cpp | 157 ++++++++++++++ EigenUtil.hpp | 29 +++ KalmanFilter.cpp | 78 +++++++ KalmanFilter.hpp | 128 ++++++++++++ KalmanFilterEngine.cpp | 360 +++++++++++++++++++++++++++++++++ KalmanFilterEngine.hpp | 185 +++++++++++++++++ KalmanFilterInput.cpp | 118 +++++++++++ KalmanFilterInput.hpp | 121 +++++++++++ KalmanFilterInputCallback.hpp | 14 ++ KalmanFilterInputSource.hpp | 28 +++ KalmanFilterInputToConsole.cpp | 25 +++ KalmanFilterInputToConsole.hpp | 30 +++ KalmanFilterObservable.cpp | 71 +++++++ KalmanFilterObservable.hpp | 110 ++++++++++ KalmanFilterOutputCallback.hpp | 15 ++ KalmanFilterSpec.cpp | 27 +++ KalmanFilterSpec.hpp | 86 ++++++++ KalmanFilterState.cpp | 231 +++++++++++++++++++++ KalmanFilterState.hpp | 163 +++++++++++++++ KalmanFilterStateToConsole.cpp | 25 +++ KalmanFilterStateToConsole.hpp | 30 +++ KalmanFilterStep.cpp | 84 ++++++++ KalmanFilterStep.hpp | 128 ++++++++++++ KalmanFilterSvc.cpp | 44 ++++ KalmanFilterSvc.hpp | 64 ++++++ KalmanFilterTransition.cpp | 55 +++++ KalmanFilterTransition.hpp | 62 ++++++ init_filter.cpp | 51 +++++ init_filter.hpp | 20 ++ print_eigen.hpp | 41 ++++ 32 files changed, 2781 insertions(+) create mode 100644 CMakeLists.txt.old create mode 100644 EXAMPLES create mode 100644 EigenUtil.cpp create mode 100644 EigenUtil.hpp create mode 100644 KalmanFilter.cpp create mode 100644 KalmanFilter.hpp create mode 100644 KalmanFilterEngine.cpp create mode 100644 KalmanFilterEngine.hpp create mode 100644 KalmanFilterInput.cpp create mode 100644 KalmanFilterInput.hpp create mode 100644 KalmanFilterInputCallback.hpp create mode 100644 KalmanFilterInputSource.hpp create mode 100644 KalmanFilterInputToConsole.cpp create mode 100644 KalmanFilterInputToConsole.hpp create mode 100644 KalmanFilterObservable.cpp create mode 100644 KalmanFilterObservable.hpp create mode 100644 KalmanFilterOutputCallback.hpp create mode 100644 KalmanFilterSpec.cpp create mode 100644 KalmanFilterSpec.hpp create mode 100644 KalmanFilterState.cpp create mode 100644 KalmanFilterState.hpp create mode 100644 KalmanFilterStateToConsole.cpp create mode 100644 KalmanFilterStateToConsole.hpp create mode 100644 KalmanFilterStep.cpp create mode 100644 KalmanFilterStep.hpp create mode 100644 KalmanFilterSvc.cpp create mode 100644 KalmanFilterSvc.hpp create mode 100644 KalmanFilterTransition.cpp create mode 100644 KalmanFilterTransition.hpp create mode 100644 init_filter.cpp create mode 100644 init_filter.hpp create mode 100644 print_eigen.hpp diff --git a/CMakeLists.txt.old b/CMakeLists.txt.old new file mode 100644 index 00000000..e85d245f --- /dev/null +++ b/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 diff --git a/EXAMPLES b/EXAMPLES new file mode 100644 index 00000000..8f69ea73 --- /dev/null +++ b/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) + diff --git a/EigenUtil.cpp b/EigenUtil.cpp new file mode 100644 index 00000000..f1d3c4a3 --- /dev/null +++ b/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 +#include +#include +#include + +namespace xo { + using xo::json::PrintJson; + using xo::json::JsonPrinter; + using xo::reflect::Reflect; + using xo::reflect::TypeDescr; + using VectorXb = Eigen::Array; + using Eigen::VectorXd; + using Eigen::MatrixXd; + +#ifdef NOT_YET + namespace reflect { + template + using EigenVectorX_Tdx = xo::reflect::StlVectorTdx>; + + /* probably need this to appear before decl for class xo::reflect::Reflect */ + template + class EstablishTdx> { + public: + static std::unique_ptr make() { + return EigenVectorX_Tdx::make(); + } /*make*/ + }; /*EstablishTdx*/ + } /*reflect*/ +#endif + + namespace eigen { + + namespace { + /* prints a VectorXd as json, in the obvious format, e.g. + * [1,2,3] + */ + template + 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(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(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 0) + *p_os << ", "; + *p_os << "["; + for(std::uint32_t j=0, n=pm->cols(); 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 + void + provide_eigen_vector_printer(PrintJson * p_pjson) + { + TypeDescr td = Reflect::require(); + std::unique_ptr pr(new EigenVectorJsonPrinter(p_pjson)); + + p_pjson->provide_printer(td, std::move(pr)); + } /*provide_eigen_vector_printer*/ + } /*namespace*/ + + void + EigenUtil::reflect_eigen() + { +#ifdef NOT_YET + Reflect::require(); + Reflect::require(); +#endif + } /*reflect_eigen*/ + + void + EigenUtil::provide_json_printers(PrintJson * p_pjson) + { + assert(p_pjson); + + provide_eigen_vector_printer(p_pjson); + provide_eigen_vector_printer(p_pjson); + + { + TypeDescr td = Reflect::require(); + std::unique_ptr pr(new MatrixXdJsonPrinter(p_pjson)); + + p_pjson->provide_printer(td, std::move(pr)); + } + } /*provide_json_printers*/ + } /*namespace eigen*/ +} /*namespace xo*/ + +/* end EigenUtil.cpp */ diff --git a/EigenUtil.hpp b/EigenUtil.hpp new file mode 100644 index 00000000..07a86d48 --- /dev/null +++ b/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; 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 */ diff --git a/KalmanFilter.cpp b/KalmanFilter.cpp new file mode 100644 index 00000000..bf94fde2 --- /dev/null +++ b/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 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 << ""; + } /*display*/ + + std::string + KalmanFilter::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilter.cpp */ diff --git a/KalmanFilter.hpp b/KalmanFilter.hpp new file mode 100644 index 00000000..295a4f7a --- /dev/null +++ b/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 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 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 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 */ diff --git a/KalmanFilterEngine.cpp b/KalmanFilterEngine.cpp new file mode 100644 index 00000000..e80f7497 --- /dev/null +++ b/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 + KalmanFilterEngine::extrapolate(utc_nanos tkp1, + ref::rp 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 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 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 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 + KalmanFilterEngine::correct1(ref::rp const & skp1_ext, + KalmanFilterObservable const & h, + ref::rp 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 + KalmanFilterEngine::correct(ref::rp const & skp1_ext, + KalmanFilterObservable const & h, + ref::rp 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 + KalmanFilterEngine::step(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1) + { + ref::rp skp1_ext + = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); + + ref::rp skp1 + = KalmanFilterEngine::correct(skp1_ext, Hkp1, zkp1); + + return skp1; + } /*step*/ + + ref::rp + 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 + KalmanFilterEngine::step1(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1, + uint32_t j) + { + ref::rp skp1_ext + = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); + + ref::rp skp1 + = KalmanFilterEngine::correct1(skp1_ext, Hkp1, zkp1, j); + + return skp1; + } /*step1*/ + + ref::rp + 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 */ diff --git a/KalmanFilterEngine.hpp b/KalmanFilterEngine.hpp new file mode 100644 index 00000000..b8c2f949 --- /dev/null +++ b/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 extrapolate(utc_nanos tkp1, + ref::rp 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 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 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 correct(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + ref::rp 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 correct1(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + ref::rp 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 step(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp 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 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 step1(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp 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 step1(KalmanFilterStep const & step_spec, + uint32_t j); + }; /*KalmanFilterEngine*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterEngine.hpp */ diff --git a/KalmanFilterInput.cpp b/KalmanFilterInput.cpp new file mode 100644 index 00000000..03b5330f --- /dev/null +++ b/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::make(utc_nanos tkp1, + VectorXb const & presence, + VectorXd const & z, + VectorXd const & Rd) + { + return new KalmanFilterInput(tkp1, presence, z, Rd); + } /*make*/ + + ref::rp + 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(); jpresence_[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(); jpresence_[j]) { + keep[jstar] = j; + ++jstar; + } + } + + log && log("keep", matrix(keep)); + + return keep; + } /*make_kept_index*/ + + void + KalmanFilterInput::reflect_self() + { + StructReflector 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 << ""; + } /*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 */ diff --git a/KalmanFilterInput.hpp b/KalmanFilterInput.hpp new file mode 100644 index 00000000..a73a72d0 --- /dev/null +++ b/KalmanFilterInput.hpp @@ -0,0 +1,121 @@ +/* @file KalmanFilterInput.hpp */ + +#pragma once + +#include "reflect/SelfTagging.hpp" +#include "time/Time.hpp" +#include "refcnt/Refcounted.hpp" +#include +#include + +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; + using VectorXi = Eigen::VectorXi; + using VectorXd = Eigen::VectorXd; + using uint32_t = std::uint32_t; + + public: + KalmanFilterInput() = default; + + static ref::rp 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 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; + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterInput const & x) + { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInput.hpp */ diff --git a/KalmanFilterInputCallback.hpp b/KalmanFilterInputCallback.hpp new file mode 100644 index 00000000..d9b2f213 --- /dev/null +++ b/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>; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInputCallback.hpp */ diff --git a/KalmanFilterInputSource.hpp b/KalmanFilterInputSource.hpp new file mode 100644 index 00000000..30c5cbf1 --- /dev/null +++ b/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 src = ...; + * rp 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 */ + diff --git a/KalmanFilterInputToConsole.cpp b/KalmanFilterInputToConsole.cpp new file mode 100644 index 00000000..a0a848ca --- /dev/null +++ b/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::make() { + return new KalmanFilterInputToConsole(); + } /*make*/ + + void + KalmanFilterInputToConsole::display(std::ostream & os) const + { + os << ""; + } /*display*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInputToConsole.cpp */ diff --git a/KalmanFilterInputToConsole.hpp b/KalmanFilterInputToConsole.hpp new file mode 100644 index 00000000..7d48ef7e --- /dev/null +++ b/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> + { + public: + KalmanFilterInputToConsole() = default; + + static ref::rp 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 */ diff --git a/KalmanFilterObservable.cpp b/KalmanFilterObservable.cpp new file mode 100644 index 00000000..90f31460 --- /dev/null +++ b/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"; + } /*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 */ diff --git a/KalmanFilterObservable.hpp b/KalmanFilterObservable.hpp new file mode 100644 index 00000000..87c4f9d1 --- /dev/null +++ b/KalmanFilterObservable.hpp @@ -0,0 +1,110 @@ +/* @file KalmanFilterObservable.hpp */ + +#pragma once + +#include "time/Time.hpp" +#include +#include + +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 */ + diff --git a/KalmanFilterOutputCallback.hpp b/KalmanFilterOutputCallback.hpp new file mode 100644 index 00000000..69fb8484 --- /dev/null +++ b/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>; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterOutputCallback.hpp */ diff --git a/KalmanFilterSpec.cpp b/KalmanFilterSpec.cpp new file mode 100644 index 00000000..296aeb99 --- /dev/null +++ b/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 << ""; + } /*display*/ + + std::string + KalmanFilterSpec::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterSpec.cpp */ diff --git a/KalmanFilterSpec.hpp b/KalmanFilterSpec.hpp new file mode 100644 index 00000000..56ffd783 --- /dev/null +++ b/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 const & sk, + KalmanFilterInputPtr const & zkp1)>; + + public: + explicit KalmanFilterSpec(ref::rp s0, MkStepFn mkstepfn) + : start_ext_{std::move(s0)}, + mk_step_fn_{std::move(mkstepfn)} {} + + ref::rp 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 const & sk, + ref::rp 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 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 */ diff --git a/KalmanFilterState.cpp b/KalmanFilterState.cpp new file mode 100644 index 00000000..7656908c --- /dev/null +++ b/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 +#include + +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::make() + { + return new KalmanFilterState(); + } /*make*/ + + rp + 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 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::make() + { + return new KalmanFilterStateExt(); + } /*make*/ + + rp + KalmanFilterStateExt::make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + rp 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 sr; + + if (sr.is_incomplete()) { + /* TODO: use sr.adopt_ancestors() */ + + 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 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 << ""; + } /*display*/ + + std::string + KalmanFilterState::display_string() const + { + std::stringstream ss; + ss << *this; + return ss.str(); + } /*display_string*/ + + // ----- KalmanFilterStateExt ----- + + ref::rp + 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 << "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 */ diff --git a/KalmanFilterState.hpp b/KalmanFilterState.hpp new file mode 100644 index 00000000..113c4c6b --- /dev/null +++ b/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 +#include +#include + +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 make(); + static ref::rp 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 make(); + static ref::rp make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + ref::rp zk); + + /* create state object for initial filter state */ + static ref::rp 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 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 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 zk_; + }; /*KalamnFilterStateExt*/ + } /*namespace filter*/ +} /*namespace xo*/ + +/* end KalmanFilterState.hpp */ diff --git a/KalmanFilterStateToConsole.cpp b/KalmanFilterStateToConsole.cpp new file mode 100644 index 00000000..8e76ce84 --- /dev/null +++ b/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::make() { + return new KalmanFilterStateToConsole(); + } /*make*/ + + void + KalmanFilterStateToConsole::display(std::ostream & os) const + { + os << ""; + } /*display*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterStateToConsole.cpp */ diff --git a/KalmanFilterStateToConsole.hpp b/KalmanFilterStateToConsole.hpp new file mode 100644 index 00000000..0d525cea --- /dev/null +++ b/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 + { + public: + KalmanFilterStateToConsole() = default; + + static ref::rp 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 */ diff --git a/KalmanFilterStep.cpp b/KalmanFilterStep.cpp new file mode 100644 index 00000000..f62f2d21 --- /dev/null +++ b/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 + KalmanFilterStep::extrapolate() const + { + return KalmanFilterEngine::extrapolate(this->tkp1(), + this->state(), + this->model() /*transition*/); + } /*extrapolate*/ + + MatrixXd + KalmanFilterStep::gain(ref::rp const & skp1_ext) const + { + return KalmanFilterEngine::kalman_gain(skp1_ext, + this->obs()); + } /*gain*/ + + VectorXd + KalmanFilterStep::gain1(ref::rp const & skp1_ext, + uint32_t j) const + { + return KalmanFilterEngine::kalman_gain1(skp1_ext, + this->obs(), + j); + + } /*gain1*/ + + ref::rp + KalmanFilterStep::correct(ref::rp const & skp1_ext) + { + return KalmanFilterEngine::correct(skp1_ext, + this->obs(), + this->input()); + } /*correct*/ + + ref::rp + KalmanFilterStep::correct1(ref::rp 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 << "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 */ diff --git a/KalmanFilterStep.hpp b/KalmanFilterStep.hpp new file mode 100644 index 00000000..89813cc2 --- /dev/null +++ b/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 state, + KalmanFilterTransition model, + KalmanFilterObservable obs, + ref::rp zkp1) + : KalmanFilterStepBase(model, obs), + state_{std::move(state)}, + input_{std::move(zkp1)} {} + + ref::rp const & state() const { return state_; } + ref::rp 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 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 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 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 correct(ref::rp 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 correct1(ref::rp 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 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 */ diff --git a/KalmanFilterSvc.cpp b/KalmanFilterSvc.cpp new file mode 100644 index 00000000..5e32b102 --- /dev/null +++ b/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::make(KalmanFilterSpec spec) + { + return new KalmanFilterSvc(std::move(spec)); + } /*make*/ + + KalmanFilterSvc::KalmanFilterSvc(KalmanFilterSpec spec) + : filter_{std::move(spec)} + {} + + void + KalmanFilterSvc::notify_ev(ref::rp 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 << "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 */ diff --git a/KalmanFilterSvc.hpp b/KalmanFilterSvc.hpp new file mode 100644 index 00000000..33d25514 --- /dev/null +++ b/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>, + public xo::reactor::DirectSourcePtr> { + public: + using AbstractSource = xo::reactor::AbstractSource; + + public: + /* named ctor idiom */ + static ref::rp make(KalmanFilterSpec spec); + + KalmanFilter const & filter() const { return filter_; } + + /* notify incoming observations; will trigger kalman filter step */ + void notify_ev(ref::rp 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 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 pub_; + }; /*KalmanFilterSvc*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* KalmanFilterSvc.hpp */ diff --git a/KalmanFilterTransition.cpp b/KalmanFilterTransition.cpp new file mode 100644 index 00000000..65be627b --- /dev/null +++ b/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 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 << ""; + } /*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 */ diff --git a/KalmanFilterTransition.hpp b/KalmanFilterTransition.hpp new file mode 100644 index 00000000..fc777c00 --- /dev/null +++ b/KalmanFilterTransition.hpp @@ -0,0 +1,62 @@ +/* @file KalmanFilterTransition.hpp */ + +#pragma once + +#include "time/Time.hpp" +#include +#include + +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 */ diff --git a/init_filter.cpp b/init_filter.cpp new file mode 100644 index 00000000..80407cc4 --- /dev/null +++ b/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::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::require() + { + InitEvidence retval; + + /* subsystem dependencies for filter/ */ + retval ^= InitSubsys::require(); + + /* filter/'s own initialization code */ + retval ^= Subsystem::provide("filter", &init); + + return retval; + } /*require*/ +} /*namespace xo*/ + +/* end init_filter.cpp */ diff --git a/init_filter.hpp b/init_filter.hpp new file mode 100644 index 00000000..79cfc6d0 --- /dev/null +++ b/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 { + static void init(); + static InitEvidence require(); + }; +} /*namespace xo*/ + +/* end init_filter.hpp */ diff --git a/print_eigen.hpp b/print_eigen.hpp new file mode 100644 index 00000000..953c5904 --- /dev/null +++ b/print_eigen.hpp @@ -0,0 +1,41 @@ +/* @file print_eigen.hpp */ + +#include +#include + +namespace logutil { + template + class matrix { + public: + matrix(T x) : x_{std::move(x)} {} + + /* print this value */ + T x_; + }; /*matrix*/ + + template + using vector = matrix; + + template + inline std::ostream & + operator<<(std::ostream & s, matrix const & mat) + { + s << "["; + for(std::uint32_t i = 0, m = mat.x_.rows(); i 0) + s << "; "; + + for(std::uint32_t j = 0, n = mat.x_.cols(); j 0) + s << ' '; + + s << mat.x_(i, j); + } + } + s << "]"; + + return s; + } /*operator<<*/ +} /*namespace logutil*/ + +/* end print_eigen.hpp */ From e1f39b7b0de413a6522905a971f843e9cee91dcb Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Mon, 23 Oct 2023 11:11:56 -0400 Subject: [PATCH 02/25] initial implementation --- CMakeLists.txt | 47 ++ EigenUtil.cpp | 157 ---- KalmanFilter.cpp | 78 -- KalmanFilter.hpp | 128 ---- KalmanFilterEngine.cpp | 360 --------- KalmanFilterEngine.hpp | 185 ----- KalmanFilterInput.cpp | 118 --- KalmanFilterInput.hpp | 121 --- KalmanFilterInputCallback.hpp | 14 - KalmanFilterInputSource.hpp | 28 - KalmanFilterInputToConsole.hpp | 30 - KalmanFilterObservable.hpp | 110 --- KalmanFilterOutputCallback.hpp | 15 - KalmanFilterSpec.cpp | 27 - KalmanFilterSpec.hpp | 86 --- KalmanFilterState.cpp | 231 ------ KalmanFilterState.hpp | 163 ----- KalmanFilterStateToConsole.hpp | 30 - KalmanFilterStep.cpp | 84 --- KalmanFilterSvc.hpp | 64 -- KalmanFilterTransition.cpp | 55 -- KalmanFilterTransition.hpp | 62 -- cmake/xo_kalmanfilterConfig.cmake.in | 17 + .../xo/kalmanfilter/EigenUtil.hpp | 0 include/xo/kalmanfilter/KalmanFilter.hpp | 127 ++++ .../xo/kalmanfilter/KalmanFilterEngine.hpp | 185 +++++ include/xo/kalmanfilter/KalmanFilterInput.hpp | 121 +++ .../KalmanFilterInputCallback.hpp | 14 + .../kalmanfilter/KalmanFilterInputSource.hpp | 27 + .../KalmanFilterInputToConsole.hpp | 30 + .../kalmanfilter/KalmanFilterObservable.hpp | 109 +++ .../KalmanFilterOutputCallback.hpp | 15 + include/xo/kalmanfilter/KalmanFilterSpec.hpp | 86 +++ include/xo/kalmanfilter/KalmanFilterState.hpp | 163 +++++ .../KalmanFilterStateToConsole.hpp | 30 + .../xo/kalmanfilter/KalmanFilterStep.hpp | 0 include/xo/kalmanfilter/KalmanFilterSvc.hpp | 64 ++ .../kalmanfilter/KalmanFilterTransition.hpp | 62 ++ include/xo/kalmanfilter/init_filter.hpp | 20 + include/xo/kalmanfilter/print_eigen.hpp | 41 ++ init_filter.cpp | 51 -- init_filter.hpp | 20 - print_eigen.hpp | 41 -- src/kalmanfilter/CMakeLists.txt | 31 + src/kalmanfilter/EigenUtil.cpp | 157 ++++ src/kalmanfilter/KalmanFilter.cpp | 78 ++ src/kalmanfilter/KalmanFilterEngine.cpp | 360 +++++++++ src/kalmanfilter/KalmanFilterInput.cpp | 118 +++ .../KalmanFilterInputToConsole.cpp | 2 +- .../kalmanfilter/KalmanFilterObservable.cpp | 2 +- src/kalmanfilter/KalmanFilterSpec.cpp | 27 + src/kalmanfilter/KalmanFilterState.cpp | 231 ++++++ .../KalmanFilterStateToConsole.cpp | 2 +- src/kalmanfilter/KalmanFilterStep.cpp | 84 +++ .../kalmanfilter/KalmanFilterSvc.cpp | 0 src/kalmanfilter/KalmanFilterTransition.cpp | 55 ++ src/kalmanfilter/init_filter.cpp | 52 ++ utest/CMakeLists.txt | 54 ++ utest/KalmanFilter.test.cpp | 690 ++++++++++++++++++ utest/filter_utest_main.cpp | 6 + 60 files changed, 3104 insertions(+), 2261 deletions(-) create mode 100644 CMakeLists.txt delete mode 100644 EigenUtil.cpp delete mode 100644 KalmanFilter.cpp delete mode 100644 KalmanFilter.hpp delete mode 100644 KalmanFilterEngine.cpp delete mode 100644 KalmanFilterEngine.hpp delete mode 100644 KalmanFilterInput.cpp delete mode 100644 KalmanFilterInput.hpp delete mode 100644 KalmanFilterInputCallback.hpp delete mode 100644 KalmanFilterInputSource.hpp delete mode 100644 KalmanFilterInputToConsole.hpp delete mode 100644 KalmanFilterObservable.hpp delete mode 100644 KalmanFilterOutputCallback.hpp delete mode 100644 KalmanFilterSpec.cpp delete mode 100644 KalmanFilterSpec.hpp delete mode 100644 KalmanFilterState.cpp delete mode 100644 KalmanFilterState.hpp delete mode 100644 KalmanFilterStateToConsole.hpp delete mode 100644 KalmanFilterStep.cpp delete mode 100644 KalmanFilterSvc.hpp delete mode 100644 KalmanFilterTransition.cpp delete mode 100644 KalmanFilterTransition.hpp create mode 100644 cmake/xo_kalmanfilterConfig.cmake.in rename EigenUtil.hpp => include/xo/kalmanfilter/EigenUtil.hpp (100%) create mode 100644 include/xo/kalmanfilter/KalmanFilter.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterEngine.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterInput.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterInputCallback.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterInputSource.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterObservable.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterSpec.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterState.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp rename KalmanFilterStep.hpp => include/xo/kalmanfilter/KalmanFilterStep.hpp (100%) create mode 100644 include/xo/kalmanfilter/KalmanFilterSvc.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterTransition.hpp create mode 100644 include/xo/kalmanfilter/init_filter.hpp create mode 100644 include/xo/kalmanfilter/print_eigen.hpp delete mode 100644 init_filter.cpp delete mode 100644 init_filter.hpp delete mode 100644 print_eigen.hpp create mode 100644 src/kalmanfilter/CMakeLists.txt create mode 100644 src/kalmanfilter/EigenUtil.cpp create mode 100644 src/kalmanfilter/KalmanFilter.cpp create mode 100644 src/kalmanfilter/KalmanFilterEngine.cpp create mode 100644 src/kalmanfilter/KalmanFilterInput.cpp rename KalmanFilterInputToConsole.cpp => src/kalmanfilter/KalmanFilterInputToConsole.cpp (93%) rename KalmanFilterObservable.cpp => src/kalmanfilter/KalmanFilterObservable.cpp (98%) create mode 100644 src/kalmanfilter/KalmanFilterSpec.cpp create mode 100644 src/kalmanfilter/KalmanFilterState.cpp rename KalmanFilterStateToConsole.cpp => src/kalmanfilter/KalmanFilterStateToConsole.cpp (93%) create mode 100644 src/kalmanfilter/KalmanFilterStep.cpp rename KalmanFilterSvc.cpp => src/kalmanfilter/KalmanFilterSvc.cpp (100%) create mode 100644 src/kalmanfilter/KalmanFilterTransition.cpp create mode 100644 src/kalmanfilter/init_filter.cpp create mode 100644 utest/CMakeLists.txt create mode 100644 utest/KalmanFilter.test.cpp create mode 100644 utest/filter_utest_main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..f4120636 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,47 @@ +# xo-kalmanfilter/CMakeLists.txt + +cmake_minimum_required(VERSION 3.10) + +project(xo_kalmanfilter VERSION 1.0) +enable_language(CXX) + +# common XO cmake macros (see proj/xo-cmake) +include(xo_macros/xo_cxx) +include(xo_macros/code-coverage) + +# ---------------------------------------------------------------- +# unit test setup + +enable_testing() +# activate code coverage for all executables + libraries (when configured with -DCODE_COVERAGE=ON) +add_code_coverage() +# 1. assuming that /nix/store/ prefixes .hpp files belonging to gcc, catch2 etc. +# we're not interested in code coverage for these sources. +# 2. exclude the utest/ subdir, we don't need coverage on the unit tests themselves; +# rather, want coverage on the code that the unit tests exercise. +# +# NOTE: this seems to work only with the 'ccov-all' target. In particular, doesn't seem to do anything with the 'ccov' target +# +add_code_coverage_all_targets(EXCLUDE /nix/store/* ${PROJECT_SOURCE_DIR}/utest/* ${PROJECT_BINARY_DIR}/local/* ${PROJECT_SOURCE_DIR}/repo/*) + +# ---------------------------------------------------------------- +# c++ settings + +# one-time project-specific c++ flags. usually empty +set(PROJECT_CXX_FLAGS "") +#set(PROJECT_CXX_FLAGS "-fconcepts-diagnostics-depth=2") +add_definitions(${PROJECT_CXX_FLAGS}) + +xo_toplevel_compile_options() + +# ---------------------------------------------------------------- + +add_subdirectory(src/kalmanfilter) +add_subdirectory(utest) + +# ---------------------------------------------------------------- +# provide find_package() support for reactor customers + +xo_export_cmake_config(${PROJECT_NAME} ${PROJECT_VERSION} ${PROJECT_NAME}Targets) + +# end CMakeLists.txt diff --git a/EigenUtil.cpp b/EigenUtil.cpp deleted file mode 100644 index f1d3c4a3..00000000 --- a/EigenUtil.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/* file EigenUtil.cpp - * - * author: Roland Conybeare, Sep 2022 - */ - -#include "EigenUtil.hpp" -#include "printjson/PrintJson.hpp" -#include "reflect/Reflect.hpp" -#include -#include -#include -#include - -namespace xo { - using xo::json::PrintJson; - using xo::json::JsonPrinter; - using xo::reflect::Reflect; - using xo::reflect::TypeDescr; - using VectorXb = Eigen::Array; - using Eigen::VectorXd; - using Eigen::MatrixXd; - -#ifdef NOT_YET - namespace reflect { - template - using EigenVectorX_Tdx = xo::reflect::StlVectorTdx>; - - /* probably need this to appear before decl for class xo::reflect::Reflect */ - template - class EstablishTdx> { - public: - static std::unique_ptr make() { - return EigenVectorX_Tdx::make(); - } /*make*/ - }; /*EstablishTdx*/ - } /*reflect*/ -#endif - - namespace eigen { - - namespace { - /* prints a VectorXd as json, in the obvious format, e.g. - * [1,2,3] - */ - template - 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(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(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 0) - *p_os << ", "; - *p_os << "["; - for(std::uint32_t j=0, n=pm->cols(); 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 - void - provide_eigen_vector_printer(PrintJson * p_pjson) - { - TypeDescr td = Reflect::require(); - std::unique_ptr pr(new EigenVectorJsonPrinter(p_pjson)); - - p_pjson->provide_printer(td, std::move(pr)); - } /*provide_eigen_vector_printer*/ - } /*namespace*/ - - void - EigenUtil::reflect_eigen() - { -#ifdef NOT_YET - Reflect::require(); - Reflect::require(); -#endif - } /*reflect_eigen*/ - - void - EigenUtil::provide_json_printers(PrintJson * p_pjson) - { - assert(p_pjson); - - provide_eigen_vector_printer(p_pjson); - provide_eigen_vector_printer(p_pjson); - - { - TypeDescr td = Reflect::require(); - std::unique_ptr pr(new MatrixXdJsonPrinter(p_pjson)); - - p_pjson->provide_printer(td, std::move(pr)); - } - } /*provide_json_printers*/ - } /*namespace eigen*/ -} /*namespace xo*/ - -/* end EigenUtil.cpp */ diff --git a/KalmanFilter.cpp b/KalmanFilter.cpp deleted file mode 100644 index bf94fde2..00000000 --- a/KalmanFilter.cpp +++ /dev/null @@ -1,78 +0,0 @@ -/* @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 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 << ""; - } /*display*/ - - std::string - KalmanFilter::display_string() const - { - return tostr(*this); - } /*display_string*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilter.cpp */ diff --git a/KalmanFilter.hpp b/KalmanFilter.hpp deleted file mode 100644 index 295a4f7a..00000000 --- a/KalmanFilter.hpp +++ /dev/null @@ -1,128 +0,0 @@ -/* @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 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 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 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 */ diff --git a/KalmanFilterEngine.cpp b/KalmanFilterEngine.cpp deleted file mode 100644 index e80f7497..00000000 --- a/KalmanFilterEngine.cpp +++ /dev/null @@ -1,360 +0,0 @@ -/* @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 - KalmanFilterEngine::extrapolate(utc_nanos tkp1, - ref::rp 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 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 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 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 - KalmanFilterEngine::correct1(ref::rp const & skp1_ext, - KalmanFilterObservable const & h, - ref::rp 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 - KalmanFilterEngine::correct(ref::rp const & skp1_ext, - KalmanFilterObservable const & h, - ref::rp 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 - KalmanFilterEngine::step(utc_nanos tkp1, - ref::rp const & sk, - KalmanFilterTransition const & Fk, - KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1) - { - ref::rp skp1_ext - = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); - - ref::rp skp1 - = KalmanFilterEngine::correct(skp1_ext, Hkp1, zkp1); - - return skp1; - } /*step*/ - - ref::rp - 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 - KalmanFilterEngine::step1(utc_nanos tkp1, - ref::rp const & sk, - KalmanFilterTransition const & Fk, - KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1, - uint32_t j) - { - ref::rp skp1_ext - = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); - - ref::rp skp1 - = KalmanFilterEngine::correct1(skp1_ext, Hkp1, zkp1, j); - - return skp1; - } /*step1*/ - - ref::rp - 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 */ diff --git a/KalmanFilterEngine.hpp b/KalmanFilterEngine.hpp deleted file mode 100644 index b8c2f949..00000000 --- a/KalmanFilterEngine.hpp +++ /dev/null @@ -1,185 +0,0 @@ -/* @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 extrapolate(utc_nanos tkp1, - ref::rp 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 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 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 correct(ref::rp const & skp1_ext, - KalmanFilterObservable const & Hkp1, - ref::rp 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 correct1(ref::rp const & skp1_ext, - KalmanFilterObservable const & Hkp1, - ref::rp 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 step(utc_nanos tkp1, - ref::rp const & sk, - KalmanFilterTransition const & Fk, - KalmanFilterObservable const & Hkp1, - ref::rp 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 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 step1(utc_nanos tkp1, - ref::rp const & sk, - KalmanFilterTransition const & Fk, - KalmanFilterObservable const & Hkp1, - ref::rp 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 step1(KalmanFilterStep const & step_spec, - uint32_t j); - }; /*KalmanFilterEngine*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterEngine.hpp */ diff --git a/KalmanFilterInput.cpp b/KalmanFilterInput.cpp deleted file mode 100644 index 03b5330f..00000000 --- a/KalmanFilterInput.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/* @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::make(utc_nanos tkp1, - VectorXb const & presence, - VectorXd const & z, - VectorXd const & Rd) - { - return new KalmanFilterInput(tkp1, presence, z, Rd); - } /*make*/ - - ref::rp - 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(); jpresence_[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(); jpresence_[j]) { - keep[jstar] = j; - ++jstar; - } - } - - log && log("keep", matrix(keep)); - - return keep; - } /*make_kept_index*/ - - void - KalmanFilterInput::reflect_self() - { - StructReflector 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 << ""; - } /*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 */ diff --git a/KalmanFilterInput.hpp b/KalmanFilterInput.hpp deleted file mode 100644 index a73a72d0..00000000 --- a/KalmanFilterInput.hpp +++ /dev/null @@ -1,121 +0,0 @@ -/* @file KalmanFilterInput.hpp */ - -#pragma once - -#include "reflect/SelfTagging.hpp" -#include "time/Time.hpp" -#include "refcnt/Refcounted.hpp" -#include -#include - -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; - using VectorXi = Eigen::VectorXi; - using VectorXd = Eigen::VectorXd; - using uint32_t = std::uint32_t; - - public: - KalmanFilterInput() = default; - - static ref::rp 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 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; - - inline std::ostream & - operator<<(std::ostream & os, KalmanFilterInput const & x) - { - x.display(os); - return os; - } /*operator<<*/ - - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterInput.hpp */ diff --git a/KalmanFilterInputCallback.hpp b/KalmanFilterInputCallback.hpp deleted file mode 100644 index d9b2f213..00000000 --- a/KalmanFilterInputCallback.hpp +++ /dev/null @@ -1,14 +0,0 @@ -/* @file KalmanFilterInputCallback.hpp */ - -#pragma once - -#include "refcnt/Refcounted.hpp" -#include "filter/KalmanFilter.hpp" - -namespace xo { - namespace kalman { - using KalmanFilterInputCallback = reactor::Sink1>; - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterInputCallback.hpp */ diff --git a/KalmanFilterInputSource.hpp b/KalmanFilterInputSource.hpp deleted file mode 100644 index 30c5cbf1..00000000 --- a/KalmanFilterInputSource.hpp +++ /dev/null @@ -1,28 +0,0 @@ -/* @file KalmanFilterInputSource.hpp */ - -#pragma once - -#include "reactor/EventSource.hpp" -#include "filter/KalmanFilterInputCallback.hpp" - -namespace xo { - namespace kalman { - /* Use: - * rp src = ...; - * rp 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 */ - diff --git a/KalmanFilterInputToConsole.hpp b/KalmanFilterInputToConsole.hpp deleted file mode 100644 index 7d48ef7e..00000000 --- a/KalmanFilterInputToConsole.hpp +++ /dev/null @@ -1,30 +0,0 @@ -/* @file KalmanFilterInputToConsole.hpp */ - -#pragma once - -#include "reactor/Sink.hpp" -#include "filter/KalmanFilterInput.hpp" - -namespace xo { - namespace kalman { - class KalmanFilterInputToConsole - : public xo::reactor::SinkToConsole> - { - public: - KalmanFilterInputToConsole() = default; - - static ref::rp 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 */ diff --git a/KalmanFilterObservable.hpp b/KalmanFilterObservable.hpp deleted file mode 100644 index 87c4f9d1..00000000 --- a/KalmanFilterObservable.hpp +++ /dev/null @@ -1,110 +0,0 @@ -/* @file KalmanFilterObservable.hpp */ - -#pragma once - -#include "time/Time.hpp" -#include -#include - -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 */ - diff --git a/KalmanFilterOutputCallback.hpp b/KalmanFilterOutputCallback.hpp deleted file mode 100644 index 69fb8484..00000000 --- a/KalmanFilterOutputCallback.hpp +++ /dev/null @@ -1,15 +0,0 @@ -/* @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>; - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterOutputCallback.hpp */ diff --git a/KalmanFilterSpec.cpp b/KalmanFilterSpec.cpp deleted file mode 100644 index 296aeb99..00000000 --- a/KalmanFilterSpec.cpp +++ /dev/null @@ -1,27 +0,0 @@ -/* @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 << ""; - } /*display*/ - - std::string - KalmanFilterSpec::display_string() const - { - return tostr(*this); - } /*display_string*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterSpec.cpp */ diff --git a/KalmanFilterSpec.hpp b/KalmanFilterSpec.hpp deleted file mode 100644 index 56ffd783..00000000 --- a/KalmanFilterSpec.hpp +++ /dev/null @@ -1,86 +0,0 @@ -/* @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 const & sk, - KalmanFilterInputPtr const & zkp1)>; - - public: - explicit KalmanFilterSpec(ref::rp s0, MkStepFn mkstepfn) - : start_ext_{std::move(s0)}, - mk_step_fn_{std::move(mkstepfn)} {} - - ref::rp 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 const & sk, - ref::rp 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 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 */ diff --git a/KalmanFilterState.cpp b/KalmanFilterState.cpp deleted file mode 100644 index 7656908c..00000000 --- a/KalmanFilterState.cpp +++ /dev/null @@ -1,231 +0,0 @@ -/* @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 -#include - -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::make() - { - return new KalmanFilterState(); - } /*make*/ - - rp - 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 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::make() - { - return new KalmanFilterStateExt(); - } /*make*/ - - rp - KalmanFilterStateExt::make(uint32_t k, - utc_nanos tk, - VectorXd x, - MatrixXd P, - KalmanFilterTransition transition, - MatrixXd K, - int32_t j, - rp 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 sr; - - if (sr.is_incomplete()) { - /* TODO: use sr.adopt_ancestors() */ - - 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 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 << ""; - } /*display*/ - - std::string - KalmanFilterState::display_string() const - { - std::stringstream ss; - ss << *this; - return ss.str(); - } /*display_string*/ - - // ----- KalmanFilterStateExt ----- - - ref::rp - 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 << "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 */ diff --git a/KalmanFilterState.hpp b/KalmanFilterState.hpp deleted file mode 100644 index 113c4c6b..00000000 --- a/KalmanFilterState.hpp +++ /dev/null @@ -1,163 +0,0 @@ -/* @file KalmanFilterState.hpp */ - -#pragma once - -#include "reflect/SelfTagging.hpp" -#include "filter/KalmanFilterInput.hpp" -#include "filter/KalmanFilterTransition.hpp" -#include "time/Time.hpp" -#include -#include -#include - -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 make(); - static ref::rp 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 make(); - static ref::rp make(uint32_t k, - utc_nanos tk, - VectorXd x, - MatrixXd P, - KalmanFilterTransition transition, - MatrixXd K, - int32_t j, - ref::rp zk); - - /* create state object for initial filter state */ - static ref::rp 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 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 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 zk_; - }; /*KalamnFilterStateExt*/ - } /*namespace filter*/ -} /*namespace xo*/ - -/* end KalmanFilterState.hpp */ diff --git a/KalmanFilterStateToConsole.hpp b/KalmanFilterStateToConsole.hpp deleted file mode 100644 index 0d525cea..00000000 --- a/KalmanFilterStateToConsole.hpp +++ /dev/null @@ -1,30 +0,0 @@ -/* @file KalmanFilterStateToConsole.hpp */ - -#pragma once - -#include "reactor/Sink.hpp" -#include "filter/KalmanFilterState.hpp" - -namespace xo { - namespace kalman { - class KalmanFilterStateToConsole - : public xo::reactor::SinkToConsole - { - public: - KalmanFilterStateToConsole() = default; - - static ref::rp 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 */ diff --git a/KalmanFilterStep.cpp b/KalmanFilterStep.cpp deleted file mode 100644 index f62f2d21..00000000 --- a/KalmanFilterStep.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/* @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 - KalmanFilterStep::extrapolate() const - { - return KalmanFilterEngine::extrapolate(this->tkp1(), - this->state(), - this->model() /*transition*/); - } /*extrapolate*/ - - MatrixXd - KalmanFilterStep::gain(ref::rp const & skp1_ext) const - { - return KalmanFilterEngine::kalman_gain(skp1_ext, - this->obs()); - } /*gain*/ - - VectorXd - KalmanFilterStep::gain1(ref::rp const & skp1_ext, - uint32_t j) const - { - return KalmanFilterEngine::kalman_gain1(skp1_ext, - this->obs(), - j); - - } /*gain1*/ - - ref::rp - KalmanFilterStep::correct(ref::rp const & skp1_ext) - { - return KalmanFilterEngine::correct(skp1_ext, - this->obs(), - this->input()); - } /*correct*/ - - ref::rp - KalmanFilterStep::correct1(ref::rp 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 << "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 */ diff --git a/KalmanFilterSvc.hpp b/KalmanFilterSvc.hpp deleted file mode 100644 index 33d25514..00000000 --- a/KalmanFilterSvc.hpp +++ /dev/null @@ -1,64 +0,0 @@ -/* @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>, - public xo::reactor::DirectSourcePtr> { - public: - using AbstractSource = xo::reactor::AbstractSource; - - public: - /* named ctor idiom */ - static ref::rp make(KalmanFilterSpec spec); - - KalmanFilter const & filter() const { return filter_; } - - /* notify incoming observations; will trigger kalman filter step */ - void notify_ev(ref::rp 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 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 pub_; - }; /*KalmanFilterSvc*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* KalmanFilterSvc.hpp */ diff --git a/KalmanFilterTransition.cpp b/KalmanFilterTransition.cpp deleted file mode 100644 index 65be627b..00000000 --- a/KalmanFilterTransition.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/* @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 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 << ""; - } /*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 */ diff --git a/KalmanFilterTransition.hpp b/KalmanFilterTransition.hpp deleted file mode 100644 index fc777c00..00000000 --- a/KalmanFilterTransition.hpp +++ /dev/null @@ -1,62 +0,0 @@ -/* @file KalmanFilterTransition.hpp */ - -#pragma once - -#include "time/Time.hpp" -#include -#include - -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 */ diff --git a/cmake/xo_kalmanfilterConfig.cmake.in b/cmake/xo_kalmanfilterConfig.cmake.in new file mode 100644 index 00000000..366e45ba --- /dev/null +++ b/cmake/xo_kalmanfilterConfig.cmake.in @@ -0,0 +1,17 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) + +# note: changes to find_dependency() calls here +# must coordinate with xo_dependency() calls +# in xo-reactor/src/reactor/CMakeLists.txt +# +find_dependency(reactor) +find_dependnecy(eigen3) +#find_dependency(reflect) +#find_dependency(webutil) +#find_dependency(printjson) +#find_dependency(callback) + +include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") +check_required_components("@PROJECT_NAME@") diff --git a/EigenUtil.hpp b/include/xo/kalmanfilter/EigenUtil.hpp similarity index 100% rename from EigenUtil.hpp rename to include/xo/kalmanfilter/EigenUtil.hpp diff --git a/include/xo/kalmanfilter/KalmanFilter.hpp b/include/xo/kalmanfilter/KalmanFilter.hpp new file mode 100644 index 00000000..0df87a61 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilter.hpp @@ -0,0 +1,127 @@ +/* @file KalmanFilter.hpp */ + +#pragma once + +#include "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 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 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 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 */ diff --git a/include/xo/kalmanfilter/KalmanFilterEngine.hpp b/include/xo/kalmanfilter/KalmanFilterEngine.hpp new file mode 100644 index 00000000..ce21bf3c --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterEngine.hpp @@ -0,0 +1,185 @@ +/* @file KalmanFilterEngine.hpp */ + +#pragma once + +#include "KalmanFilterStep.hpp" +#include "KalmanFilterState.hpp" +#include "KalmanFilterTransition.hpp" +#include "KalmanFilterObservable.hpp" +#include "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 extrapolate(utc_nanos tkp1, + ref::rp 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 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 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 correct(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + ref::rp 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 correct1(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + ref::rp 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 step(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp 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 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 step1(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp 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 step1(KalmanFilterStep const & step_spec, + uint32_t j); + }; /*KalmanFilterEngine*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterEngine.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterInput.hpp b/include/xo/kalmanfilter/KalmanFilterInput.hpp new file mode 100644 index 00000000..70fee5af --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterInput.hpp @@ -0,0 +1,121 @@ +/* @file KalmanFilterInput.hpp */ + +#pragma once + +#include "xo/reflect/SelfTagging.hpp" +//#include "time/Time.hpp" +//#include "xo/refcnt/Refcounted.hpp" +#include +#include + +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; + using VectorXi = Eigen::VectorXi; + using VectorXd = Eigen::VectorXd; + using uint32_t = std::uint32_t; + + public: + KalmanFilterInput() = default; + + static ref::rp 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 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; + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterInput const & x) + { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInput.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp b/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp new file mode 100644 index 00000000..df86a16b --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp @@ -0,0 +1,14 @@ +/* @file KalmanFilterInputCallback.hpp */ + +#pragma once + +#include "xo/refcnt/Refcounted.hpp" +#include "KalmanFilter.hpp" + +namespace xo { + namespace kalman { + using KalmanFilterInputCallback = reactor::Sink1>; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInputCallback.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterInputSource.hpp b/include/xo/kalmanfilter/KalmanFilterInputSource.hpp new file mode 100644 index 00000000..7e801936 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterInputSource.hpp @@ -0,0 +1,27 @@ +/* @file KalmanFilterInputSource.hpp */ + +#pragma once + +#include "xo/reactor/EventSource.hpp" +#include "KalmanFilterInputCallback.hpp" + +namespace xo { + namespace kalman { + /* Use: + * rp src = ...; + * rp 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 */ diff --git a/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp b/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp new file mode 100644 index 00000000..7d81226a --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp @@ -0,0 +1,30 @@ +/* @file KalmanFilterInputToConsole.hpp */ + +#pragma once + +#include "xo/reactor/Sink.hpp" +#include "KalmanFilterInput.hpp" + +namespace xo { + namespace kalman { + class KalmanFilterInputToConsole + : public xo::reactor::SinkToConsole> + { + public: + KalmanFilterInputToConsole() = default; + + static ref::rp 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 */ diff --git a/include/xo/kalmanfilter/KalmanFilterObservable.hpp b/include/xo/kalmanfilter/KalmanFilterObservable.hpp new file mode 100644 index 00000000..c3981095 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterObservable.hpp @@ -0,0 +1,109 @@ +/* @file KalmanFilterObservable.hpp */ + +#pragma once + +//#include "time/Time.hpp" +#include +#include + +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 */ diff --git a/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp b/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp new file mode 100644 index 00000000..cd858606 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp @@ -0,0 +1,15 @@ +/* @file KalmanFilterOutputCallback.hpp */ + +#pragma once + +#include "xo/reactor/Sink.hpp" +#include "KalmanFilter.hpp" + +namespace xo { + namespace kalman { + /* callback for consuming kalman filter output */ + using KalmanFilterOutputCallback = reactor::Sink1>; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterOutputCallback.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterSpec.hpp b/include/xo/kalmanfilter/KalmanFilterSpec.hpp new file mode 100644 index 00000000..306f298a --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterSpec.hpp @@ -0,0 +1,86 @@ +/* @file KalmanFilterSpec.hpp */ + +#pragma once + +#include "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 const & sk, + KalmanFilterInputPtr const & zkp1)>; + + public: + explicit KalmanFilterSpec(ref::rp s0, MkStepFn mkstepfn) + : start_ext_{std::move(s0)}, + mk_step_fn_{std::move(mkstepfn)} {} + + ref::rp 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 const & sk, + ref::rp 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 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 */ diff --git a/include/xo/kalmanfilter/KalmanFilterState.hpp b/include/xo/kalmanfilter/KalmanFilterState.hpp new file mode 100644 index 00000000..504def9c --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterState.hpp @@ -0,0 +1,163 @@ +/* @file KalmanFilterState.hpp */ + +#pragma once + +#include "xo/reflect/SelfTagging.hpp" +#include "KalmanFilterInput.hpp" +#include "KalmanFilterTransition.hpp" +//#include "time/Time.hpp" +#include +#include +#include + +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 make(); + static ref::rp 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 make(); + static ref::rp make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + ref::rp zk); + + /* create state object for initial filter state */ + static ref::rp 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 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 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 zk_; + }; /*KalamnFilterStateExt*/ + } /*namespace filter*/ +} /*namespace xo*/ + +/* end KalmanFilterState.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp b/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp new file mode 100644 index 00000000..9d072035 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp @@ -0,0 +1,30 @@ +/* @file KalmanFilterStateToConsole.hpp */ + +#pragma once + +#include "xo/reactor/Sink.hpp" +#include "KalmanFilterState.hpp" + +namespace xo { + namespace kalman { + class KalmanFilterStateToConsole + : public xo::reactor::SinkToConsole + { + public: + KalmanFilterStateToConsole() = default; + + static ref::rp 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 */ diff --git a/KalmanFilterStep.hpp b/include/xo/kalmanfilter/KalmanFilterStep.hpp similarity index 100% rename from KalmanFilterStep.hpp rename to include/xo/kalmanfilter/KalmanFilterStep.hpp diff --git a/include/xo/kalmanfilter/KalmanFilterSvc.hpp b/include/xo/kalmanfilter/KalmanFilterSvc.hpp new file mode 100644 index 00000000..94702a52 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterSvc.hpp @@ -0,0 +1,64 @@ +/* @file KalmanFilterSvc.hpp */ + +#include "xo/reactor/Sink.hpp" +#include "xo/reactor/DirectSourcePtr.hpp" +#include "KalmanFilter.hpp" +#include "KalmanFilterInputSource.hpp" +#include "KalmanFilterOutputCallback.hpp" +#include "xo/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>, + public xo::reactor::DirectSourcePtr> { + public: + using AbstractSource = xo::reactor::AbstractSource; + + public: + /* named ctor idiom */ + static ref::rp make(KalmanFilterSpec spec); + + KalmanFilter const & filter() const { return filter_; } + + /* notify incoming observations; will trigger kalman filter step */ + void notify_ev(ref::rp 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 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 pub_; + }; /*KalmanFilterSvc*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* KalmanFilterSvc.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterTransition.hpp b/include/xo/kalmanfilter/KalmanFilterTransition.hpp new file mode 100644 index 00000000..22d3de5b --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterTransition.hpp @@ -0,0 +1,62 @@ +/* @file KalmanFilterTransition.hpp */ + +#pragma once + +//#include "time/Time.hpp" +#include +#include + +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 */ diff --git a/include/xo/kalmanfilter/init_filter.hpp b/include/xo/kalmanfilter/init_filter.hpp new file mode 100644 index 00000000..292a63ff --- /dev/null +++ b/include/xo/kalmanfilter/init_filter.hpp @@ -0,0 +1,20 @@ +/* file init_kalmanfilter.hpp + * + * author: Roland Conybeare, Aug 2022 + */ + +#pragma once + +#include "xo/subsys/Subsystem.hpp" + +namespace xo { + enum S_kalmanfilter_tag {}; + + template<> + struct InitSubsys { + static void init(); + static InitEvidence require(); + }; +} /*namespace xo*/ + +/* end init_kalmanfilter.hpp */ diff --git a/include/xo/kalmanfilter/print_eigen.hpp b/include/xo/kalmanfilter/print_eigen.hpp new file mode 100644 index 00000000..ea6f7375 --- /dev/null +++ b/include/xo/kalmanfilter/print_eigen.hpp @@ -0,0 +1,41 @@ +/* @file print_eigen.hpp */ + +#include +#include + +namespace logutil { + template + class matrix { + public: + matrix(T x) : x_{std::move(x)} {} + + /* print this value */ + T x_; + }; /*matrix*/ + + template + using vector = matrix; + + template + inline std::ostream & + operator<<(std::ostream & s, matrix const & mat) + { + s << "["; + for(std::uint32_t i = 0, m = mat.x_.rows(); i 0) + s << "; "; + + for(std::uint32_t j = 0, n = mat.x_.cols(); j 0) + s << ' '; + + s << mat.x_(i, j); + } + } + s << "]"; + + return s; + } /*operator<<*/ +} /*namespace logutil*/ + +/* end print_eigen.hpp */ diff --git a/init_filter.cpp b/init_filter.cpp deleted file mode 100644 index 80407cc4..00000000 --- a/init_filter.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/* 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::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::require() - { - InitEvidence retval; - - /* subsystem dependencies for filter/ */ - retval ^= InitSubsys::require(); - - /* filter/'s own initialization code */ - retval ^= Subsystem::provide("filter", &init); - - return retval; - } /*require*/ -} /*namespace xo*/ - -/* end init_filter.cpp */ diff --git a/init_filter.hpp b/init_filter.hpp deleted file mode 100644 index 79cfc6d0..00000000 --- a/init_filter.hpp +++ /dev/null @@ -1,20 +0,0 @@ -/* file init_filter.hpp - * - * author: Roland Conybeare, Aug 2022 - */ - -#pragma once - -#include "subsys/Subsystem.hpp" - -namespace xo { - enum S_filter_tag {}; - - template<> - struct InitSubsys { - static void init(); - static InitEvidence require(); - }; -} /*namespace xo*/ - -/* end init_filter.hpp */ diff --git a/print_eigen.hpp b/print_eigen.hpp deleted file mode 100644 index 953c5904..00000000 --- a/print_eigen.hpp +++ /dev/null @@ -1,41 +0,0 @@ -/* @file print_eigen.hpp */ - -#include -#include - -namespace logutil { - template - class matrix { - public: - matrix(T x) : x_{std::move(x)} {} - - /* print this value */ - T x_; - }; /*matrix*/ - - template - using vector = matrix; - - template - inline std::ostream & - operator<<(std::ostream & s, matrix const & mat) - { - s << "["; - for(std::uint32_t i = 0, m = mat.x_.rows(); i 0) - s << "; "; - - for(std::uint32_t j = 0, n = mat.x_.cols(); j 0) - s << ' '; - - s << mat.x_(i, j); - } - } - s << "]"; - - return s; - } /*operator<<*/ -} /*namespace logutil*/ - -/* end print_eigen.hpp */ diff --git a/src/kalmanfilter/CMakeLists.txt b/src/kalmanfilter/CMakeLists.txt new file mode 100644 index 00000000..30ca8c02 --- /dev/null +++ b/src/kalmanfilter/CMakeLists.txt @@ -0,0 +1,31 @@ +# xo-kalmanfilter/src/kalmanfilter/CMakeLists.txt + +set(SELF_LIB xo_kalmanfilter) + +set(SELF_SRCS + EigenUtil.cpp + KalmanFilterInput.cpp + KalmanFilterInputToConsole.cpp + KalmanFilterState.cpp + KalmanFilterStateToConsole.cpp + KalmanFilterTransition.cpp + KalmanFilterObservable.cpp + KalmanFilterEngine.cpp + KalmanFilter.cpp + KalmanFilterStep.cpp + KalmanFilterSpec.cpp + KalmanFilterSvc.cpp + init_filter.cpp +) + +xo_add_shared_library4(${SELF_LIB} ${PROJECT_NAME}Targets ${PROJECT_VERSION} 1 ${SELF_SRCS}) + +# ---------------------------------------------------------------- +# internal dependencies + +xo_dependency(${SELF_LIB} reactor) + +# ---------------------------------------------------------------- +# external dependencies + +xo_external_target_dependency(${SELF_LIB} eigen3 Eigen3::Eigen) diff --git a/src/kalmanfilter/EigenUtil.cpp b/src/kalmanfilter/EigenUtil.cpp new file mode 100644 index 00000000..b5770440 --- /dev/null +++ b/src/kalmanfilter/EigenUtil.cpp @@ -0,0 +1,157 @@ +/* file EigenUtil.cpp + * + * author: Roland Conybeare, Sep 2022 + */ + +#include "EigenUtil.hpp" +#include "xo/printjson/PrintJson.hpp" +#include "xo/reflect/Reflect.hpp" +#include +#include +#include +#include + +namespace xo { + using xo::json::PrintJson; + using xo::json::JsonPrinter; + using xo::reflect::Reflect; + using xo::reflect::TypeDescr; + using VectorXb = Eigen::Array; + using Eigen::VectorXd; + using Eigen::MatrixXd; + +#ifdef NOT_YET + namespace reflect { + template + using EigenVectorX_Tdx = xo::reflect::StlVectorTdx>; + + /* probably need this to appear before decl for class xo::reflect::Reflect */ + template + class EstablishTdx> { + public: + static std::unique_ptr make() { + return EigenVectorX_Tdx::make(); + } /*make*/ + }; /*EstablishTdx*/ + } /*reflect*/ +#endif + + namespace eigen { + + namespace { + /* prints a VectorXd as json, in the obvious format, e.g. + * [1,2,3] + */ + template + 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(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(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 0) + *p_os << ", "; + *p_os << "["; + for(std::uint32_t j=0, n=pm->cols(); 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 + void + provide_eigen_vector_printer(PrintJson * p_pjson) + { + TypeDescr td = Reflect::require(); + std::unique_ptr pr(new EigenVectorJsonPrinter(p_pjson)); + + p_pjson->provide_printer(td, std::move(pr)); + } /*provide_eigen_vector_printer*/ + } /*namespace*/ + + void + EigenUtil::reflect_eigen() + { +#ifdef NOT_YET + Reflect::require(); + Reflect::require(); +#endif + } /*reflect_eigen*/ + + void + EigenUtil::provide_json_printers(PrintJson * p_pjson) + { + assert(p_pjson); + + provide_eigen_vector_printer(p_pjson); + provide_eigen_vector_printer(p_pjson); + + { + TypeDescr td = Reflect::require(); + std::unique_ptr pr(new MatrixXdJsonPrinter(p_pjson)); + + p_pjson->provide_printer(td, std::move(pr)); + } + } /*provide_json_printers*/ + } /*namespace eigen*/ +} /*namespace xo*/ + +/* end EigenUtil.cpp */ diff --git a/src/kalmanfilter/KalmanFilter.cpp b/src/kalmanfilter/KalmanFilter.cpp new file mode 100644 index 00000000..56507650 --- /dev/null +++ b/src/kalmanfilter/KalmanFilter.cpp @@ -0,0 +1,78 @@ +/* @file KalmanFilter.cpp */ + +#include "KalmanFilter.hpp" +#include "KalmanFilterEngine.hpp" +#include "print_eigen.hpp" +#include "xo/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 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 << ""; + } /*display*/ + + std::string + KalmanFilter::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilter.cpp */ diff --git a/src/kalmanfilter/KalmanFilterEngine.cpp b/src/kalmanfilter/KalmanFilterEngine.cpp new file mode 100644 index 00000000..e9533549 --- /dev/null +++ b/src/kalmanfilter/KalmanFilterEngine.cpp @@ -0,0 +1,360 @@ +/* @file KalmanFilterEngine.cpp + * + */ + +#include "KalmanFilterEngine.hpp" +#include "print_eigen.hpp" +#include "xo/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 + KalmanFilterEngine::extrapolate(utc_nanos tkp1, + ref::rp 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 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 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 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 + KalmanFilterEngine::correct1(ref::rp const & skp1_ext, + KalmanFilterObservable const & h, + ref::rp 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 + KalmanFilterEngine::correct(ref::rp const & skp1_ext, + KalmanFilterObservable const & h, + ref::rp 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 + KalmanFilterEngine::step(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1) + { + ref::rp skp1_ext + = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); + + ref::rp skp1 + = KalmanFilterEngine::correct(skp1_ext, Hkp1, zkp1); + + return skp1; + } /*step*/ + + ref::rp + 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 + KalmanFilterEngine::step1(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1, + uint32_t j) + { + ref::rp skp1_ext + = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); + + ref::rp skp1 + = KalmanFilterEngine::correct1(skp1_ext, Hkp1, zkp1, j); + + return skp1; + } /*step1*/ + + ref::rp + 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 */ diff --git a/src/kalmanfilter/KalmanFilterInput.cpp b/src/kalmanfilter/KalmanFilterInput.cpp new file mode 100644 index 00000000..96f0d6ca --- /dev/null +++ b/src/kalmanfilter/KalmanFilterInput.cpp @@ -0,0 +1,118 @@ +/* @file KalmanFilterInput.cpp */ + +#include "KalmanFilterInput.hpp" +#include "xo/reflect/StructReflector.hpp" +#include "Eigen/src/Core/Matrix.h" +#include "print_eigen.hpp" +#include "xo/indentlog/scope.hpp" +#include "xo/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::make(utc_nanos tkp1, + VectorXb const & presence, + VectorXd const & z, + VectorXd const & Rd) + { + return new KalmanFilterInput(tkp1, presence, z, Rd); + } /*make*/ + + ref::rp + 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(); jpresence_[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(); jpresence_[j]) { + keep[jstar] = j; + ++jstar; + } + } + + log && log("keep", matrix(keep)); + + return keep; + } /*make_kept_index*/ + + void + KalmanFilterInput::reflect_self() + { + StructReflector 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 << ""; + } /*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 */ diff --git a/KalmanFilterInputToConsole.cpp b/src/kalmanfilter/KalmanFilterInputToConsole.cpp similarity index 93% rename from KalmanFilterInputToConsole.cpp rename to src/kalmanfilter/KalmanFilterInputToConsole.cpp index a0a848ca..8fadf031 100644 --- a/KalmanFilterInputToConsole.cpp +++ b/src/kalmanfilter/KalmanFilterInputToConsole.cpp @@ -1,7 +1,7 @@ /* @file KalmanFilterInputToConsole.cpp */ #include "KalmanFilterInputToConsole.hpp" -#include "indentlog/print/tag.hpp" +#include "xo/indentlog/print/tag.hpp" namespace xo { using xo::xtag; diff --git a/KalmanFilterObservable.cpp b/src/kalmanfilter/KalmanFilterObservable.cpp similarity index 98% rename from KalmanFilterObservable.cpp rename to src/kalmanfilter/KalmanFilterObservable.cpp index 90f31460..ad90c1f1 100644 --- a/KalmanFilterObservable.cpp +++ b/src/kalmanfilter/KalmanFilterObservable.cpp @@ -2,7 +2,7 @@ #include "KalmanFilterObservable.hpp" #include "print_eigen.hpp" -#include "indentlog/scope.hpp" +#include "xo/indentlog/scope.hpp" namespace xo { using xo::scope; diff --git a/src/kalmanfilter/KalmanFilterSpec.cpp b/src/kalmanfilter/KalmanFilterSpec.cpp new file mode 100644 index 00000000..45185551 --- /dev/null +++ b/src/kalmanfilter/KalmanFilterSpec.cpp @@ -0,0 +1,27 @@ +/* @file KalmanFilterSpec.cpp */ + +#include "KalmanFilterSpec.hpp" +#include "xo/indentlog/scope.hpp" + +namespace xo { + using xo::tostr; + using xo::xtag; + + namespace kalman { + void + KalmanFilterSpec::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterSpec::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterSpec.cpp */ diff --git a/src/kalmanfilter/KalmanFilterState.cpp b/src/kalmanfilter/KalmanFilterState.cpp new file mode 100644 index 00000000..4b5679e3 --- /dev/null +++ b/src/kalmanfilter/KalmanFilterState.cpp @@ -0,0 +1,231 @@ +/* @file KalmanFilterState.cpp */ + +#include "KalmanFilterState.hpp" +#include "print_eigen.hpp" +#include "xo/reflect/StructReflector.hpp" +#include "xo/reflect/TaggedPtr.hpp" +#include "xo/indentlog/scope.hpp" +#include "Eigen/src/Core/Matrix.h" +#include +#include + +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::make() + { + return new KalmanFilterState(); + } /*make*/ + + rp + 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 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::make() + { + return new KalmanFilterStateExt(); + } /*make*/ + + rp + KalmanFilterStateExt::make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + rp 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 sr; + + if (sr.is_incomplete()) { + /* TODO: use sr.adopt_ancestors() */ + + 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 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 << ""; + } /*display*/ + + std::string + KalmanFilterState::display_string() const + { + std::stringstream ss; + ss << *this; + return ss.str(); + } /*display_string*/ + + // ----- KalmanFilterStateExt ----- + + ref::rp + 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 << "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 */ diff --git a/KalmanFilterStateToConsole.cpp b/src/kalmanfilter/KalmanFilterStateToConsole.cpp similarity index 93% rename from KalmanFilterStateToConsole.cpp rename to src/kalmanfilter/KalmanFilterStateToConsole.cpp index 8e76ce84..9559d339 100644 --- a/KalmanFilterStateToConsole.cpp +++ b/src/kalmanfilter/KalmanFilterStateToConsole.cpp @@ -1,7 +1,7 @@ /* @file KalmanFilterStateToConsole.cpp */ #include "KalmanFilterStateToConsole.hpp" -#include "indentlog/print/tag.hpp" +#include "xo/indentlog/print/tag.hpp" namespace xo { using xo::xtag; diff --git a/src/kalmanfilter/KalmanFilterStep.cpp b/src/kalmanfilter/KalmanFilterStep.cpp new file mode 100644 index 00000000..8ca90211 --- /dev/null +++ b/src/kalmanfilter/KalmanFilterStep.cpp @@ -0,0 +1,84 @@ +/* @file KalmanFilterStep.cpp */ + +#include "KalmanFilterStep.hpp" +#include "KalmanFilterEngine.hpp" +#include "KalmanFilterState.hpp" +#include "xo/indentlog/scope.hpp" + +namespace xo { + using xo::scope; + using xo::tostr; + using xo::xtag; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + ref::rp + KalmanFilterStep::extrapolate() const + { + return KalmanFilterEngine::extrapolate(this->tkp1(), + this->state(), + this->model() /*transition*/); + } /*extrapolate*/ + + MatrixXd + KalmanFilterStep::gain(ref::rp const & skp1_ext) const + { + return KalmanFilterEngine::kalman_gain(skp1_ext, + this->obs()); + } /*gain*/ + + VectorXd + KalmanFilterStep::gain1(ref::rp const & skp1_ext, + uint32_t j) const + { + return KalmanFilterEngine::kalman_gain1(skp1_ext, + this->obs(), + j); + + } /*gain1*/ + + ref::rp + KalmanFilterStep::correct(ref::rp const & skp1_ext) + { + return KalmanFilterEngine::correct(skp1_ext, + this->obs(), + this->input()); + } /*correct*/ + + ref::rp + KalmanFilterStep::correct1(ref::rp 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 << "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 */ diff --git a/KalmanFilterSvc.cpp b/src/kalmanfilter/KalmanFilterSvc.cpp similarity index 100% rename from KalmanFilterSvc.cpp rename to src/kalmanfilter/KalmanFilterSvc.cpp diff --git a/src/kalmanfilter/KalmanFilterTransition.cpp b/src/kalmanfilter/KalmanFilterTransition.cpp new file mode 100644 index 00000000..a6ec63ae --- /dev/null +++ b/src/kalmanfilter/KalmanFilterTransition.cpp @@ -0,0 +1,55 @@ +/* @file KalmanFilterTransition.cpp */ + +#include "KalmanFilterTransition.hpp" +#include "print_eigen.hpp" +#include "xo/reflect/StructReflector.hpp" +#include "xo/indentlog/scope.hpp" + +namespace xo { + using xo::reflect::StructReflector; + using logutil::matrix; + using xo::xtag; + + namespace kalman { + void + KalmanFilterTransition::reflect_self() + { + StructReflector 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 << ""; + } /*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 */ diff --git a/src/kalmanfilter/init_filter.cpp b/src/kalmanfilter/init_filter.cpp new file mode 100644 index 00000000..3417214b --- /dev/null +++ b/src/kalmanfilter/init_filter.cpp @@ -0,0 +1,52 @@ +/* file init_filter.cpp + * + * author: Roland Conybeare, Aug 2022 + */ + +#include "init_filter.hpp" +#include "xo/reactor/init_reactor.hpp" + +#include "KalmanFilterState.hpp" +#include "EigenUtil.hpp" +#include "xo/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::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::require() + { + InitEvidence retval; + + /* subsystem dependencies for filter/ */ + retval ^= InitSubsys::require(); + + /* filter/'s own initialization code */ + retval ^= Subsystem::provide("kalmanfilter", &init); + + return retval; + } /*require*/ +} /*namespace xo*/ + +/* end init_filter.cpp */ diff --git a/utest/CMakeLists.txt b/utest/CMakeLists.txt new file mode 100644 index 00000000..9f21bfa4 --- /dev/null +++ b/utest/CMakeLists.txt @@ -0,0 +1,54 @@ +# xo-kalmanfilter/utest/CMakeLists.txt + +set(SELF_EXE utest.filter) + +set(SELF_SRCS + KalmanFilter.test.cpp + filter_utest_main.cpp) + +add_executable(${SELF_EXE} ${SELF_SRCS}) +xo_include_options2(${SELF_EXE}) + +add_test(NAME ${SELF_EXE} COMMAND ${SELF_EXE}) +target_code_coverage(${SELF_EXE} AUTO ALL) + +# ---------------------------------------------------------------- +# create convenience symlink to canned data + +#create_symlink("${CMAKE_SOURCE_DIR}/utestdata/" "src/filter/utest/utestdata") + +# ---------------------------------------------------------------- +# generic project dependency + +# PROJECT_SOURCE_DIR: +# so we can for example write +# #include "logutil/scope.hpp" +# from anywhere in the project +# PROJECT_BINARY_DIR: +# since version file will be in build directory, need that directory +# to also be included in compiler's include path +# +xo_self_dependency(${SELF_EXE} xo_kalmanfilter) + +# ---------------------------------------------------------------- +# internal dependencies: logutil, ... + +xo_dependency(${SELF_EXE} xo_statistics) + +# ---------------------------------------------------------------- +# external dependencies: catch2.. + +xo_external_target_dependency(${SELF_EXE} Catch2 Catch2::Catch2) + +# ---------------------------------------------------------------- +# make standard directories for std:: includes explicit +# so that +# (1) they appear in compile_commands.json. +# (2) clangd (run from emacs lsp-mode) can find them +# +if(CMAKE_EXPORT_COMPILE_COMMANDS) + set(CMAKE_CXX_STANDARD_INCLUDE_DIRECTORIES + ${CMAKE_CXX_IMPLICIT_INCLUDE_DIRECTORIES}) +endif() + +# end CMakeLists.txt diff --git a/utest/KalmanFilter.test.cpp b/utest/KalmanFilter.test.cpp new file mode 100644 index 00000000..679f461f --- /dev/null +++ b/utest/KalmanFilter.test.cpp @@ -0,0 +1,690 @@ +/* @file KalmanFilter.test.cpp */ + +#include "xo/kalmanfilter/KalmanFilter.hpp" +#include "xo/kalmanfilter/KalmanFilterEngine.hpp" +#include "xo/kalmanfilter/print_eigen.hpp" +#include "statistics/SampleStatistics.hpp" +#include "xo/randomgen/normalgen.hpp" +#include "xo/randomgen/xoshiro256.hpp" +#include "xo/indentlog/scope.hpp" +#include "xo/indentlog/log_level.hpp" +#include +#include + +namespace xo { + using xo::kalman::KalmanFilterSpec; + using xo::kalman::KalmanFilterStep; + using xo::kalman::KalmanFilterEngine; + using xo::kalman::KalmanFilterStateExt; + using xo::kalman::KalmanFilterState; + using xo::kalman::KalmanFilterTransition; + using xo::kalman::KalmanFilterObservable; + using xo::kalman::KalmanFilterInput; + using xo::statistics::SampleStatistics; + using xo::rng::normalgen; + using xo::rng::xoshiro256ss; + using xo::time::timeutil; + using xo::time::utc_nanos; + using xo::time::seconds; + using xo::ref::rp; + using xo::log_level; + using logutil::matrix; + //using logutil::scope; + //using logutil::tostr; + //using logutil::xtag; + using xo::print::ccs; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace ut { + namespace { + /* step for kalman filter with: + * - single state variable x[0] + * - identity process model - x(k+1) = F(k).x(k), with F(k) = | 1 | + * - no process noise + * - single observation z[0] + * - identity coupling matrix - z(k) = H(k).x(k) + w(k), with H(k) = | 1 | + */ + KalmanFilterSpec::MkStepFn + kalman_identity1_mkstep_fn() + { + /* kalman state transition matrix: use identity <--> state is constant */ + MatrixXd F = MatrixXd::Identity(1, 1); + + /* state transition noise: set this to zero; + * measuring something that's known to be constant + */ + MatrixXd Q = MatrixXd::Zero(1, 1); + + /* single direct observation */ + MatrixXd H = MatrixXd::Identity(1, 1); + + /* observation errors understood to have + * mean 0, sdev 1 + * + * This is consistent with normal_rng below, + * so R is correctly specified + */ + MatrixXd R = MatrixXd::Identity(1, 1); + + return [F, Q, H, R](rp const & sk, + rp const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk = KalmanFilterObservable::keep_all(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_identity1_mkstep_fn*/ + } /*namespace*/ + + /* example 1. + * repeated direct observation of a scalar + * use rng to generate observations + */ + TEST_CASE("kalman-identity", "[kalmanfilter]") { + /* setting up trivial filter for repeated indept + * measurements of a constant. + * + * True value of unknown set to 10, + * utest observes filter converging toward that value + */ + + /* seed for rng */ + uint64_t seed = 14950319842636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on 'measurements', + * use as reference implementation for filter + */ + SampleStatistics z_stats; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0) = [0] */ + VectorXd x0(1); + x0 << 10.0 + normal_rng(); + + INFO(tostr("x0=", x0)); + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0 = 1.0 * MatrixXd::Identity(1, 1); + + /* F, Q, K, j, zk not used for initial state */ + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1 /*j*/, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_identity1_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + + rp sk = spec.start_ext(); + + for(uint32_t i_step = 1; i_step < 100; ++i_step) { + /* note: for this filter, measurement time doesn't matter */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(1); + z << 10.0 + normal_rng(); + + INFO(tostr("z=", z)); + + z_stats.include_sample(z[0]); + + ref::rp inputk + = KalmanFilterInput::make_present(tkp1, z); + + KalmanFilterStep step_spec = spec.make_step(sk, inputk); + + rp skp1 + = KalmanFilterEngine::step(step_spec); + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 1); + REQUIRE(skp1->state_v().size() == 1); + REQUIRE(skp1->state_v()[0] == Approx(z_stats.mean()).epsilon(1e-6)); + REQUIRE(skp1->state_cov().rows() == 1); + REQUIRE(skp1->state_cov().cols() == 1); + REQUIRE(skp1->gain().rows() == 1); + REQUIRE(skp1->gain().cols() == 1); + REQUIRE(skp1->observable() == -1); + + /* z_stats reflects k = z_stats.n_sample() N(0,1) 'random' vars; + * variance of sum (i.e. z_stats.mean() * k) is proportional to k; + * variance of mean like 1/k + * + * kalman filter also should compute covariance estimate like 1/k + */ + + REQUIRE(skp1->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + REQUIRE(skp1->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + /* estimate at each step should be (approximately) + * average of measurements taken so far. + * approximate because also affected by prior + */ + + sk = skp1; + } + + REQUIRE(sk->state_v()[0] == Approx(10.0).epsilon(1e-2)); + REQUIRE(sk->state_cov()(0, 0) == Approx(0.01).epsilon(1e-6)); + REQUIRE(sk->gain()(0, 0) == Approx(0.01).epsilon(1e-6)); + } /*TEST_CASE(kalman-identity)*/ + + /* example 2. + * like example 1, but using "separate observation" variants: + * KalmanGain::correct1() // per observation + * instead of + * KalmanGain::correct() // per observation set + */ + TEST_CASE("kalman-identity1", "[kalmanfilter]") { + /* setting up trivial filter for repeated indept + * measurements of a constant. + * + * True value of unknown set to 10, + * utest observes filter converging toward that value + * + */ + + /* seed for rng */ + uint64_t seed = 14950319842636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on 'measurements', + * use as reference implementation for filter + */ + SampleStatistics z_stats; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0) = [0] */ + VectorXd x0(1); + x0 << 10.0 + normal_rng(); + + INFO(tostr("x0=", x0)); + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0 = 1.0 * MatrixXd::Identity(1, 1); + + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_identity1_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + + rp sk = spec.start_ext(); + + for(uint32_t i_step = 1; i_step < 100; ++i_step) { + /* note: for this filter, measurement time doesn't matter */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(1); + z << 10.0 + normal_rng(); + + INFO(tostr("z=", z)); + + z_stats.include_sample(z[0]); + + ref::rp inputk + = KalmanFilterInput::make_present(tkp1, z); + + KalmanFilterStep step_spec + = spec.make_step(sk, inputk); + + rp skp1 + = KalmanFilterEngine::step1(step_spec, 0 /*j*/); + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 1); + REQUIRE(skp1->state_v().size() == 1); + REQUIRE(skp1->state_v()[0] == Approx(z_stats.mean()).epsilon(1e-6)); + REQUIRE(skp1->state_cov().rows() == 1); + REQUIRE(skp1->state_cov().cols() == 1); + REQUIRE(skp1->gain().rows() == 1); + REQUIRE(skp1->gain().cols() == 1); + REQUIRE(skp1->observable() == 0); + + /* z_stats reflects k = z_stats.n_sample() N(0,1) 'random' vars; + * variance of sum (i.e. z_stats.mean() * k) is proportional to k; + * variance of mean like 1/k + * + * kalman filter also should compute covariance estimate like 1/k + */ + + REQUIRE(skp1->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + REQUIRE(skp1->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + /* estimate at each step should be (approximately) + * average of measurements taken so far. + * approximate because also affected by prior + */ + + sk = skp1; + } + + REQUIRE(sk->state_v()[0] == Approx(10.0).epsilon(1e-2)); + REQUIRE(sk->state_cov()(0, 0) == Approx(0.01).epsilon(1e-6)); + REQUIRE(sk->gain()(0, 0) == Approx(0.01).epsilon(1e-6)); + } /*TEST_CASE(kalman-identity1)*/ + + namespace { + /* step for kalman filter with: + * - single state variable x[0] + * - identity process model: x(k+1) = F(k).x(k), with F(k) = | 1 | + * - no process noise + * - two observations z[0], z[1] + * - identity coupling matrix: z(k) = H(k).x(k) + w(k), with + * H(k) = | 1 | + * | 1 | + * + * w(k) = | w1 | with w1 ~ N(0,1) + * | w2 | + */ + KalmanFilterSpec::MkStepFn + kalman_identity2_mkstep_fn() + { + /* kalman state transition matrix: use identity <-> state is constant */ + MatrixXd F = MatrixXd::Identity(1, 1); + + /* state transition noise: set to 0 */ + MatrixXd Q = MatrixXd::Zero(1, 1); + + /* two direct observations */ + MatrixXd H = MatrixXd::Constant(2 /*#rows*/, 1 /*#cols*/, 1.0 /*M(i,j)*/); + + /* observation errors: N(0,1) */ + MatrixXd R = MatrixXd::Identity(2, 2); + + return [F, Q, H, R](rp const & sk, + rp const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk = KalmanFilterObservable::keep_all(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_identity2_mkstep_fn*/ + } /*namespace*/ + + TEST_CASE("kalman-identity2", "[kalmanfilter]") { + /* variation on filter in kalman-identity1 utest above; + * this time make 2 observations per step + */ + + /* seed for rng */ + uint64_t seed = 14950319842636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on 'measurements', + * use as reference implementation for filter + */ + SampleStatistics z_stats; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0) = [0] */ + VectorXd x0(1); + x0 << 10.0 + normal_rng(); + + INFO(tostr("x0=", x0)); + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0 = 1.0 * MatrixXd::Identity(1, 1); + + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1 /*j*/, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_identity2_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + rp sk = spec.start_ext(); + + /* need 1/2 as many filter steps to reach same confidence + * as in test "kalman-identity" + */ + for(uint32_t i_step = 1; i_step < 51; ++i_step) { + INFO(tostr(xtag("i_step", i_step))); + + /* note: for this filter, measurement time doesn't affect behavior */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(2); + z << 10.0 + normal_rng(), 10.0 + normal_rng(); + + z_stats.include_sample(z[0]); + z_stats.include_sample(z[1]); + + INFO(tostr(xtag("i_step", i_step), xtag("z", z))); + + ref::rp inputk + = KalmanFilterInput::make_present(tkp1, z); + + KalmanFilterStep step_spec = spec.make_step(sk, inputk); + + rp skp1 = KalmanFilterEngine::step(step_spec); + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 1); + REQUIRE(skp1->state_v().size() == 1); + REQUIRE(skp1->state_v()[0] == Approx(z_stats.mean()).epsilon(1e-6)); + REQUIRE(skp1->state_cov().rows() == 1); + REQUIRE(skp1->state_cov().cols() == 1); + REQUIRE(skp1->gain().rows() == 1); + REQUIRE(skp1->gain().cols() == 2); + REQUIRE(skp1->observable() == -1); + /* z_stats reflects 2*k = z_stats.n_sample() N(0,1) 'random' vars + * (since 2 vars per step) + * variance of sum (i.e. z_stats.mean() * k) is proportional to k; + * variance of mean like 1/k + * + * kalman filter also should compute covariance estimate like 1/k + * + */ + + REQUIRE(skp1->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + REQUIRE(skp1->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + REQUIRE(skp1->gain()(0, 1) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + /* estimate at each step should be (approximately) + * average of measurements taken so far. + * approximate because also affected by prior + */ + + sk = skp1; + } + + REQUIRE(sk->state_v()[0] == Approx(10.0).epsilon(1e-2)); + REQUIRE(sk->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-3)); + /* result is close but not identical, + * because initial confidence P0 counts as one sample, + * so have odd #of samples + */ + REQUIRE(sk->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-3)); + REQUIRE(sk->gain()(0, 1) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-3)); + } /*TEST_CASE(kalman-identity2)*/ + + namespace { + /* step for kalman filter with: + * - two state variables x[0], x[1] + * x[0] subject to random disturbances, reverts towards mean 1 + * x[1] is 1 + * - process model: x(k+1) = F(k).x(k) + v(k) with + * F(k) = | 0.95 0.05 | v(k) = | v1 |, v ~ N(0, 0.25) + * | 0 1 | | 0 | + * - one observation z[0] + * - coupling matrix: z(k) = H(k).x(k) + w(k), with + * H(k) = | 1 0 | + * + * w(k) ~ N(0,1) + */ + KalmanFilterSpec::MkStepFn + kalman_revert1_mkstep_fn() + { + /* kalman state transition matrix */ + MatrixXd F(2,2); + F << 0.95, 0.05, 0, 1; + + /* state transition noise */ + MatrixXd Q(2,2); + Q << 0.0001, 0.0, 0.0, 0.0; + + /* coupling matrix */ + MatrixXd H(1,2); + H << 1.0, 0.0; + + /* observation errors */ + MatrixXd R(1,1); + R << 0.25; + + return [F, Q, H, R](rp const & sk, + rp const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk = KalmanFilterObservable::keep_all(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_revert1_mkstep_fn*/ + } /*namespace*/ + + TEST_CASE("kalman-revert1", "[kalmanfilter]") { + /* like test "kalman-identity", + * but introduce some system noise. + */ + + constexpr bool c_debug_enabled = false; + scope lscope(XO_DEBUG2(c_debug_enabled, "TEST(kalman_revert)")); + + /* seed for rng */ + uint64_t seed = 14950139742636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on observations, + * use as reference when assessing filter behavior + */ + SampleStatistics z_stats; + + /* write output to file - use as baseline for regression testing */ + std::string self_test_name = Catch::getResultCapture().getCurrentTestName(); + + /* write space-delimited output, suitable for gnuplot + * omit always-constant values, rely on unit test verifying these + */ + std::ofstream out(self_test_name); + out << "step z0 x0 P00 K0" << std::endl; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0). + * x(0)[1] is constant 1, used to achieve mean reversion + */ + VectorXd x0(2); + x0 << 1.0 + normal_rng(), 1.0; + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0(2,2); + P0 << 1.0, 0.0, 0.0, 0.0; + + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1 /*j*/, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_revert1_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + rp sk = spec.start_ext(); + + for(uint32_t i_step = 1; i_step < 100; ++i_step) { + /* note: for this filter, measurement time doesn't affect behavior */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(1); + z << 1.0 + normal_rng(); + + z_stats.include_sample(z[0]); + + ref::rp inputk + = KalmanFilterInput::make_present(tkp1, z); + KalmanFilterStep step_spec = spec.make_step(sk, inputk); + rp skp1 = KalmanFilterEngine::step(step_spec); + + if (c_debug_enabled) { + lscope.log("filter", + xtag("step", i_step), + xtag("z", matrix(z)), + xtag("x", matrix(skp1->state_v())), + xtag("P", matrix(skp1->state_cov())), + xtag("K", matrix(skp1->gain()))); + } + + /* headings: step z0 x0 P00 K0 */ + out << i_step + << " " << z(0) + << " " << skp1->state_v()(0) + << " " << skp1->state_cov()(0, 0) + << " " << skp1->gain()(0, 0) + << "\n"; + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 2); + // + REQUIRE(skp1->state_v().size() == 2); + REQUIRE(skp1->state_v()(1) == 1.0); + // + REQUIRE(skp1->state_cov().rows() == 2); + REQUIRE(skp1->state_cov().cols() == 2); + // test skp1->state_cov()(0,0) vs baseline + REQUIRE(skp1->state_cov()(0, 0) >= 0.0); + REQUIRE(skp1->state_cov()(1, 0) == 0.0); + REQUIRE(skp1->state_cov()(0, 1) == 0.0); + REQUIRE(skp1->state_cov()(1, 1) == 0.0); + // + REQUIRE(skp1->gain().rows() == 2); + REQUIRE(skp1->gain().cols() == 1); + REQUIRE(skp1->gain()(0, 0) > 0.0); + REQUIRE(skp1->gain()(1, 0) == 0.0); + // + REQUIRE(skp1->observable() == -1); + // + + sk = skp1; + } + + out << std::flush; + out.close(); + + /* compare output with regression baseline. + * command like: + * diff kalman-revert1 utestdata/filter/kalman-revert1 + */ + char cmd_buf[256]; + snprintf(cmd_buf, sizeof(cmd_buf), + "diff %s utestdata/filter/%s\n", + self_test_name.c_str(), + self_test_name.c_str()); + + INFO(tostr(self_test_name, xtag("cmd", ccs(cmd_buf)))); + + std::int32_t err = ::system(cmd_buf); + + REQUIRE(err == 0); + } /*TEST_CASE(kalman-drift)*/ + +#ifdef NOT_IN_USE + namespace { + /* step for kalman filter with: + * - two state variables x[0], x[1] + * - identity process model: x(k+1) = F(k).x(k), with + * F(k) = | 1 0 | + * | 0 1 | + * - no process noise + * - two observations z[0], z[1] + * - simple coupling matrix: z(k) = H(k).x(k) + w(k), with + * H(k) = | 1 0 | + * | 0 -1 | + * (so sign of z[1] is reversed w.r.t x[1]) + * + * w(k) = | w1 | with w1 ~ N(0,1) + * | w2 | + */ + KalmanFilterSpec::MkStepFn + kalman_identity2x2_mkstep_fn() + { + /* kalman state transition matrix: use identity <-> state is constant */ + MatrixXd F = MatrixXd::Identity(2, 2); + + /* state transition noise: set to 0 */ + MatrixXd Q = MatrixXd::Zero(2, 2); + + /* two direct observations */ + MatrixXd H = MatrixXd::Constant(2 /*#rows*/, 1 /*#cols*/, 1.0 /*M(i,j)*/); + + /* observation errors: N(0,1) */ + MatrixXd R = MatrixXd::Identity(2, 2); + + return [F, Q, H, R](KalmanFilterState const & sk, + KalmanFilterInput const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_identity2_mkstep_fn*/ + } /*namespace*/ +#endif + } /*namespace ut*/ +} /*namespace xo*/ + +/* end KalmanFilter.test.cpp */ diff --git a/utest/filter_utest_main.cpp b/utest/filter_utest_main.cpp new file mode 100644 index 00000000..9ad0266a --- /dev/null +++ b/utest/filter_utest_main.cpp @@ -0,0 +1,6 @@ +/* @file filter_utest_main.cpp */ + +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" + +/* end filter_utest_main.cpp */ From 10d49e511cc4f92e131c518721b6bdade27edbdf Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Mon, 23 Oct 2023 12:02:52 -0400 Subject: [PATCH 03/25] bugfix: spelling - must use Eigen3 w/ find_package() on linux --- src/kalmanfilter/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/kalmanfilter/CMakeLists.txt b/src/kalmanfilter/CMakeLists.txt index 30ca8c02..b7036526 100644 --- a/src/kalmanfilter/CMakeLists.txt +++ b/src/kalmanfilter/CMakeLists.txt @@ -28,4 +28,4 @@ xo_dependency(${SELF_LIB} reactor) # ---------------------------------------------------------------- # external dependencies -xo_external_target_dependency(${SELF_LIB} eigen3 Eigen3::Eigen) +xo_external_target_dependency(${SELF_LIB} Eigen3 Eigen3::Eigen) From 9bb3f8c668f09d5a075a8d62584187203719388a Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Mon, 23 Oct 2023 12:30:37 -0400 Subject: [PATCH 04/25] kalmanfilter: genx symlinks -> canned baseline data --- utest/CMakeLists.txt | 5 +- utest/KalmanFilter.test.cpp | 3 - utest/utestdata/filter/kalman-revert1 | 100 ++++++++++++++++++++++++++ 3 files changed, 104 insertions(+), 4 deletions(-) create mode 100644 utest/utestdata/filter/kalman-revert1 diff --git a/utest/CMakeLists.txt b/utest/CMakeLists.txt index 9f21bfa4..a118b8a7 100644 --- a/utest/CMakeLists.txt +++ b/utest/CMakeLists.txt @@ -13,9 +13,12 @@ add_test(NAME ${SELF_EXE} COMMAND ${SELF_EXE}) target_code_coverage(${SELF_EXE} AUTO ALL) # ---------------------------------------------------------------- -# create convenience symlink to canned data +# create convenience symlink from build dir back to canned data. +# This is ok since we don't implement install for unit tests #create_symlink("${CMAKE_SOURCE_DIR}/utestdata/" "src/filter/utest/utestdata") +file(MAKE_DIRECTORY ${PROJECT_BINARY_DIR}/utest/utestdata) +file(CREATE_LINK ${PROJECT_SOURCE_DIR}/utest/utestdata/filter ${PROJECT_BINARY_DIR}/utest/utestdata/filter SYMBOLIC) # ---------------------------------------------------------------- # generic project dependency diff --git a/utest/KalmanFilter.test.cpp b/utest/KalmanFilter.test.cpp index 679f461f..c0b35601 100644 --- a/utest/KalmanFilter.test.cpp +++ b/utest/KalmanFilter.test.cpp @@ -29,9 +29,6 @@ namespace xo { using xo::ref::rp; using xo::log_level; using logutil::matrix; - //using logutil::scope; - //using logutil::tostr; - //using logutil::xtag; using xo::print::ccs; using Eigen::MatrixXd; using Eigen::VectorXd; diff --git a/utest/utestdata/filter/kalman-revert1 b/utest/utestdata/filter/kalman-revert1 new file mode 100644 index 00000000..343af232 --- /dev/null +++ b/utest/utestdata/filter/kalman-revert1 @@ -0,0 +1,100 @@ +step z0 x0 P00 K0 +1 1.33055 1.28103 0.195775 0.783099 +2 1.26559 1.2664 0.103557 0.414227 +3 0.790814 1.1272 0.0680813 0.272325 +4 0.173362 0.933668 0.0493859 0.197543 +5 2.12197 1.11662 0.0378989 0.151595 +6 1.00197 1.09766 0.0301647 0.120659 +7 -0.312247 0.954347 0.0246315 0.0985261 +8 1.76439 1.02286 0.020499 0.081996 +9 0.518422 0.986867 0.0173123 0.0692491 +10 0.756683 0.973864 0.0147938 0.0591754 +11 -0.609708 0.894249 0.0127646 0.0510585 +12 0.330435 0.874259 0.011104 0.0444159 +13 1.03713 0.886639 0.00972751 0.03891 +14 -0.994029 0.827609 0.00857454 0.0342982 +15 1.58353 0.858947 0.00760022 0.0304009 +16 0.494725 0.855945 0.00677073 0.0270829 +17 1.0205 0.866962 0.00606004 0.0242401 +18 1.98651 0.897865 0.00544782 0.0217913 +19 1.70559 0.918761 0.00491797 0.0196719 +20 1.44881 0.932201 0.00445755 0.0178302 +21 2.16318 0.955508 0.00405605 0.0162242 +22 0.353787 0.948782 0.00370485 0.0148194 +23 1.72676 0.961879 0.00339684 0.0135874 +24 -1.55312 0.932313 0.00312606 0.0125043 +25 -0.523229 0.918847 0.00288753 0.0115501 +26 1.81676 0.932476 0.00267702 0.0107081 +27 1.65387 0.943006 0.00249094 0.00996377 +28 -0.766534 0.929922 0.00232623 0.00930491 +29 1.1143 0.935004 0.00218024 0.00872095 +30 0.908743 0.938011 0.0020507 0.00820282 +31 1.80832 0.947825 0.00193566 0.00774263 +32 1.61306 0.955293 0.00183339 0.00733354 +33 1.10591 0.958563 0.0017424 0.00696961 +34 1.54404 0.964512 0.0016614 0.00664561 +35 1.47595 0.969526 0.00158925 0.00635699 +36 -1.16643 0.958012 0.00152494 0.00609975 +37 2.772 0.970748 0.00146759 0.00587036 +38 0.0147533 0.966786 0.00141643 0.00566572 +39 1.5648 0.971716 0.00137077 0.00548308 +40 0.758209 0.971987 0.00133001 0.00532003 +41 0.602221 0.971467 0.0012936 0.00517441 +42 1.02795 0.973171 0.00126108 0.00504433 +43 1.61126 0.977651 0.00123203 0.0049281 +44 3.23237 0.98964 0.00120606 0.00482423 +45 -1.20629 0.979766 0.00118284 0.00473137 +46 2.34054 0.987098 0.00116209 0.00464835 +47 1.886 0.991852 0.00114353 0.00457412 +48 1.62241 0.9951 0.00112693 0.00450773 +49 0.10733 0.991395 0.00111209 0.00444835 +50 0.111089 0.987954 0.00109881 0.00439523 +51 -0.045289 0.984062 0.00108693 0.00434771 +52 0.339478 0.98208 0.0010763 0.0043052 +53 -0.109018 0.978316 0.00106679 0.00426715 +54 -1.15684 0.970358 0.00105828 0.00423311 +55 -0.289587 0.966538 0.00105066 0.00420265 +56 1.04772 0.968543 0.00104385 0.00417538 +57 -0.0931435 0.965703 0.00103774 0.00415098 +58 2.56415 0.974011 0.00103228 0.00412914 +59 0.352559 0.972751 0.0010274 0.00410959 +60 0.173725 0.970838 0.00102302 0.00409209 +61 0.80765 0.971625 0.00101911 0.00407643 +62 2.41789 0.978913 0.0010156 0.0040624 +63 2.63322 0.986663 0.00101246 0.00404985 +64 2.59753 0.993833 0.00100965 0.00403861 +65 0.530642 0.992274 0.00100714 0.00402855 +66 1.24782 0.993686 0.00100489 0.00401955 +67 0.695031 0.992802 0.00100287 0.00401149 +68 2.80672 1.00042 0.00100107 0.00400427 +69 0.122651 0.996894 0.000999451 0.0039978 +70 1.22154 0.997945 0.000998005 0.00399202 +71 0.269797 0.995145 0.00099671 0.00398684 +72 1.78662 0.998538 0.00099555 0.0039822 +73 0.4817 0.996555 0.000994512 0.00397805 +74 1.05221 0.996948 0.000993582 0.00397433 +75 1.75822 1.00012 0.00099275 0.003971 +76 1.31536 1.00137 0.000992005 0.00396802 +77 1.29006 1.00244 0.000991338 0.00396535 +78 0.857458 1.00175 0.000990741 0.00396296 +79 0.269498 0.998761 0.000990206 0.00396082 +80 0.131184 0.995388 0.000989727 0.00395891 +81 0.736283 0.994592 0.000989298 0.00395719 +82 -0.32478 0.989642 0.000988914 0.00395566 +83 1.3352 0.991525 0.00098857 0.00395428 +84 0.279734 0.989133 0.000988263 0.00395305 +85 1.74807 0.992673 0.000987987 0.00395195 +86 1.82522 0.996328 0.00098774 0.00395096 +87 -0.281213 0.991464 0.000987519 0.00395008 +88 1.61161 0.994338 0.000987322 0.00394929 +89 2.63571 1.0011 0.000987144 0.00394858 +90 -0.139888 0.996542 0.000986986 0.00394794 +91 3.48906 1.00655 0.000986844 0.00394738 +92 2.01113 1.01019 0.000986717 0.00394687 +93 1.84914 1.01299 0.000986603 0.00394641 +94 0.480262 1.01025 0.000986501 0.003946 +95 1.45288 1.01148 0.00098641 0.00394564 +96 -0.0838873 1.00659 0.000986328 0.00394531 +97 -0.570744 1.00004 0.000986255 0.00394502 +98 -0.214659 0.995244 0.000986189 0.00394476 +99 1.7904 0.998618 0.000986131 0.00394452 From d7a884f651d0d57d0e099149f68339f8711a6caa Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Mon, 23 Oct 2023 20:29:14 -0400 Subject: [PATCH 05/25] bugfix: compile fix (include path ) --- utest/KalmanFilter.test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utest/KalmanFilter.test.cpp b/utest/KalmanFilter.test.cpp index c0b35601..fea572ec 100644 --- a/utest/KalmanFilter.test.cpp +++ b/utest/KalmanFilter.test.cpp @@ -3,7 +3,7 @@ #include "xo/kalmanfilter/KalmanFilter.hpp" #include "xo/kalmanfilter/KalmanFilterEngine.hpp" #include "xo/kalmanfilter/print_eigen.hpp" -#include "statistics/SampleStatistics.hpp" +#include "xo/statistics/SampleStatistics.hpp" #include "xo/randomgen/normalgen.hpp" #include "xo/randomgen/xoshiro256.hpp" #include "xo/indentlog/scope.hpp" From ba0b989b6dab3cf8bb099d0f73ac2e63cb8bbe10 Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Tue, 24 Oct 2023 22:08:35 -0400 Subject: [PATCH 06/25] build: bugfix for .cmake eigen dep --- cmake/xo_kalmanfilterConfig.cmake.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/xo_kalmanfilterConfig.cmake.in b/cmake/xo_kalmanfilterConfig.cmake.in index 366e45ba..950e70c0 100644 --- a/cmake/xo_kalmanfilterConfig.cmake.in +++ b/cmake/xo_kalmanfilterConfig.cmake.in @@ -7,7 +7,7 @@ include(CMakeFindDependencyMacro) # in xo-reactor/src/reactor/CMakeLists.txt # find_dependency(reactor) -find_dependnecy(eigen3) +find_dependency(Eigen3) #find_dependency(reflect) #find_dependency(webutil) #find_dependency(printjson) From 45a81a2a7a77b87fe0703834390b5eb34e8a30b0 Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Tue, 24 Oct 2023 22:19:57 -0400 Subject: [PATCH 07/25] + .gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..378eac25 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build From 31105757e6e41b5f5438dd1a2fab3656acb01fab Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Tue, 24 Oct 2023 22:20:14 -0400 Subject: [PATCH 08/25] tidy -- remove dead model file --- CMakeLists.txt.old | 37 ------------------------------------- 1 file changed, 37 deletions(-) delete mode 100644 CMakeLists.txt.old diff --git a/CMakeLists.txt.old b/CMakeLists.txt.old deleted file mode 100644 index e85d245f..00000000 --- a/CMakeLists.txt.old +++ /dev/null @@ -1,37 +0,0 @@ -# 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 From 4b2a991dc661b1c466fafe9b5ffdd12c06a32d3d Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Fri, 15 Mar 2024 19:41:08 -0400 Subject: [PATCH 09/25] build: streamline CMAKE_MODULE_PATH interaction --- .gitignore | 7 ++++++- CMakeLists.txt | 3 +-- cmake/xo-bootstrap-macros.cmake | 12 ++++++++++++ 3 files changed, 19 insertions(+), 3 deletions(-) create mode 100644 cmake/xo-bootstrap-macros.cmake diff --git a/.gitignore b/.gitignore index 378eac25..13c0afb7 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,6 @@ -build +# clangd working space (see emacs+lsp) +.cache +# typical cmake build directory (source-tree-nephew) +.build* +# symlink to builddir/compile_commands.json; should be set manually in dev sandbox +compile_commands.json diff --git a/CMakeLists.txt b/CMakeLists.txt index f4120636..3c83fec9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,8 +6,7 @@ project(xo_kalmanfilter VERSION 1.0) enable_language(CXX) # common XO cmake macros (see proj/xo-cmake) -include(xo_macros/xo_cxx) -include(xo_macros/code-coverage) +include(cmake/xo-bootstrap-macros.cmake) # ---------------------------------------------------------------- # unit test setup diff --git a/cmake/xo-bootstrap-macros.cmake b/cmake/xo-bootstrap-macros.cmake new file mode 100644 index 00000000..16644435 --- /dev/null +++ b/cmake/xo-bootstrap-macros.cmake @@ -0,0 +1,12 @@ +if (("${CMAKE_MODULE_PATH}" STREQUAL "") OR ("${CMAKE_MODULE_PATH}" STREQUAL "prefix")) + # default to typical install location for xo-project-macros + set(CMAKE_MODULE_PATH ${CMAKE_INSTALL_PREFIX}/share/cmake) +endif() + +message("-- CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}") +message("-- CMAKE_MODULE_PATH=${CMAKE_MODULE_PATH}") + +# needs to have been installed somewhere on CMAKE_MODULE_PATH, +# (e.g. from xo-cmake with the same value for CMAKE_INSTALL_PREFIX) +# +include(xo_macros/xo-project-macros) From c8f17169d95504e89634bb4e57668fdcc65e2d49 Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 16 Mar 2024 13:04:46 -0400 Subject: [PATCH 10/25] xo-kalmanfilter: + README.md --- README.md | 56 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 00000000..fd4ef52d --- /dev/null +++ b/README.md @@ -0,0 +1,56 @@ +# kalman filter library + +linear kalman filter implementation. + +## Getting Started + +### build + install dependencies + +build+install these first + +- xo-reactor [github.com/Rconybea/xo-reactor](https://github.com/Rconybea/xo-reactor) + +See `.github/workflows/main.yml` in this repo for example build+install on ubuntu + +### build + install xo-kalmanfilter +``` +$ cd xo-kalmanfilter +$ mkdir build +$ cd build +$ INSTALL_PREFIX=/usr/local # or wherever you prefer +$ cmake -DCMAKE_MODULE_PATH=${INSTALL_PREFIX}/share/cmake -DCMAKE_PREFIX_PATH=${INSTALL_PREFIX} -DCMAKE_INSTALL_PREFIX=${INSTALL_PREFIX} .. +$ make +$ make install +``` +(also see .github/workflows/main.yml) + +### build for unit test coverage +``` +$ cd xo-kalmanfilter +$ mkdir ccov +$ cd ccov +$ cmake -DCMAKE_MODULE_PATH=${INSTALL_PREFIX}/share/cmake -DCMAKE_PREFIX_PATH=${INSTALL_PREFIX} -DCODE_COVERAGE=ON -DCMAKE_BUILD_TYPE=Debug .. +``` + +## Development + +### LSP support + +LSP looks for compile commands in the root of the source tree; +cmake creates them in the root of its build directory. + +``` +$ cd xo-kalmanfilter +$ ln -s build/compile_commands.json +``` + +### display cmake variables + +- `-L` list variables +- `-A` include 'advanced' variables +- `-H` include help text + +``` +$ cd xo-kalmanfilter/build +$ cmake -LAH +``` From 637e0902437318a026a7f5015a7497bb09bf8b2d Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 16 Mar 2024 13:06:44 -0400 Subject: [PATCH 11/25] github: worfklow for ubuntu build --- .github/workflows/main.yml | 309 +++++++++++++++++++++++++++++++++++++ 1 file changed, 309 insertions(+) create mode 100644 .github/workflows/main.yml diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 00000000..a34ea6aa --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,309 @@ +name: build xo-optionutil + dependencies + +on: + push: + branches: [ "main" ] + pull_request: + branches: [ "main" ] + +env: + # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) + BUILD_TYPE: Release + +jobs: + build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix + runs-on: ubuntu-latest + + steps: + - name: checkout source + uses: actions/checkout@v3 + + - name: Install dependencies + run: | + echo "::group::install catch2" + # install catch2. see + # [[https://stackoverflow.com/questions/57982945/how-to-apt-get-install-in-a-github-actions-workflow]] + sudo apt-get install -y catch2 + echo "::endgroup" + + #echo "::group::install pybind11" + #sudo apt-get install -y pybind11-dev + #echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-cmake + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-cmake + path: repo/xo-cmake + + - name: build xo-cmake + run: | + XONAME=xo-cmake + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-indentlog + uses: actions/checkout@v3 + with: + repository: Rconybea/indentlog + path: repo/xo-indentlog + + - name: build xo-indentlog + run: | + XONAME=xo-indentlog + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-refcnt + uses: actions/checkout@v3 + with: + repository: Rconybea/refcnt + path: repo/xo-refcnt + + - name: build xo-refcnt + run: | + XONAME=xo-refcnt + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-subsys + uses: actions/checkout@v3 + with: + repository: Rconybea/subsys + path: repo/xo-subsys + + - name: build xo-subsys + run: | + XONAME=xo-subsys + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-reflect + uses: actions/checkout@v3 + with: + repository: Rconybea/reflect + path: repo/xo-reflect + + - name: build xo-reflect + run: | + XONAME=xo-reflect + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + +# - name: clone xo-callback +# uses: actions/checkout@v3 +# with: +# repository: Rconybea/xo-callback +# path: repo/xo-callback +# +# - name: build xo-callback +# run: | +# XONAME=xo-callback +# XOSRC=repo/${XONAME} +# BUILDDIR=${{github.workspace}}/build_${XONAME} +# PREFIX=${{github.workspace}}/local +# +# echo "::group::repo dir tree" +# tree -L 2 repo +# echo "::endgroup" +# +# echo "::group::configure ${XONAME}" +# cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} +# echo "::endgroup" +# +# echo "::group::compile ${XONAME}" +# cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j +# echo "::endgroup" +# +# echo "::group::local install ${XONAME}" +# cmake --install ${BUILDDIR} +# echo "::endgroup" +# +# echo "::group::local dir tree" +# tree -L 3 ${PREFIX} +# echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: clone xo-printjson + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-printjson + path: repo/xo-printjson + + - name: build xo-printjson + run: | + XONAME=xo-printjson + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + + - name: Configure self (kalmanfilter) + # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. + # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type + run: | + XONAME=xo-kalmanfilter + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_MODULE_PATH=${PREFIX}/share/cmake -DCMAKE_PREFIX_PATH=${PREFIX} -DCMAKE_INSTALL_PREFIX=${PREFIX} -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # Execute tests defined by the CMake configuration. + # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail + (cd ${BUILDDIR} && ctest -C ${{env.BUILD_TYPE}}) From ce1ed005a5f419d97176cf8a3944cbc0e49a5725 Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 16 Mar 2024 13:11:56 -0400 Subject: [PATCH 12/25] worfklow: + xo-reactor dep --- .github/workflows/main.yml | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index a34ea6aa..d99c8cb7 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -276,6 +276,41 @@ jobs: # ---------------------------------------------------------------- + - name: clone xo-reactor + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-reactor + path: repo/xo-reactor + + - name: build xo-reactor + run: | + XONAME=xo-reactor + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + - name: Configure self (kalmanfilter) # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type From 856e2f8de2a80f6b4408140601220632c00bcedd Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 16 Mar 2024 13:15:13 -0400 Subject: [PATCH 13/25] worfklow: + xo-webutil dep --- .github/workflows/main.yml | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index d99c8cb7..38bd87d6 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -276,6 +276,41 @@ jobs: # ---------------------------------------------------------------- + - name: clone xo-webutil + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-webutil + path: repo/xo-webutil + + - name: build xo-webutil + run: | + XONAME=xo-webutil + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + - name: clone xo-reactor uses: actions/checkout@v3 with: From 661f52373e9d3c1f447c27a60bcf01bcacdc030b Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 16 Mar 2024 13:17:33 -0400 Subject: [PATCH 14/25] worfklow: + xo-callback dep --- .github/workflows/main.yml | 64 +++++++++++++++++++------------------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 38bd87d6..3060d6ab 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -206,38 +206,38 @@ jobs: # ---------------------------------------------------------------- -# - name: clone xo-callback -# uses: actions/checkout@v3 -# with: -# repository: Rconybea/xo-callback -# path: repo/xo-callback -# -# - name: build xo-callback -# run: | -# XONAME=xo-callback -# XOSRC=repo/${XONAME} -# BUILDDIR=${{github.workspace}}/build_${XONAME} -# PREFIX=${{github.workspace}}/local -# -# echo "::group::repo dir tree" -# tree -L 2 repo -# echo "::endgroup" -# -# echo "::group::configure ${XONAME}" -# cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} -# echo "::endgroup" -# -# echo "::group::compile ${XONAME}" -# cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j -# echo "::endgroup" -# -# echo "::group::local install ${XONAME}" -# cmake --install ${BUILDDIR} -# echo "::endgroup" -# -# echo "::group::local dir tree" -# tree -L 3 ${PREFIX} -# echo "::endgroup" + - name: clone xo-callback + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-callback + path: repo/xo-callback + + - name: build xo-callback + run: | + XONAME=xo-callback + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" # ---------------------------------------------------------------- From 12df1e773bc9f28a57822378d7f769e56d948819 Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 16 Mar 2024 13:20:19 -0400 Subject: [PATCH 15/25] worfklow: + xo-ordinaltree dep --- .github/workflows/main.yml | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 3060d6ab..4de8815b 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -311,6 +311,41 @@ jobs: # ---------------------------------------------------------------- + - name: clone xo-ordinaltree + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-ordinaltree + path: repo/xo-ordinaltree + + - name: build xo-ordinaltree + run: | + XONAME=xo-ordinaltree + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + - name: clone xo-reactor uses: actions/checkout@v3 with: From 9157edb3f90d3a453de0166b9ecb71d56fe1d3be Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 16 Mar 2024 13:24:34 -0400 Subject: [PATCH 16/25] workflows: + xo-randomgen dep --- .github/workflows/main.yml | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 4de8815b..e0d1e7d4 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -311,6 +311,41 @@ jobs: # ---------------------------------------------------------------- + - name: clone xo-randomgen + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-randomgen + path: repo/xo-randomgen + + - name: build xo-randomgen + run: | + XONAME=xo-randomgen + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + - name: clone xo-ordinaltree uses: actions/checkout@v3 with: From 9e19cb85823aafc474afe593c6a3ea3443c12d28 Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 16 Mar 2024 13:32:51 -0400 Subject: [PATCH 17/25] worfklow: bugfix: repo name for xo-randomgen --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index e0d1e7d4..a5ec0bb5 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -314,7 +314,7 @@ jobs: - name: clone xo-randomgen uses: actions/checkout@v3 with: - repository: Rconybea/xo-randomgen + repository: Rconybea/randomgen path: repo/xo-randomgen - name: build xo-randomgen From a70370d239bfb78e930d290cf905eac4c43a768b Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 16 Mar 2024 13:43:42 -0400 Subject: [PATCH 18/25] workflows: + eigen3 dep --- .github/workflows/main.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index a5ec0bb5..f6a824ec 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -29,6 +29,10 @@ jobs: sudo apt-get install -y catch2 echo "::endgroup" + echo "::group::install eigen3" + sudo apt-get install -y eigen3-dev + echo "::endgroup" + #echo "::group::install pybind11" #sudo apt-get install -y pybind11-dev #echo "::endgroup" From 70f53491381c4dd5736175b104f5d150b25a77db Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 16 Mar 2024 13:45:24 -0400 Subject: [PATCH 19/25] workflow: bugfix: eigen3-dev -> libeigen3-dev --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index f6a824ec..ee59661d 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -30,7 +30,7 @@ jobs: echo "::endgroup" echo "::group::install eigen3" - sudo apt-get install -y eigen3-dev + sudo apt-get install -y libeigen3-dev echo "::endgroup" #echo "::group::install pybind11" From da011d38cf4713c02a2e6bb9676ed3cd126756fa Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 16 Mar 2024 14:18:11 -0400 Subject: [PATCH 20/25] workflow: + xo-statistics dep --- .github/workflows/main.yml | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index ee59661d..3c4faa53 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -420,6 +420,41 @@ jobs: # ---------------------------------------------------------------- + - name: clone xo-statistics + uses: actions/checkout@v3 + with: + repository: Rconybea/xo-statistics + path: repo/xo-statistics + + - name: build xo-statistics + run: | + XONAME=xo-statistics + XOSRC=repo/${XONAME} + BUILDDIR=${{github.workspace}}/build_${XONAME} + PREFIX=${{github.workspace}}/local + + echo "::group::repo dir tree" + tree -L 2 repo + echo "::endgroup" + + echo "::group::configure ${XONAME}" + cmake -B ${BUILDDIR} -DCMAKE_INSTALL_PREFIX=${PREFIX} ${XOSRC} + echo "::endgroup" + + echo "::group::compile ${XONAME}" + cmake --build ${BUILDDIR} --config ${{env.BUILD_TYPE}} -j + echo "::endgroup" + + echo "::group::local install ${XONAME}" + cmake --install ${BUILDDIR} + echo "::endgroup" + + echo "::group::local dir tree" + tree -L 3 ${PREFIX} + echo "::endgroup" + + # ---------------------------------------------------------------- + - name: Configure self (kalmanfilter) # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type From 591c2451d86fe0568e4022da5b3afc72af1e69ef Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Fri, 29 Mar 2024 14:33:41 -0400 Subject: [PATCH 21/25] build: suppress bootstrap message in submodule build --- cmake/xo-bootstrap-macros.cmake | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/cmake/xo-bootstrap-macros.cmake b/cmake/xo-bootstrap-macros.cmake index 16644435..96592216 100644 --- a/cmake/xo-bootstrap-macros.cmake +++ b/cmake/xo-bootstrap-macros.cmake @@ -3,8 +3,10 @@ if (("${CMAKE_MODULE_PATH}" STREQUAL "") OR ("${CMAKE_MODULE_PATH}" STREQUAL "pr set(CMAKE_MODULE_PATH ${CMAKE_INSTALL_PREFIX}/share/cmake) endif() -message("-- CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}") -message("-- CMAKE_MODULE_PATH=${CMAKE_MODULE_PATH}") +if (NOT XO_SUBMODULE_BUILD) + message("-- CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}") + message("-- CMAKE_MODULE_PATH=${CMAKE_MODULE_PATH}") +endif() # needs to have been installed somewhere on CMAKE_MODULE_PATH, # (e.g. from xo-cmake with the same value for CMAKE_INSTALL_PREFIX) From a5f060f3580b47426008a4cc68b6fec3f8376b73 Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Fri, 29 Mar 2024 14:51:22 -0400 Subject: [PATCH 22/25] cosmetic: whitespace --- include/xo/kalmanfilter/KalmanFilterStep.hpp | 202 +++++++++---------- src/kalmanfilter/CMakeLists.txt | 9 +- 2 files changed, 105 insertions(+), 106 deletions(-) diff --git a/include/xo/kalmanfilter/KalmanFilterStep.hpp b/include/xo/kalmanfilter/KalmanFilterStep.hpp index 89813cc2..432ac570 100644 --- a/include/xo/kalmanfilter/KalmanFilterStep.hpp +++ b/include/xo/kalmanfilter/KalmanFilterStep.hpp @@ -8,121 +8,121 @@ #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)} {} + 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_; } + /* 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*/ + 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; + /* 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 state, - KalmanFilterTransition model, - KalmanFilterObservable obs, - ref::rp zkp1) - : KalmanFilterStepBase(model, obs), - state_{std::move(state)}, - input_{std::move(zkp1)} {} - - ref::rp const & state() const { return state_; } - ref::rp const & input() const { return input_; } + public: + KalmanFilterStep() = default; + KalmanFilterStep(ref::rp state, + KalmanFilterTransition model, + KalmanFilterObservable obs, + ref::rp zkp1) + : KalmanFilterStepBase(model, obs), + state_{std::move(state)}, + input_{std::move(zkp1)} {} - utc_nanos tkp1() const { return input_->tkp1(); } + ref::rp const & state() const { return state_; } + ref::rp const & input() const { return input_; } - /* 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 extrapolate() const; + utc_nanos tkp1() const { return input_->tkp1(); } - /* 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 const & skp1_ext) const; + /* 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 extrapolate() 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 const & skp1_ext, - uint32_t j) 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 const & skp1_ext) const; - /* compute correction to extrapolated filter state {x(k+1|k), P(k+1|k)}, - * for observation z(k+1) = .input.z() - */ - ref::rp correct(ref::rp const & skp1_ext); + /* 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 const & skp1_ext, + uint32_t j) const; - /* 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 correct1(ref::rp const & skp1_ext, - uint32_t j); + /* compute correction to extrapolated filter state {x(k+1|k), P(k+1|k)}, + * for observation z(k+1) = .input.z() + */ + ref::rp correct(ref::rp const & skp1_ext); - void display(std::ostream & os) const; - std::string display_string() const; + /* 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 correct1(ref::rp const & skp1_ext, + uint32_t j); - private: - /* system state: timestamp, estimated process state, process covariance - * asof beginning of this step - */ - ref::rp state_; - /* input: observations at time t(k+1) */ - KalmanFilterInputPtr input_; - }; /*KalmanFilterStep*/ + void display(std::ostream & os) const; + std::string display_string() const; - inline std::ostream & - operator<<(std::ostream & os, KalmanFilterStep const & x) { - x.display(os); - return os; - } /*operator<<*/ + private: + /* system state: timestamp, estimated process state, process covariance + * asof beginning of this step + */ + ref::rp state_; + /* input: observations at time t(k+1) */ + KalmanFilterInputPtr input_; + }; /*KalmanFilterStep*/ - } /*namespace kalman*/ + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterStep const & x) { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ } /*namespace xo*/ /* end KalmanFilterStep.hpp */ diff --git a/src/kalmanfilter/CMakeLists.txt b/src/kalmanfilter/CMakeLists.txt index b7036526..882db194 100644 --- a/src/kalmanfilter/CMakeLists.txt +++ b/src/kalmanfilter/CMakeLists.txt @@ -21,11 +21,10 @@ set(SELF_SRCS xo_add_shared_library4(${SELF_LIB} ${PROJECT_NAME}Targets ${PROJECT_VERSION} 1 ${SELF_SRCS}) # ---------------------------------------------------------------- -# internal dependencies +# Dependencies +# +# REMINDER: must coordinate with find_dependency() calls in +# [xo-kalmanfilter/cmake/xo_kalmanfilterConfig.cmake.in] xo_dependency(${SELF_LIB} reactor) - -# ---------------------------------------------------------------- -# external dependencies - xo_external_target_dependency(${SELF_LIB} Eigen3 Eigen3::Eigen) From 83810d6c0048424a527e93484bf328d5deb0ce89 Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Mon, 1 Apr 2024 19:49:51 -0400 Subject: [PATCH 23/25] build: comments in CMakeLists.txt --- src/kalmanfilter/CMakeLists.txt | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/kalmanfilter/CMakeLists.txt b/src/kalmanfilter/CMakeLists.txt index b7036526..2dbdcff0 100644 --- a/src/kalmanfilter/CMakeLists.txt +++ b/src/kalmanfilter/CMakeLists.txt @@ -21,11 +21,10 @@ set(SELF_SRCS xo_add_shared_library4(${SELF_LIB} ${PROJECT_NAME}Targets ${PROJECT_VERSION} 1 ${SELF_SRCS}) # ---------------------------------------------------------------- -# internal dependencies - +# Dependencies +# +# REMINDER: these must coodinate with find_dependency() calls in +# [xo-kalmanfilter/cmake/xo_kalmanfilterConfig.cmake.in] +# xo_dependency(${SELF_LIB} reactor) - -# ---------------------------------------------------------------- -# external dependencies - xo_external_target_dependency(${SELF_LIB} Eigen3 Eigen3::Eigen) From e140c6d2858a89b6d1402e44e4e2fbca9d9a5fe6 Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 14 Sep 2024 19:15:11 -0500 Subject: [PATCH 24/25] xo-kalmanfilter: build: update to latest xo-cmake macros --- CMakeLists.txt | 20 ++---------------- cmake/xo-bootstrap-macros.cmake | 33 ++++++++++++++++++++++++------ utest/CMakeLists.txt | 36 +-------------------------------- 3 files changed, 30 insertions(+), 59 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c83fec9..a49c8cb2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,25 +3,11 @@ cmake_minimum_required(VERSION 3.10) project(xo_kalmanfilter VERSION 1.0) -enable_language(CXX) -# common XO cmake macros (see proj/xo-cmake) +include(GNUInstallDirs) include(cmake/xo-bootstrap-macros.cmake) -# ---------------------------------------------------------------- -# unit test setup - -enable_testing() -# activate code coverage for all executables + libraries (when configured with -DCODE_COVERAGE=ON) -add_code_coverage() -# 1. assuming that /nix/store/ prefixes .hpp files belonging to gcc, catch2 etc. -# we're not interested in code coverage for these sources. -# 2. exclude the utest/ subdir, we don't need coverage on the unit tests themselves; -# rather, want coverage on the code that the unit tests exercise. -# -# NOTE: this seems to work only with the 'ccov-all' target. In particular, doesn't seem to do anything with the 'ccov' target -# -add_code_coverage_all_targets(EXCLUDE /nix/store/* ${PROJECT_SOURCE_DIR}/utest/* ${PROJECT_BINARY_DIR}/local/* ${PROJECT_SOURCE_DIR}/repo/*) +xo_cxx_toplevel_options3() # ---------------------------------------------------------------- # c++ settings @@ -31,8 +17,6 @@ set(PROJECT_CXX_FLAGS "") #set(PROJECT_CXX_FLAGS "-fconcepts-diagnostics-depth=2") add_definitions(${PROJECT_CXX_FLAGS}) -xo_toplevel_compile_options() - # ---------------------------------------------------------------- add_subdirectory(src/kalmanfilter) diff --git a/cmake/xo-bootstrap-macros.cmake b/cmake/xo-bootstrap-macros.cmake index 96592216..aba31169 100644 --- a/cmake/xo-bootstrap-macros.cmake +++ b/cmake/xo-bootstrap-macros.cmake @@ -1,14 +1,35 @@ -if (("${CMAKE_MODULE_PATH}" STREQUAL "") OR ("${CMAKE_MODULE_PATH}" STREQUAL "prefix")) - # default to typical install location for xo-project-macros - set(CMAKE_MODULE_PATH ${CMAKE_INSTALL_PREFIX}/share/cmake) +# ---------------------------------------------------------------- +# for example: +# $ PREFIX=/usr/local # for example +# $ cmake -DCMAKE_MODULE_PATH=prefix -DCMAKE_INSTALL_PREFIX=$PREFIX -B .build +# +# will get +# CMAKE_MODULE_PATH +# from xo-cmake-config --cmake-module-path +# +# and expect .cmake macros in +# CMAKE_MODULE_PATH/xo_macros/xo_cxx.cmake +# ---------------------------------------------------------------- + +find_program(XO_CMAKE_CONFIG_EXECUTABLE NAMES xo-cmake-config REQUIRED) + +if ("${XO_CMAKE_CONFIG_EXECUTABLE}" STREQUAL "XO_CMAKE_CONFIG_EXECUTABLE-NOT_FOUND") + message(FATAL "could not find xo-cmake-config executable") endif() +message(STATUS "XO_CMAKE_CONFIG_EXECUTABLE=${XO_CMAKE_CONFIG_EXECUTABLE}") + if (NOT XO_SUBMODULE_BUILD) - message("-- CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}") - message("-- CMAKE_MODULE_PATH=${CMAKE_MODULE_PATH}") + if (("${CMAKE_MODULE_PATH}" STREQUAL "") OR ("${CMAKE_MODULE_PATH}" STREQUAL prefix)) + # default to typical install location for xo-project-macros + execute_process(COMMAND ${XO_CMAKE_CONFIG_EXECUTABLE} --cmake-module-path OUTPUT_VARIABLE CMAKE_MODULE_PATH) + message(STATUS "CMAKE_MODULE_PATH=${CMAKE_MODULE_PATH}") + endif() endif() # needs to have been installed somewhere on CMAKE_MODULE_PATH, # (e.g. from xo-cmake with the same value for CMAKE_INSTALL_PREFIX) # -include(xo_macros/xo-project-macros) +include(xo_macros/xo_cxx) + +xo_cxx_bootstrap_message() diff --git a/utest/CMakeLists.txt b/utest/CMakeLists.txt index a118b8a7..1cd562ad 100644 --- a/utest/CMakeLists.txt +++ b/utest/CMakeLists.txt @@ -6,11 +6,7 @@ set(SELF_SRCS KalmanFilter.test.cpp filter_utest_main.cpp) -add_executable(${SELF_EXE} ${SELF_SRCS}) -xo_include_options2(${SELF_EXE}) - -add_test(NAME ${SELF_EXE} COMMAND ${SELF_EXE}) -target_code_coverage(${SELF_EXE} AUTO ALL) +xo_add_utest_executable(${SELF_EXE} ${SELF_SRCS}) # ---------------------------------------------------------------- # create convenience symlink from build dir back to canned data. @@ -20,38 +16,8 @@ target_code_coverage(${SELF_EXE} AUTO ALL) file(MAKE_DIRECTORY ${PROJECT_BINARY_DIR}/utest/utestdata) file(CREATE_LINK ${PROJECT_SOURCE_DIR}/utest/utestdata/filter ${PROJECT_BINARY_DIR}/utest/utestdata/filter SYMBOLIC) -# ---------------------------------------------------------------- -# generic project dependency - -# PROJECT_SOURCE_DIR: -# so we can for example write -# #include "logutil/scope.hpp" -# from anywhere in the project -# PROJECT_BINARY_DIR: -# since version file will be in build directory, need that directory -# to also be included in compiler's include path -# xo_self_dependency(${SELF_EXE} xo_kalmanfilter) - -# ---------------------------------------------------------------- -# internal dependencies: logutil, ... - xo_dependency(${SELF_EXE} xo_statistics) - -# ---------------------------------------------------------------- -# external dependencies: catch2.. - xo_external_target_dependency(${SELF_EXE} Catch2 Catch2::Catch2) -# ---------------------------------------------------------------- -# make standard directories for std:: includes explicit -# so that -# (1) they appear in compile_commands.json. -# (2) clangd (run from emacs lsp-mode) can find them -# -if(CMAKE_EXPORT_COMPILE_COMMANDS) - set(CMAKE_CXX_STANDARD_INCLUDE_DIRECTORIES - ${CMAKE_CXX_IMPLICIT_INCLUDE_DIRECTORIES}) -endif() - # end CMakeLists.txt From 2ced8429c0c7fbd1b644349283e0a0654dddec61 Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Sat, 14 Sep 2024 19:15:47 -0500 Subject: [PATCH 25/25] xo-kalmanfilter: bugfix: track xo::ref::rp -> xo::rp --- include/xo/kalmanfilter/KalmanFilter.hpp | 6 +-- .../xo/kalmanfilter/KalmanFilterEngine.hpp | 32 +++++++------- include/xo/kalmanfilter/KalmanFilterInput.hpp | 6 +-- .../KalmanFilterInputCallback.hpp | 2 +- .../KalmanFilterInputToConsole.hpp | 4 +- .../KalmanFilterOutputCallback.hpp | 2 +- include/xo/kalmanfilter/KalmanFilterSpec.hpp | 12 ++--- include/xo/kalmanfilter/KalmanFilterState.hpp | 18 ++++---- .../KalmanFilterStateToConsole.hpp | 2 +- include/xo/kalmanfilter/KalmanFilterStep.hpp | 20 ++++----- include/xo/kalmanfilter/KalmanFilterSvc.hpp | 10 ++--- src/kalmanfilter/KalmanFilter.cpp | 2 +- src/kalmanfilter/KalmanFilterEngine.cpp | 44 +++++++++---------- src/kalmanfilter/KalmanFilterInput.cpp | 4 +- .../KalmanFilterInputToConsole.cpp | 2 +- src/kalmanfilter/KalmanFilterState.cpp | 4 +- .../KalmanFilterStateToConsole.cpp | 2 +- src/kalmanfilter/KalmanFilterStep.cpp | 14 +++--- src/kalmanfilter/KalmanFilterSvc.cpp | 4 +- utest/KalmanFilter.test.cpp | 10 ++--- 20 files changed, 100 insertions(+), 100 deletions(-) diff --git a/include/xo/kalmanfilter/KalmanFilter.hpp b/include/xo/kalmanfilter/KalmanFilter.hpp index 0df87a61..896b19ca 100644 --- a/include/xo/kalmanfilter/KalmanFilter.hpp +++ b/include/xo/kalmanfilter/KalmanFilter.hpp @@ -85,7 +85,7 @@ namespace xo { utc_nanos tm() const { return state_ext_->tm(); } KalmanFilterSpec const & filter_spec() const { return filter_spec_; } KalmanFilterStep const & step() const { return step_; } - ref::rp const & state_ext() const { return state_ext_; } + rp 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() @@ -95,7 +95,7 @@ namespace xo { * - .filter_spec_k, .step_k, .state_k updated * for observations in input_kp1 */ - void notify_input(ref::rp const & input_kp1); + void notify_input(rp const & input_kp1); void display(std::ostream & os) const; std::string display_string() const; @@ -112,7 +112,7 @@ namespace xo { /* filter state as of most recent observation; * result of applying KalmanFilterEngine::step() to contents of .step */ - ref::rp state_ext_; + rp state_ext_; }; /*KalmanFilter*/ inline std::ostream & diff --git a/include/xo/kalmanfilter/KalmanFilterEngine.hpp b/include/xo/kalmanfilter/KalmanFilterEngine.hpp index ce21bf3c..5ac6ef9a 100644 --- a/include/xo/kalmanfilter/KalmanFilterEngine.hpp +++ b/include/xo/kalmanfilter/KalmanFilterEngine.hpp @@ -39,8 +39,8 @@ namespace xo { * retval.x = x(k+1|k) * retval.P = P(k+1|k) */ - static ref::rp extrapolate(utc_nanos tkp1, - ref::rp const & sk, + static rp extrapolate(utc_nanos tkp1, + rp const & sk, KalmanFilterTransition const & Fk); /* compute kalman gain matrix for filter step t(k) -> t(k+1) @@ -59,7 +59,7 @@ namespace xo { * Hkp1. relates model state to observable variables, * for step t(k) -> t(k+1) */ - static MatrixXd kalman_gain(ref::rp const & skp1_ext, + static MatrixXd kalman_gain(rp const & skp1_ext, KalmanFilterObservable const & Hkp1); /* compute kalman gain for a single observation z(k)[j]. @@ -80,7 +80,7 @@ namespace xo { * amount of innovation in observable #j, i.e. for difference between * expected and actual value for that observable. */ - static VectorXd kalman_gain1(ref::rp const & skp1_ext, + static VectorXd kalman_gain1(rp const & skp1_ext, KalmanFilterObservable const & Hkp1, uint32_t j); @@ -98,9 +98,9 @@ namespace xo { * * return new filter state+cov */ - static ref::rp correct(ref::rp const & skp1_ext, + static rp correct(rp const & skp1_ext, KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1); + rp const & zkp1); /* correct extrapolated filter state for observation * of j'th filter observable z(k+1)[j] @@ -108,9 +108,9 @@ namespace xo { * Can use this when observation errors are uncorrelated * (i.e. observation error matrix R is diagonal) */ - static ref::rp correct1(ref::rp const & skp1_ext, + static rp correct1(rp const & skp1_ext, KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1, + rp const & zkp1, uint32_t j); /* step filter from t(k) -> t(k+1) @@ -123,11 +123,11 @@ namespace xo { * H (coupling matrix), R (error covar matrix) * zkp1. observations z(k+1) for time t(k+1) */ - static ref::rp step(utc_nanos tkp1, - ref::rp const & sk, + static rp step(utc_nanos tkp1, + rp const & sk, KalmanFilterTransition const & Fk, KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1); + rp const & zkp1); /* step filter from t(k) -> tk(k+1) * same as @@ -136,7 +136,7 @@ namespace xo { * step_spec. encapsulates Fk (transition-related params) * and Q (system noise covar matrix) */ - static ref::rp step(KalmanFilterStep const & step_spec); + static rp step(KalmanFilterStep const & step_spec); /* step filter from t(k) -> t(k+1) * @@ -150,11 +150,11 @@ namespace xo { * j. identifies a single filter observable -- * step will only consume observation z(k+1)[j] */ - static ref::rp step1(utc_nanos tkp1, - ref::rp const & sk, + static rp step1(utc_nanos tkp1, + rp const & sk, KalmanFilterTransition const & Fk, KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1, + rp const & zkp1, uint32_t j); /* step filter from t(k) -> t(k+1) @@ -176,7 +176,7 @@ namespace xo { * j. identifies a single filter observable -- * step will only consume observation z(k+1)[j] */ - static ref::rp step1(KalmanFilterStep const & step_spec, + static rp step1(KalmanFilterStep const & step_spec, uint32_t j); }; /*KalmanFilterEngine*/ } /*namespace kalman*/ diff --git a/include/xo/kalmanfilter/KalmanFilterInput.hpp b/include/xo/kalmanfilter/KalmanFilterInput.hpp index 70fee5af..9319ce58 100644 --- a/include/xo/kalmanfilter/KalmanFilterInput.hpp +++ b/include/xo/kalmanfilter/KalmanFilterInput.hpp @@ -32,13 +32,13 @@ namespace xo { public: KalmanFilterInput() = default; - static ref::rp make(utc_nanos tkp1, + static rp 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 make_present(utc_nanos tkp1, + static rp make_present(utc_nanos tkp1, VectorXd const & z); /* reflect KalmanFilterInput object representation */ @@ -106,7 +106,7 @@ namespace xo { VectorXd Rd_; }; /*KalmanFilterInput*/ - using KalmanFilterInputPtr = ref::rp; + using KalmanFilterInputPtr = rp; inline std::ostream & operator<<(std::ostream & os, KalmanFilterInput const & x) diff --git a/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp b/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp index df86a16b..d89c6b68 100644 --- a/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp +++ b/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp @@ -7,7 +7,7 @@ namespace xo { namespace kalman { - using KalmanFilterInputCallback = reactor::Sink1>; + using KalmanFilterInputCallback = reactor::Sink1>; } /*namespace kalman*/ } /*namespace xo*/ diff --git a/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp b/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp index 7d81226a..bdc00e10 100644 --- a/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp +++ b/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp @@ -8,12 +8,12 @@ namespace xo { namespace kalman { class KalmanFilterInputToConsole - : public xo::reactor::SinkToConsole> + : public xo::reactor::SinkToConsole> { public: KalmanFilterInputToConsole() = default; - static ref::rp make(); + static rp make(); virtual void display(std::ostream & os) const; //virtual std::string display_string() const; diff --git a/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp b/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp index cd858606..69cfb4bc 100644 --- a/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp +++ b/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp @@ -8,7 +8,7 @@ namespace xo { namespace kalman { /* callback for consuming kalman filter output */ - using KalmanFilterOutputCallback = reactor::Sink1>; + using KalmanFilterOutputCallback = reactor::Sink1>; } /*namespace kalman*/ } /*namespace xo*/ diff --git a/include/xo/kalmanfilter/KalmanFilterSpec.hpp b/include/xo/kalmanfilter/KalmanFilterSpec.hpp index 306f298a..3bcca6ad 100644 --- a/include/xo/kalmanfilter/KalmanFilterSpec.hpp +++ b/include/xo/kalmanfilter/KalmanFilterSpec.hpp @@ -40,15 +40,15 @@ namespace xo { * } */ using MkStepFn = std::function const & sk, + (rp const & sk, KalmanFilterInputPtr const & zkp1)>; public: - explicit KalmanFilterSpec(ref::rp s0, MkStepFn mkstepfn) + explicit KalmanFilterSpec(rp s0, MkStepFn mkstepfn) : start_ext_{std::move(s0)}, mk_step_fn_{std::move(mkstepfn)} {} - ref::rp const & start_ext() const { return start_ext_; } + rp const & start_ext() const { return start_ext_; } /* get step parameters (i.e. matrices F, Q, H, R) * for step t(k) -> t(k+1). * @@ -56,8 +56,8 @@ namespace xo { * - to allow time stepping to be observation-driven * - to allow for selective outlier removal */ - KalmanFilterStep make_step(ref::rp const & sk, - ref::rp const & zkp1) { + KalmanFilterStep make_step(rp const & sk, + rp const & zkp1) { return this->mk_step_fn_(sk, zkp1); } /*make_step*/ @@ -66,7 +66,7 @@ namespace xo { private: /* starting state */ - ref::rp start_ext_; + rp start_ext_; /* creates kalman filter step object on demand; * step object specifies inputs to 1 step in discrete diff --git a/include/xo/kalmanfilter/KalmanFilterState.hpp b/include/xo/kalmanfilter/KalmanFilterState.hpp index 504def9c..fe814b9a 100644 --- a/include/xo/kalmanfilter/KalmanFilterState.hpp +++ b/include/xo/kalmanfilter/KalmanFilterState.hpp @@ -24,8 +24,8 @@ namespace xo { using uint32_t = std::uint32_t; public: - static ref::rp make(); - static ref::rp make(uint32_t k, + static rp make(); + static rp make(uint32_t k, utc_nanos tk, VectorXd x, MatrixXd P, @@ -100,18 +100,18 @@ namespace xo { using int32_t = std::int32_t; public: - static ref::rp make(); - static ref::rp make(uint32_t k, + static rp make(); + static rp make(uint32_t k, utc_nanos tk, VectorXd x, MatrixXd P, KalmanFilterTransition transition, MatrixXd K, int32_t j, - ref::rp zk); + rp zk); /* create state object for initial filter state */ - static ref::rp initial(utc_nanos t0, + static rp initial(utc_nanos t0, VectorXd x0, MatrixXd P0); @@ -120,7 +120,7 @@ namespace xo { int32_t observable() const { return j_; } MatrixXd const & gain() const { return K_; } - ref::rp const & zk() const { return zk_; } + rp const & zk() const { return zk_; } virtual void display(std::ostream & os) const override; @@ -137,7 +137,7 @@ namespace xo { KalmanFilterTransition transition, MatrixXd K, int32_t j, - ref::rp zk); + rp zk); private: /* if -1: not used; @@ -155,7 +155,7 @@ namespace xo { /* input leading to state k. * empty for initial state (i.e. when .k is 0) */ - ref::rp zk_; + rp zk_; }; /*KalamnFilterStateExt*/ } /*namespace filter*/ } /*namespace xo*/ diff --git a/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp b/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp index 9d072035..4f86b574 100644 --- a/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp +++ b/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp @@ -13,7 +13,7 @@ namespace xo { public: KalmanFilterStateToConsole() = default; - static ref::rp make(); + static rp make(); virtual void display(std::ostream & os) const; //virtual std::string display_string() const; diff --git a/include/xo/kalmanfilter/KalmanFilterStep.hpp b/include/xo/kalmanfilter/KalmanFilterStep.hpp index 432ac570..781b63a6 100644 --- a/include/xo/kalmanfilter/KalmanFilterStep.hpp +++ b/include/xo/kalmanfilter/KalmanFilterStep.hpp @@ -55,16 +55,16 @@ namespace xo { public: KalmanFilterStep() = default; - KalmanFilterStep(ref::rp state, + KalmanFilterStep(rp state, KalmanFilterTransition model, KalmanFilterObservable obs, - ref::rp zkp1) + rp zkp1) : KalmanFilterStepBase(model, obs), state_{std::move(state)}, input_{std::move(zkp1)} {} - ref::rp const & state() const { return state_; } - ref::rp const & input() const { return input_; } + rp const & state() const { return state_; } + rp const & input() const { return input_; } utc_nanos tkp1() const { return input_->tkp1(); } @@ -74,14 +74,14 @@ namespace xo { * P(k+1|k) * does not use the t(k+1) observations .input.z */ - ref::rp extrapolate() const; + rp 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 const & skp1_ext) const; + MatrixXd gain(rp 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)}, @@ -90,18 +90,18 @@ namespace xo { * 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 const & skp1_ext, + VectorXd gain1(rp 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 correct(ref::rp const & skp1_ext); + rp correct(rp 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 correct1(ref::rp const & skp1_ext, + rp correct1(rp const & skp1_ext, uint32_t j); void display(std::ostream & os) const; @@ -111,7 +111,7 @@ namespace xo { /* system state: timestamp, estimated process state, process covariance * asof beginning of this step */ - ref::rp state_; + rp state_; /* input: observations at time t(k+1) */ KalmanFilterInputPtr input_; }; /*KalmanFilterStep*/ diff --git a/include/xo/kalmanfilter/KalmanFilterSvc.hpp b/include/xo/kalmanfilter/KalmanFilterSvc.hpp index 94702a52..4b63cbee 100644 --- a/include/xo/kalmanfilter/KalmanFilterSvc.hpp +++ b/include/xo/kalmanfilter/KalmanFilterSvc.hpp @@ -15,19 +15,19 @@ namespace xo { * sinks that want to consume KalmanFilterSvc events will use * .attach_sink() (or .add_callback()) */ - class KalmanFilterSvc : public xo::reactor::Sink1>, - public xo::reactor::DirectSourcePtr> { + class KalmanFilterSvc : public xo::reactor::Sink1>, + public xo::reactor::DirectSourcePtr> { public: using AbstractSource = xo::reactor::AbstractSource; public: /* named ctor idiom */ - static ref::rp make(KalmanFilterSpec spec); + static rp make(KalmanFilterSpec spec); KalmanFilter const & filter() const { return filter_; } /* notify incoming observations; will trigger kalman filter step */ - void notify_ev(ref::rp const & input_kp1) override; + void notify_ev(rp const & input_kp1) override; // ----- inherited from reactor::AbstractSink ----- @@ -52,7 +52,7 @@ namespace xo { /* passive kalman filter */ KalmanFilter filter_; /* receive filter input from this source; see .attach_input() */ - ref::rp input_src_; + rp input_src_; /* counts lifetime #of input events (see .notify_ev()) */ uint32_t n_in_ev_ = 0; /* publish filter state updates to these callbacks */ diff --git a/src/kalmanfilter/KalmanFilter.cpp b/src/kalmanfilter/KalmanFilter.cpp index 56507650..5c415580 100644 --- a/src/kalmanfilter/KalmanFilter.cpp +++ b/src/kalmanfilter/KalmanFilter.cpp @@ -24,7 +24,7 @@ namespace xo { {} /*ctor*/ void - KalmanFilter::notify_input(ref::rp const & input_kp1) + KalmanFilter::notify_input(rp const & input_kp1) { scope log(XO_ENTER0(info)); diff --git a/src/kalmanfilter/KalmanFilterEngine.cpp b/src/kalmanfilter/KalmanFilterEngine.cpp index e9533549..fe783e8a 100644 --- a/src/kalmanfilter/KalmanFilterEngine.cpp +++ b/src/kalmanfilter/KalmanFilterEngine.cpp @@ -19,9 +19,9 @@ namespace xo { namespace kalman { // ----- KalmanFilterEngine ----- - ref::rp + rp KalmanFilterEngine::extrapolate(utc_nanos tkp1, - ref::rp const & s, + rp const & s, KalmanFilterTransition const & f) { //constexpr char const * c_self_name @@ -60,7 +60,7 @@ namespace xo { } /*extrapolate*/ VectorXd - KalmanFilterEngine::kalman_gain1(ref::rp const & skp1_ext, + KalmanFilterEngine::kalman_gain1(rp const & skp1_ext, KalmanFilterObservable const & h, uint32_t j) { @@ -110,7 +110,7 @@ namespace xo { } /*kalman_gain1*/ MatrixXd - KalmanFilterEngine::kalman_gain(ref::rp const & skp1_ext, + KalmanFilterEngine::kalman_gain(rp const & skp1_ext, KalmanFilterObservable const & h) { scope log(XO_DEBUG(false /*debug_enabled*/)); @@ -214,10 +214,10 @@ namespace xo { return K; } /*kalman_gain*/ - ref::rp - KalmanFilterEngine::correct1(ref::rp const & skp1_ext, + rp + KalmanFilterEngine::correct1(rp const & skp1_ext, KalmanFilterObservable const & h, - ref::rp const & zkp1, + rp const & zkp1, uint32_t j) { uint32_t n = skp1_ext->n_state(); @@ -261,10 +261,10 @@ namespace xo { zkp1); } /*correct1*/ - ref::rp - KalmanFilterEngine::correct(ref::rp const & skp1_ext, + rp + KalmanFilterEngine::correct(rp const & skp1_ext, KalmanFilterObservable const & h, - ref::rp const & zkp1) + rp const & zkp1) { uint32_t n = skp1_ext->n_state(); /* K :: [n x m] */ @@ -300,23 +300,23 @@ namespace xo { zkp1); } /*correct*/ - ref::rp + rp KalmanFilterEngine::step(utc_nanos tkp1, - ref::rp const & sk, + rp const & sk, KalmanFilterTransition const & Fk, KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1) + rp const & zkp1) { - ref::rp skp1_ext + rp skp1_ext = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); - ref::rp skp1 + rp skp1 = KalmanFilterEngine::correct(skp1_ext, Hkp1, zkp1); return skp1; } /*step*/ - ref::rp + rp KalmanFilterEngine::step(KalmanFilterStep const & step_spec) { return step(step_spec.tkp1(), @@ -326,24 +326,24 @@ namespace xo { step_spec.input()); } /*step*/ - ref::rp + rp KalmanFilterEngine::step1(utc_nanos tkp1, - ref::rp const & sk, + rp const & sk, KalmanFilterTransition const & Fk, KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1, + rp const & zkp1, uint32_t j) { - ref::rp skp1_ext + rp skp1_ext = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); - ref::rp skp1 + rp skp1 = KalmanFilterEngine::correct1(skp1_ext, Hkp1, zkp1, j); return skp1; } /*step1*/ - ref::rp + rp KalmanFilterEngine::step1(KalmanFilterStep const & step_spec, uint32_t j) { diff --git a/src/kalmanfilter/KalmanFilterInput.cpp b/src/kalmanfilter/KalmanFilterInput.cpp index 96f0d6ca..a93a855c 100644 --- a/src/kalmanfilter/KalmanFilterInput.cpp +++ b/src/kalmanfilter/KalmanFilterInput.cpp @@ -19,7 +19,7 @@ namespace xo { using std::uint32_t; namespace kalman { - ref::rp + rp KalmanFilterInput::make(utc_nanos tkp1, VectorXb const & presence, VectorXd const & z, @@ -28,7 +28,7 @@ namespace xo { return new KalmanFilterInput(tkp1, presence, z, Rd); } /*make*/ - ref::rp + rp KalmanFilterInput::make_present(utc_nanos tkp1, VectorXd const & z) { diff --git a/src/kalmanfilter/KalmanFilterInputToConsole.cpp b/src/kalmanfilter/KalmanFilterInputToConsole.cpp index 8fadf031..b3cd9d21 100644 --- a/src/kalmanfilter/KalmanFilterInputToConsole.cpp +++ b/src/kalmanfilter/KalmanFilterInputToConsole.cpp @@ -7,7 +7,7 @@ namespace xo { using xo::xtag; namespace kalman { - ref::rp + rp KalmanFilterInputToConsole::make() { return new KalmanFilterInputToConsole(); } /*make*/ diff --git a/src/kalmanfilter/KalmanFilterState.cpp b/src/kalmanfilter/KalmanFilterState.cpp index 4b5679e3..411e6a26 100644 --- a/src/kalmanfilter/KalmanFilterState.cpp +++ b/src/kalmanfilter/KalmanFilterState.cpp @@ -14,7 +14,7 @@ namespace xo { using xo::reflect::TaggedRcptr; using xo::reflect::StructReflector; using xo::time::utc_nanos; - using xo::ref::rp; + using xo::rp; using logutil::matrix; using logutil::vector; //using xo::scope; @@ -190,7 +190,7 @@ namespace xo { // ----- KalmanFilterStateExt ----- - ref::rp + rp KalmanFilterStateExt::initial(utc_nanos t0, VectorXd x0, MatrixXd P0) diff --git a/src/kalmanfilter/KalmanFilterStateToConsole.cpp b/src/kalmanfilter/KalmanFilterStateToConsole.cpp index 9559d339..c7043e2c 100644 --- a/src/kalmanfilter/KalmanFilterStateToConsole.cpp +++ b/src/kalmanfilter/KalmanFilterStateToConsole.cpp @@ -7,7 +7,7 @@ namespace xo { using xo::xtag; namespace kalman { - ref::rp + rp KalmanFilterStateToConsole::make() { return new KalmanFilterStateToConsole(); } /*make*/ diff --git a/src/kalmanfilter/KalmanFilterStep.cpp b/src/kalmanfilter/KalmanFilterStep.cpp index 8ca90211..7313924f 100644 --- a/src/kalmanfilter/KalmanFilterStep.cpp +++ b/src/kalmanfilter/KalmanFilterStep.cpp @@ -13,7 +13,7 @@ namespace xo { using Eigen::VectorXd; namespace kalman { - ref::rp + rp KalmanFilterStep::extrapolate() const { return KalmanFilterEngine::extrapolate(this->tkp1(), @@ -22,14 +22,14 @@ namespace xo { } /*extrapolate*/ MatrixXd - KalmanFilterStep::gain(ref::rp const & skp1_ext) const + KalmanFilterStep::gain(rp const & skp1_ext) const { return KalmanFilterEngine::kalman_gain(skp1_ext, this->obs()); } /*gain*/ VectorXd - KalmanFilterStep::gain1(ref::rp const & skp1_ext, + KalmanFilterStep::gain1(rp const & skp1_ext, uint32_t j) const { return KalmanFilterEngine::kalman_gain1(skp1_ext, @@ -38,16 +38,16 @@ namespace xo { } /*gain1*/ - ref::rp - KalmanFilterStep::correct(ref::rp const & skp1_ext) + rp + KalmanFilterStep::correct(rp const & skp1_ext) { return KalmanFilterEngine::correct(skp1_ext, this->obs(), this->input()); } /*correct*/ - ref::rp - KalmanFilterStep::correct1(ref::rp const & skp1_ext, + rp + KalmanFilterStep::correct1(rp const & skp1_ext, uint32_t j) { return KalmanFilterEngine::correct1(skp1_ext, diff --git a/src/kalmanfilter/KalmanFilterSvc.cpp b/src/kalmanfilter/KalmanFilterSvc.cpp index 5e32b102..c1392dfd 100644 --- a/src/kalmanfilter/KalmanFilterSvc.cpp +++ b/src/kalmanfilter/KalmanFilterSvc.cpp @@ -3,7 +3,7 @@ #include "KalmanFilterSvc.hpp" namespace xo { - using xo::ref::rp; + using xo::rp; using xo::scope; using xo::xtag; @@ -19,7 +19,7 @@ namespace xo { {} void - KalmanFilterSvc::notify_ev(ref::rp const & input_kp1) + KalmanFilterSvc::notify_ev(rp const & input_kp1) { this->filter_.notify_input(input_kp1); diff --git a/utest/KalmanFilter.test.cpp b/utest/KalmanFilter.test.cpp index fea572ec..a310adb4 100644 --- a/utest/KalmanFilter.test.cpp +++ b/utest/KalmanFilter.test.cpp @@ -26,7 +26,7 @@ namespace xo { using xo::time::timeutil; using xo::time::utc_nanos; using xo::time::seconds; - using xo::ref::rp; + using xo::rp; using xo::log_level; using logutil::matrix; using xo::print::ccs; @@ -144,7 +144,7 @@ namespace xo { z_stats.include_sample(z[0]); - ref::rp inputk + rp inputk = KalmanFilterInput::make_present(tkp1, z); KalmanFilterStep step_spec = spec.make_step(sk, inputk); @@ -259,7 +259,7 @@ namespace xo { z_stats.include_sample(z[0]); - ref::rp inputk + rp inputk = KalmanFilterInput::make_present(tkp1, z); KalmanFilterStep step_spec @@ -408,7 +408,7 @@ namespace xo { INFO(tostr(xtag("i_step", i_step), xtag("z", z))); - ref::rp inputk + rp inputk = KalmanFilterInput::make_present(tkp1, z); KalmanFilterStep step_spec = spec.make_step(sk, inputk); @@ -571,7 +571,7 @@ namespace xo { z_stats.include_sample(z[0]); - ref::rp inputk + rp inputk = KalmanFilterInput::make_present(tkp1, z); KalmanFilterStep step_spec = spec.make_step(sk, inputk); rp skp1 = KalmanFilterEngine::step(step_spec);