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