From e1f39b7b0de413a6522905a971f843e9cee91dcb Mon Sep 17 00:00:00 2001 From: Roland Conybeare Date: Mon, 23 Oct 2023 11:11:56 -0400 Subject: [PATCH] initial implementation --- CMakeLists.txt | 47 ++ EigenUtil.cpp | 157 ---- KalmanFilter.cpp | 78 -- KalmanFilter.hpp | 128 ---- KalmanFilterEngine.cpp | 360 --------- KalmanFilterEngine.hpp | 185 ----- KalmanFilterInput.cpp | 118 --- KalmanFilterInput.hpp | 121 --- KalmanFilterInputCallback.hpp | 14 - KalmanFilterInputSource.hpp | 28 - KalmanFilterInputToConsole.hpp | 30 - KalmanFilterObservable.hpp | 110 --- KalmanFilterOutputCallback.hpp | 15 - KalmanFilterSpec.cpp | 27 - KalmanFilterSpec.hpp | 86 --- KalmanFilterState.cpp | 231 ------ KalmanFilterState.hpp | 163 ----- KalmanFilterStateToConsole.hpp | 30 - KalmanFilterStep.cpp | 84 --- KalmanFilterSvc.hpp | 64 -- KalmanFilterTransition.cpp | 55 -- KalmanFilterTransition.hpp | 62 -- cmake/xo_kalmanfilterConfig.cmake.in | 17 + .../xo/kalmanfilter/EigenUtil.hpp | 0 include/xo/kalmanfilter/KalmanFilter.hpp | 127 ++++ .../xo/kalmanfilter/KalmanFilterEngine.hpp | 185 +++++ include/xo/kalmanfilter/KalmanFilterInput.hpp | 121 +++ .../KalmanFilterInputCallback.hpp | 14 + .../kalmanfilter/KalmanFilterInputSource.hpp | 27 + .../KalmanFilterInputToConsole.hpp | 30 + .../kalmanfilter/KalmanFilterObservable.hpp | 109 +++ .../KalmanFilterOutputCallback.hpp | 15 + include/xo/kalmanfilter/KalmanFilterSpec.hpp | 86 +++ include/xo/kalmanfilter/KalmanFilterState.hpp | 163 +++++ .../KalmanFilterStateToConsole.hpp | 30 + .../xo/kalmanfilter/KalmanFilterStep.hpp | 0 include/xo/kalmanfilter/KalmanFilterSvc.hpp | 64 ++ .../kalmanfilter/KalmanFilterTransition.hpp | 62 ++ include/xo/kalmanfilter/init_filter.hpp | 20 + include/xo/kalmanfilter/print_eigen.hpp | 41 ++ init_filter.cpp | 51 -- init_filter.hpp | 20 - print_eigen.hpp | 41 -- src/kalmanfilter/CMakeLists.txt | 31 + src/kalmanfilter/EigenUtil.cpp | 157 ++++ src/kalmanfilter/KalmanFilter.cpp | 78 ++ src/kalmanfilter/KalmanFilterEngine.cpp | 360 +++++++++ src/kalmanfilter/KalmanFilterInput.cpp | 118 +++ .../KalmanFilterInputToConsole.cpp | 2 +- .../kalmanfilter/KalmanFilterObservable.cpp | 2 +- src/kalmanfilter/KalmanFilterSpec.cpp | 27 + src/kalmanfilter/KalmanFilterState.cpp | 231 ++++++ .../KalmanFilterStateToConsole.cpp | 2 +- src/kalmanfilter/KalmanFilterStep.cpp | 84 +++ .../kalmanfilter/KalmanFilterSvc.cpp | 0 src/kalmanfilter/KalmanFilterTransition.cpp | 55 ++ src/kalmanfilter/init_filter.cpp | 52 ++ utest/CMakeLists.txt | 54 ++ utest/KalmanFilter.test.cpp | 690 ++++++++++++++++++ utest/filter_utest_main.cpp | 6 + 60 files changed, 3104 insertions(+), 2261 deletions(-) create mode 100644 CMakeLists.txt delete mode 100644 EigenUtil.cpp delete mode 100644 KalmanFilter.cpp delete mode 100644 KalmanFilter.hpp delete mode 100644 KalmanFilterEngine.cpp delete mode 100644 KalmanFilterEngine.hpp delete mode 100644 KalmanFilterInput.cpp delete mode 100644 KalmanFilterInput.hpp delete mode 100644 KalmanFilterInputCallback.hpp delete mode 100644 KalmanFilterInputSource.hpp delete mode 100644 KalmanFilterInputToConsole.hpp delete mode 100644 KalmanFilterObservable.hpp delete mode 100644 KalmanFilterOutputCallback.hpp delete mode 100644 KalmanFilterSpec.cpp delete mode 100644 KalmanFilterSpec.hpp delete mode 100644 KalmanFilterState.cpp delete mode 100644 KalmanFilterState.hpp delete mode 100644 KalmanFilterStateToConsole.hpp delete mode 100644 KalmanFilterStep.cpp delete mode 100644 KalmanFilterSvc.hpp delete mode 100644 KalmanFilterTransition.cpp delete mode 100644 KalmanFilterTransition.hpp create mode 100644 cmake/xo_kalmanfilterConfig.cmake.in rename EigenUtil.hpp => include/xo/kalmanfilter/EigenUtil.hpp (100%) create mode 100644 include/xo/kalmanfilter/KalmanFilter.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterEngine.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterInput.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterInputCallback.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterInputSource.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterObservable.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterSpec.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterState.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp rename KalmanFilterStep.hpp => include/xo/kalmanfilter/KalmanFilterStep.hpp (100%) create mode 100644 include/xo/kalmanfilter/KalmanFilterSvc.hpp create mode 100644 include/xo/kalmanfilter/KalmanFilterTransition.hpp create mode 100644 include/xo/kalmanfilter/init_filter.hpp create mode 100644 include/xo/kalmanfilter/print_eigen.hpp delete mode 100644 init_filter.cpp delete mode 100644 init_filter.hpp delete mode 100644 print_eigen.hpp create mode 100644 src/kalmanfilter/CMakeLists.txt create mode 100644 src/kalmanfilter/EigenUtil.cpp create mode 100644 src/kalmanfilter/KalmanFilter.cpp create mode 100644 src/kalmanfilter/KalmanFilterEngine.cpp create mode 100644 src/kalmanfilter/KalmanFilterInput.cpp rename KalmanFilterInputToConsole.cpp => src/kalmanfilter/KalmanFilterInputToConsole.cpp (93%) rename KalmanFilterObservable.cpp => src/kalmanfilter/KalmanFilterObservable.cpp (98%) create mode 100644 src/kalmanfilter/KalmanFilterSpec.cpp create mode 100644 src/kalmanfilter/KalmanFilterState.cpp rename KalmanFilterStateToConsole.cpp => src/kalmanfilter/KalmanFilterStateToConsole.cpp (93%) create mode 100644 src/kalmanfilter/KalmanFilterStep.cpp rename KalmanFilterSvc.cpp => src/kalmanfilter/KalmanFilterSvc.cpp (100%) create mode 100644 src/kalmanfilter/KalmanFilterTransition.cpp create mode 100644 src/kalmanfilter/init_filter.cpp create mode 100644 utest/CMakeLists.txt create mode 100644 utest/KalmanFilter.test.cpp create mode 100644 utest/filter_utest_main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..f4120636 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,47 @@ +# xo-kalmanfilter/CMakeLists.txt + +cmake_minimum_required(VERSION 3.10) + +project(xo_kalmanfilter VERSION 1.0) +enable_language(CXX) + +# common XO cmake macros (see proj/xo-cmake) +include(xo_macros/xo_cxx) +include(xo_macros/code-coverage) + +# ---------------------------------------------------------------- +# unit test setup + +enable_testing() +# activate code coverage for all executables + libraries (when configured with -DCODE_COVERAGE=ON) +add_code_coverage() +# 1. assuming that /nix/store/ prefixes .hpp files belonging to gcc, catch2 etc. +# we're not interested in code coverage for these sources. +# 2. exclude the utest/ subdir, we don't need coverage on the unit tests themselves; +# rather, want coverage on the code that the unit tests exercise. +# +# NOTE: this seems to work only with the 'ccov-all' target. In particular, doesn't seem to do anything with the 'ccov' target +# +add_code_coverage_all_targets(EXCLUDE /nix/store/* ${PROJECT_SOURCE_DIR}/utest/* ${PROJECT_BINARY_DIR}/local/* ${PROJECT_SOURCE_DIR}/repo/*) + +# ---------------------------------------------------------------- +# c++ settings + +# one-time project-specific c++ flags. usually empty +set(PROJECT_CXX_FLAGS "") +#set(PROJECT_CXX_FLAGS "-fconcepts-diagnostics-depth=2") +add_definitions(${PROJECT_CXX_FLAGS}) + +xo_toplevel_compile_options() + +# ---------------------------------------------------------------- + +add_subdirectory(src/kalmanfilter) +add_subdirectory(utest) + +# ---------------------------------------------------------------- +# provide find_package() support for reactor customers + +xo_export_cmake_config(${PROJECT_NAME} ${PROJECT_VERSION} ${PROJECT_NAME}Targets) + +# end CMakeLists.txt diff --git a/EigenUtil.cpp b/EigenUtil.cpp deleted file mode 100644 index f1d3c4a3..00000000 --- a/EigenUtil.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/* file EigenUtil.cpp - * - * author: Roland Conybeare, Sep 2022 - */ - -#include "EigenUtil.hpp" -#include "printjson/PrintJson.hpp" -#include "reflect/Reflect.hpp" -#include -#include -#include -#include - -namespace xo { - using xo::json::PrintJson; - using xo::json::JsonPrinter; - using xo::reflect::Reflect; - using xo::reflect::TypeDescr; - using VectorXb = Eigen::Array; - using Eigen::VectorXd; - using Eigen::MatrixXd; - -#ifdef NOT_YET - namespace reflect { - template - using EigenVectorX_Tdx = xo::reflect::StlVectorTdx>; - - /* probably need this to appear before decl for class xo::reflect::Reflect */ - template - class EstablishTdx> { - public: - static std::unique_ptr make() { - return EigenVectorX_Tdx::make(); - } /*make*/ - }; /*EstablishTdx*/ - } /*reflect*/ -#endif - - namespace eigen { - - namespace { - /* prints a VectorXd as json, in the obvious format, e.g. - * [1,2,3] - */ - template - class EigenVectorJsonPrinter : public JsonPrinter { - public: - EigenVectorJsonPrinter(PrintJson const * pjson) : JsonPrinter(pjson) {} - - virtual void print_json(TaggedPtr tp, - std::ostream * p_os) const override - { - EigenVectorType * pv = this->check_recover_native(tp, p_os); - - if (pv) { - /* EigenVectorType (VectorXb, VectorXd, ..) - * is reflected as atomic for now, out of expedience. - * - * as soon as we reflect as mt_vector, will not need this helper. - */ - *p_os << "["; - - for (std::uint32_t i = 0, n = pv->size(); i < n; ++i) { - if (i > 0) - *p_os << ","; - - /* note: need to dispatch via json printer for vector elements, - * to get special treatment for non-finite values - */ - this->pjson()->print((*pv)[i], p_os); - //*p_os << jsonp((*pv)[i], this->pjson()); - } - - *p_os << "]"; - } - } /*print_json*/ - }; /*EigenVectorJsonPrinter*/ - - /* prints a MatrixXd as json, in row-major format, e.g. - * [[1,2,3], [4,5,6], [7,8,9]] - */ - class MatrixXdJsonPrinter : public JsonPrinter { - public: - MatrixXdJsonPrinter(PrintJson const * pjson) : JsonPrinter(pjson) {} - - virtual void print_json(TaggedPtr tp, - std::ostream * p_os) const override - { - MatrixXd * pm = this->check_recover_native(tp, p_os); - - if (pm) { - /* MatrixXd is reflected as atomic for now, out of expedience */ - *p_os << "["; - - for(std::uint32_t i=0, m=pm->rows(); i 0) - *p_os << ", "; - *p_os << "["; - for(std::uint32_t j=0, n=pm->cols(); j 0) - *p_os << ","; - - /* note: need to dispatch via json printer for matrix elements, - * to get special treatment for non-finite values - */ - this->pjson()->print((*pm)(i, j), p_os); - //*p_os << jsonp((*pm)(i, j), this->pjson()); - } - *p_os << "]"; - } - - *p_os << "]"; - } - } /*print_json*/ - }; /*MatrixXdJsonPrinter*/ - - template - void - provide_eigen_vector_printer(PrintJson * p_pjson) - { - TypeDescr td = Reflect::require(); - std::unique_ptr pr(new EigenVectorJsonPrinter(p_pjson)); - - p_pjson->provide_printer(td, std::move(pr)); - } /*provide_eigen_vector_printer*/ - } /*namespace*/ - - void - EigenUtil::reflect_eigen() - { -#ifdef NOT_YET - Reflect::require(); - Reflect::require(); -#endif - } /*reflect_eigen*/ - - void - EigenUtil::provide_json_printers(PrintJson * p_pjson) - { - assert(p_pjson); - - provide_eigen_vector_printer(p_pjson); - provide_eigen_vector_printer(p_pjson); - - { - TypeDescr td = Reflect::require(); - std::unique_ptr pr(new MatrixXdJsonPrinter(p_pjson)); - - p_pjson->provide_printer(td, std::move(pr)); - } - } /*provide_json_printers*/ - } /*namespace eigen*/ -} /*namespace xo*/ - -/* end EigenUtil.cpp */ diff --git a/KalmanFilter.cpp b/KalmanFilter.cpp deleted file mode 100644 index bf94fde2..00000000 --- a/KalmanFilter.cpp +++ /dev/null @@ -1,78 +0,0 @@ -/* @file KalmanFilter.cpp */ - -#include "KalmanFilter.hpp" -#include "KalmanFilterEngine.hpp" -#include "print_eigen.hpp" -#include "indentlog/scope.hpp" -#include "Eigen/src/Core/Matrix.h" - -namespace xo { - using xo::time::utc_nanos; - //using logutil::matrix; - using xo::scope; - using xo::tostr; - using xo::xtag; - using Eigen::MatrixXd; - using Eigen::VectorXd; - - namespace kalman { - // ----- KalmanFilter ----- - - KalmanFilter::KalmanFilter(KalmanFilterSpec spec) - : filter_spec_{std::move(spec)}, - state_ext_{filter_spec_.start_ext()} - {} /*ctor*/ - - void - KalmanFilter::notify_input(ref::rp const & input_kp1) - { - scope log(XO_ENTER0(info)); - - /* on entry: - * .state_ext refers to t(k) - * on exit: - * .step refers to t(k+1) - * .state_ext refers to t(k+1) - */ - - log && log(xtag("step_dt", - input_kp1->tkp1() - this->state_ext_->tm())); - - /* establish step inputs for this filter step: - * F(k+1) (system transition matrix) - * Q(k+1) (system noise covariance matrix) - * H(k+1) (observation coupling matrix) - * R(k+1) (observation noise covariance matrix) - * z(k+1) (observation vector) - */ - this->step_ = this->filter_spec_.make_step(this->state_ext_, input_kp1); - - //if (lscope.enabled()) { lscope.log(xtag("step", this->step_)); } - - /* extrapolate filter state to t(k+1), - * and correct based on z(k+1) - */ - this->state_ext_ = KalmanFilterEngine::step(this->step_); - - //if (lscope.enabled()) { lscope.log(xtag("state_ext", this->state_ext_)); } - } /*notify_input*/ - - void - KalmanFilter::display(std::ostream & os) const - { - os << ""; - } /*display*/ - - std::string - KalmanFilter::display_string() const - { - return tostr(*this); - } /*display_string*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilter.cpp */ diff --git a/KalmanFilter.hpp b/KalmanFilter.hpp deleted file mode 100644 index 295a4f7a..00000000 --- a/KalmanFilter.hpp +++ /dev/null @@ -1,128 +0,0 @@ -/* @file KalmanFilter.hpp */ - -#pragma once - -#include "filter/KalmanFilterSpec.hpp" - -namespace xo { - namespace kalman { - /* Specification for an ordinary discrete linear kalman filter. - * - * The filter generates estimates for a process observed at a discrete - * set of times tk in {t0, t1, .., tn} - * - * At each time tk we have the following: - * - * 0. x(0) initial estimate at t(0) - * P(0) initial priors: error covariance matrix for x(0) - * - * 1. x_(k), [n x 1] vector: - * system state, denoted by vector. - * (state is not directly observable, - * filter will attempt to estimate it) - * - * 2. w_(k), [n x 1] vector - * Q(k), [n x n] matrix - * - * w_(k) denotes system noise, - * gaussian with covariance Q(k). - * noise w_(k) is not directly observable. - * - * 3. z(k), [m x 1] vector: - * - * observation vector for time tk - * - * 4. v_(k), [m x 1] vector - * R(k), [m x m] matrix - * - * v_(k) denotes observation errors, - * gaussian with covariance R(k). - * noise v_(k) is not directly observable. - * - * 5. F(k), [n x n] matrix - * state transition matrix - * model system evolves according to: - * - * x_(k+1) = F(x).x_(k) + w_(k) - * - * 6. observations z(k) depend on system state: - * - * z(k) = H(k).x_(k) + v_(k) - * - * 7. Kalman filter outputs: - * x(k), [n x 1] vector - * Q(k), [n x n] matrix - * - * x(k) is optimal estimate for system state x_(k) - * P(k) is covariance matrix specifying confidence intervals - * for pairs (x(k)[i], x(k)[j]) - * - * filter specification consists of: - * n, m, x(0), P(0), F(k), Q(k), H(k), R(k) - * The cardinality of observations z(k) can vary over time, - * so to be precise, m can vary with tk; write as m(k) - * - * More details: - * - avoid having to specify t(k) in advance; - * instead defer until observation available - * so t(k) can be taken from polling timestamp - */ - - /* encapsulate a (linear) kalman filter - * together with event publishing - */ - class KalmanFilter { - public: - using MatrixXd = Eigen::MatrixXd; - using VectorXd = Eigen::VectorXd; - using utc_nanos = xo::time::utc_nanos; - - public: - /* create filter with specification given by spec, and initial state s0 */ - explicit KalmanFilter(KalmanFilterSpec spec); - - uint32_t step_no() const { return state_ext_->step_no(); } - utc_nanos tm() const { return state_ext_->tm(); } - KalmanFilterSpec const & filter_spec() const { return filter_spec_; } - KalmanFilterStep const & step() const { return step_; } - ref::rp const & state_ext() const { return state_ext_; } - - /* notify kalman filter with input for time t(k+1) = input_kp1.tkp1() - * Require: input.tkp1() >= .current_tm() - * Promise: - * - .tm() = input_kp1.tkp1() - * - .step_no() = old .step_no() + 1 - * - .filter_spec_k, .step_k, .state_k updated - * for observations in input_kp1 - */ - void notify_input(ref::rp const & input_kp1); - - void display(std::ostream & os) const; - std::string display_string() const; - - private: - /* specification for kalman filter; - * produces process/observation matrices on demand - */ - KalmanFilterSpec filter_spec_; - - /* filter step for most recent observation */ - KalmanFilterStep step_; - - /* filter state as of most recent observation; - * result of applying KalmanFilterEngine::step() to contents of .step - */ - ref::rp state_ext_; - }; /*KalmanFilter*/ - - inline std::ostream & - operator<<(std::ostream & os, KalmanFilter const & x) { - x.display(os); - return os; - } /*operator<<*/ - - } /*namespace kalman*/ -} /*namespace xo*/ - - -/* end KalmanFilter.hpp */ diff --git a/KalmanFilterEngine.cpp b/KalmanFilterEngine.cpp deleted file mode 100644 index e80f7497..00000000 --- a/KalmanFilterEngine.cpp +++ /dev/null @@ -1,360 +0,0 @@ -/* @file KalmanFilterEngine.cpp - * - */ - -#include "KalmanFilterEngine.hpp" -#include "print_eigen.hpp" -#include "indentlog/scope.hpp" -#include "Eigen/src/Core/Matrix.h" - -namespace xo { - using xo::time::utc_nanos; - using logutil::matrix; - using xo::scope; - using xo::xtag; - using Eigen::LDLT; - using Eigen::MatrixXd; - using Eigen::VectorXd; - - namespace kalman { - // ----- KalmanFilterEngine ----- - - ref::rp - KalmanFilterEngine::extrapolate(utc_nanos tkp1, - ref::rp const & s, - KalmanFilterTransition const & f) - { - //constexpr char const * c_self_name - // = "KalmanFilterEngine::extrapolate"; - - /* prior estimates at t(k) */ - VectorXd const & x = s->state_v(); - MatrixXd const & P = s->state_cov(); - - /* model change from t(k) -> t(k+1) */ - MatrixXd const & F = f.transition_mat(); - MatrixXd const & Q = f.transition_cov(); - - if(F.cols() != x.rows()) { - scope log(XO_DEBUG(true /*debug_flag*/)); - - log("error: F*x: expected F.cols=x.rows", - xtag("F.cols", F.cols()), xtag("x.rows", x.rows())); - } - - /* x(k+1|k) */ - VectorXd x_ext = F * x; - - /* P(k+1|k) */ - MatrixXd P_ext = (F * P * F.transpose()) + Q; - - /* creating new state object here - * allows KalmanFilterSvc.is_volatile()=false - */ - - return KalmanFilterState::make(s->step_no() + 1, - tkp1, - std::move(x_ext), - std::move(P_ext), - f); - } /*extrapolate*/ - - VectorXd - KalmanFilterEngine::kalman_gain1(ref::rp const & skp1_ext, - KalmanFilterObservable const & h, - uint32_t j) - { - constexpr bool c_debug_enabled = false; - - scope log(XO_DEBUG(c_debug_enabled)); - - /* P(k+1|k) :: [n x n] */ - MatrixXd const & P_ext = skp1_ext->state_cov(); - - /* H(k) :: [m x n] */ - MatrixXd const & H = h.observable(); - /* R(k) :: [m x m] */ - MatrixXd const & R = h.observable_cov(); - - /* i'th col of H couples element #i of filter state to each member of input z(k); - * j'th row of H couples filter state to j'th observable - * - * Hj :: [1 x n] Hj is a row-vector - */ - auto Hj = H.row(j); - - /* Rjj is the j'th diagonal element of R */ - double Rjj = R(j, j); - - /* T - * M(k) = Hj * P(k+1|k) * Hj + Rjj - * - * M(k) is a [1 x 1] matrix - */ - double m = Hj * (P_ext * Hj.transpose()) + Rjj; - - /* -1 - * M(k) trivial, since M is [1 x 1] - */ - double m_inv = 1.0 / m; - - /* K :: [n x 1] */ - VectorXd K = P_ext * Hj.transpose() * m_inv; - - log && log("result", - xtag("P(k+1|k)", matrix(P_ext)), - xtag("R", matrix(R)), - xtag("m", m)); - - return K; - } /*kalman_gain1*/ - - MatrixXd - KalmanFilterEngine::kalman_gain(ref::rp const & skp1_ext, - KalmanFilterObservable const & h) - { - scope log(XO_DEBUG(false /*debug_enabled*/)); - - /* P(k+1|k) */ - MatrixXd const & P_ext = skp1_ext->state_cov(); - - MatrixXd const & H = h.observable(); - MatrixXd const & R = h.observable_cov(); - - uint32_t m = H.rows(); - uint32_t n = H.cols(); - - if ((P_ext.rows() != n) || (P_ext.cols() != n)) { - std::string err_msg - = tostr("kalman_gain: with dim(H) = [m x n] expect dim(P) = [n x n]", - xtag("m", m), xtag("n", n), - xtag("P.rows", P_ext.rows()), - xtag("P.cols", P_ext.cols())); - - throw std::runtime_error(err_msg); - } - - if ((R.rows() != m) || (R.cols() != m)) { - std::string err_msg - = tostr("kalman_gain: with dim(H) = [m x n] expect dim(R) = [m x m]", - xtag("m", m), xtag("n", n), - xtag("R.rows", R.rows()), xtag("R.cols", R.cols())); - - throw std::runtime_error(err_msg); - } - - /* kalman gain: - * T -1 - * K(k+1) = P(k+1|k).H(k) .M - * - * T / T \ -1 - * = P(k+1|k).H(k) .| H(k).P(k+1|k).H(k) + R(k) | - * \ / - * - * Notes: - * 1. the matrix M being inverted is symmetric, since represents covariances. - * 2. if diagonal of R(k) has no zeroes (i.e. all measurements are subject to error), - * then it must be non-negative definite - * 3. unless observation errors are perfectly correlated, M(k) - * is positive definite. - * 4. even though 3. holds, there may be a nearby non-positive-definite matrix M+dM, - * Factoring M with finite-precision arithmetic solves for M+dM instead of M; - * which may run into difficulty if M is only 'slighlty' +ve definite. - * If necessary add small diagonal correction D to M, - * sufficient to make M+D positive definite. - * This is equivalent to introducing additional - * uncorrelated observation error, so benign from a robustness perspective - * 5. In generally we usually want to avoid fully realizing a matrix inverse. - * In this case need to explicitly compute K as ingredient used to - * correct state covariance later. - * 6. However, if R is diagonal (which is in practice quite likely), - * then it's easy to decompose a suite of vector observations z(k+1) = [z1, ..zm]T - * into separate zi, with dt=0 separating them. - * Can use this to avoid computing the inverse. - * See .kalman_gain1(), .correct1() - * 7. .kalman_gain() works unaltered when H, R have been reindexed - * to exclude outliers/errors; this is true because .kalman_gain() does not - * use the observation vector z[], i.e. operates entirely in the reduced - * reindexed space. - */ - - MatrixXd M = H * P_ext * H.transpose() + R; - - /* will use to write M as: - * - * T T - * M = P .L.D.L .P - * - * where: - * P is a permutation matrix - * L is lower triangular, with unit diagonal - * D is diagonal - */ - LDLT ldlt = M.ldlt(); - - /* solve for the identity matrix to realize the inverse this way */ - MatrixXd I = MatrixXd::Identity(M.rows(), M.cols()); - - /* -1 - * M - */ - MatrixXd M_inv = ldlt.solve(I); - - /* K(k+1) */ - MatrixXd K = P_ext * H.transpose() * M_inv; - - log && log("result", - xtag("k", skp1_ext->step_no()), - xtag("P(k+1|k)", matrix(P_ext)), - xtag("H", matrix(H)), - xtag("R", matrix(R)), - xtag("M", matrix(M)), - xtag("K", matrix(K))); - - return K; - } /*kalman_gain*/ - - ref::rp - KalmanFilterEngine::correct1(ref::rp const & skp1_ext, - KalmanFilterObservable const & h, - ref::rp const & zkp1, - uint32_t j) - { - uint32_t n = skp1_ext->n_state(); - /* Kj :: [n x 1] */ - VectorXd Kj = kalman_gain1(skp1_ext, h, j); - /* H :: [m x n] */ - MatrixXd const & H = h.observable(); - VectorXd const & z = zkp1->z(); - - /* Hj :: [1 x n] the j'th row of H */ - auto const & Hj = H.row(j); - - - /* x(k+1|x) :: [n x 1] */ - VectorXd const & x_ext = skp1_ext->state_v(); - - /* P(k+1|k) :: [n x n] */ - MatrixXd const & P_ext = skp1_ext->state_cov(); - - /* innovj : difference between jth 'actual observation' - * and jth 'predicted observation' - */ - double innovj = z[j] - (Hj * x_ext); - - /* x(k+1) */ - VectorXd xkp1 = x_ext + (Kj * innovj); - - MatrixXd I = MatrixXd::Identity(n, n); - /* note: Kj [n x 1], Hj [1 x n], - * so Kj * Hj [n x n], with rank 1 - */ - MatrixXd Pkp1 = (I - (Kj * Hj)) * P_ext; - - return KalmanFilterStateExt::make(skp1_ext->step_no(), - skp1_ext->tm(), - xkp1, - Pkp1, - skp1_ext->transition(), - Kj, - j, - zkp1); - } /*correct1*/ - - ref::rp - KalmanFilterEngine::correct(ref::rp const & skp1_ext, - KalmanFilterObservable const & h, - ref::rp const & zkp1) - { - uint32_t n = skp1_ext->n_state(); - /* K :: [n x m] */ - MatrixXd K = kalman_gain(skp1_ext, h); - MatrixXd const & H = h.observable(); - /* z_orig[] is original observation vector before reindexing */ - VectorXd const & z_orig = zkp1->z(); - /* reindex z_orig, keeping only elements that appear in - */ - VectorXd z = z_orig(h.keep()); - - /* 'ext' short for 'extrapolated' */ - VectorXd const & x_ext = skp1_ext->state_v(); - MatrixXd const & P_ext = skp1_ext->state_cov(); - - /* innov: difference between 'actual observations' - * and 'predicted observations' - */ - VectorXd innov = z - (H * x_ext); - - /* x(k+1) :: [n x 1] */ - VectorXd xkp1 = x_ext + K * innov; - MatrixXd I = MatrixXd::Identity(n, n); - MatrixXd Pkp1 = (I - K * H) * P_ext; - - return KalmanFilterStateExt::make(skp1_ext->step_no(), - skp1_ext->tm(), - xkp1, - Pkp1, - skp1_ext->transition(), - K, - -1 /*j: not used*/, - zkp1); - } /*correct*/ - - ref::rp - KalmanFilterEngine::step(utc_nanos tkp1, - ref::rp const & sk, - KalmanFilterTransition const & Fk, - KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1) - { - ref::rp skp1_ext - = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); - - ref::rp skp1 - = KalmanFilterEngine::correct(skp1_ext, Hkp1, zkp1); - - return skp1; - } /*step*/ - - ref::rp - KalmanFilterEngine::step(KalmanFilterStep const & step_spec) - { - return step(step_spec.tkp1(), - step_spec.state(), - step_spec.model(), - step_spec.obs(), - step_spec.input()); - } /*step*/ - - ref::rp - KalmanFilterEngine::step1(utc_nanos tkp1, - ref::rp const & sk, - KalmanFilterTransition const & Fk, - KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1, - uint32_t j) - { - ref::rp skp1_ext - = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); - - ref::rp skp1 - = KalmanFilterEngine::correct1(skp1_ext, Hkp1, zkp1, j); - - return skp1; - } /*step1*/ - - ref::rp - KalmanFilterEngine::step1(KalmanFilterStep const & step_spec, - uint32_t j) - { - return step1(step_spec.tkp1(), - step_spec.state(), - step_spec.model(), - step_spec.obs(), - step_spec.input(), - j); - } /*step1*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterEngine.cpp */ diff --git a/KalmanFilterEngine.hpp b/KalmanFilterEngine.hpp deleted file mode 100644 index b8c2f949..00000000 --- a/KalmanFilterEngine.hpp +++ /dev/null @@ -1,185 +0,0 @@ -/* @file KalmanFilterEngine.hpp */ - -#pragma once - -#include "filter/KalmanFilterStep.hpp" -#include "filter/KalmanFilterState.hpp" -#include "filter/KalmanFilterTransition.hpp" -#include "filter/KalmanFilterObservable.hpp" -#include "filter/KalmanFilterInput.hpp" - -namespace xo { - namespace kalman { - class KalmanFilterEngine { - public: - using MatrixXd = Eigen::MatrixXd; - using VectorXd = Eigen::VectorXd; - using utc_nanos = xo::time::utc_nanos; - - public: - /* evolution of system state + account for system noise, - * before incorporating effect of observations z(k+1) - * x(k) --> x(k+1|k) - * P(k) --> P(k+1|k) - * - * tkp1. time t(k+1) assoc'd with step k+1 - * sk. filter state at time tk: - * sk.k = k step # (starts from 0) - * sk.tk = t(k) time t(k) assoc'd with step #k - * sk.x = x(k) estimated state at time tk - * sk.P = P(k) quality of state estimate x(k) - * (error covariance matrix) - * Fk. state transition: - * Fk.F = F(k) state transition matrix - * Fk.Q = Q(k) covariance matrix for system noise - * - * returns propagated state estimate for t(k+1): - * retval.k = k+1 - * retval.tk = t(k+1) = tkp1 - * retval.x = x(k+1|k) - * retval.P = P(k+1|k) - */ - static ref::rp extrapolate(utc_nanos tkp1, - ref::rp const & sk, - KalmanFilterTransition const & Fk); - - /* compute kalman gain matrix for filter step t(k) -> t(k+1) - * Expensive implementation using matrix inversion - * - * T - * M(k+1) = H(k).P(k+1|k).H(k) + R(k) - * - * T -1 - * K(k+1) = P(k+1|k).H(k) .M(k+1) - * - * Require: - * - skp1_ext.n_state() = Hkp1.n_state() - * - * skp1_ext. extrapolated filter state at t(k+1) - * Hkp1. relates model state to observable variables, - * for step t(k) -> t(k+1) - */ - static MatrixXd kalman_gain(ref::rp const & skp1_ext, - KalmanFilterObservable const & Hkp1); - - /* compute kalman gain for a single observation z(k)[j]. - * This is useful iff the observation error matrix R is diagonal. - * For diagonal R we can present a set of observations z(k) serially - * instead of all at once, with lower time complexity - * - * Kalman Filter specifies some space with m observables. - * j identifies one of those observables, indexing from 0. - * This corresponds to row #j of H(k), and element R[j,j] of R. - * - * Effectively, we are projecting the kalman filter assoc'd with - * {skp1_ext, Hkp1} to a filter with a single observable variable z(k)[j], - * then computing the (scalar) kalman gain for this 1-variable filter - * - * The gain vector tells us for each member of filter state, - * how much to adjust our optimal estimate for that member for a unit - * amount of innovation in observable #j, i.e. for difference between - * expected and actual value for that observable. - */ - static VectorXd kalman_gain1(ref::rp const & skp1_ext, - KalmanFilterObservable const & Hkp1, - uint32_t j); - - /* correct extrapolated state+cov estimate; - * also computes kalman gain - * - * Require: - * - skp1_ext.n_state() = Hkp1.n_state() - * - zkp1.n_obs() == Hkp1.n_observable() - * - * skp1_ext. extrapolated kalman state + covaraince at t(k+1) - * Hkp1. relates model state to observable variables - * for step t(k) -> t(k+1) - * zkp1. observations for time t(k+1) - * - * return new filter state+cov - */ - static ref::rp correct(ref::rp const & skp1_ext, - KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1); - - /* correct extrapolated filter state for observation - * of j'th filter observable z(k+1)[j] - * - * Can use this when observation errors are uncorrelated - * (i.e. observation error matrix R is diagonal) - */ - static ref::rp correct1(ref::rp const & skp1_ext, - KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1, - uint32_t j); - - /* step filter from t(k) -> t(k+1) - * - * sk. filter state from previous step: - * x (state vector), P (state covar matrix) - * Fk. transition-related params: - * F (transition matrix), Q (system noise covar matrix) - * Hkp1. observation-related params: - * H (coupling matrix), R (error covar matrix) - * zkp1. observations z(k+1) for time t(k+1) - */ - static ref::rp step(utc_nanos tkp1, - ref::rp const & sk, - KalmanFilterTransition const & Fk, - KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1); - - /* step filter from t(k) -> tk(k+1) - * same as - * .step(tkp1, sk, step_spec.model(), step_spec.obs(), zkp1); - * - * step_spec. encapsulates Fk (transition-related params) - * and Q (system noise covar matrix) - */ - static ref::rp step(KalmanFilterStep const & step_spec); - - /* step filter from t(k) -> t(k+1) - * - * sk. filter state from previous step: - * x (state vector), P (state covar matrix) - * Fk. transition-related params: - * F (transition matrix), Q (system noise covar matrix) - * Hkp1. observation-related params: - * H (coupling matrix), R (error covar matrix) - * zkp1. observations z(k+1) for time t(k+1) - * j. identifies a single filter observable -- - * step will only consume observation z(k+1)[j] - */ - static ref::rp step1(utc_nanos tkp1, - ref::rp const & sk, - KalmanFilterTransition const & Fk, - KalmanFilterObservable const & Hkp1, - ref::rp const & zkp1, - uint32_t j); - - /* step filter from t(k) -> t(k+1) - * - * same as - * .step1(step_spec.tkp1(), - * step_spec.state(), - * step_spec.model(), - * step_spec.obs(), - * step_spec.input(), - * j); - * - * step_spec. encapsulates - * x (state vector), - * P (state covar matrix), - * Fk (transition-related params), - * Q (system noise covar matrix) - * z (z(k+1), observations at time t(k+1)) - * j. identifies a single filter observable -- - * step will only consume observation z(k+1)[j] - */ - static ref::rp step1(KalmanFilterStep const & step_spec, - uint32_t j); - }; /*KalmanFilterEngine*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterEngine.hpp */ diff --git a/KalmanFilterInput.cpp b/KalmanFilterInput.cpp deleted file mode 100644 index 03b5330f..00000000 --- a/KalmanFilterInput.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/* @file KalmanFilterInput.cpp */ - -#include "KalmanFilterInput.hpp" -#include "reflect/StructReflector.hpp" -#include "Eigen/src/Core/Matrix.h" -#include "print_eigen.hpp" -#include "indentlog/scope.hpp" -#include "reflect/TaggedRcptr.hpp" - -namespace xo { - using xo::reflect::Reflect; - using xo::reflect::TaggedRcptr; - using xo::reflect::StructReflector; - using xo::scope; - using logutil::matrix; - using xo::xtag; - using Eigen::MatrixXd; - using Eigen::VectorXi; - using std::uint32_t; - - namespace kalman { - ref::rp - KalmanFilterInput::make(utc_nanos tkp1, - VectorXb const & presence, - VectorXd const & z, - VectorXd const & Rd) - { - return new KalmanFilterInput(tkp1, presence, z, Rd); - } /*make*/ - - ref::rp - KalmanFilterInput::make_present(utc_nanos tkp1, - VectorXd const & z) - { - VectorXb presence = VectorXb::Ones(z.cols()); - - return new KalmanFilterInput(tkp1, - presence, - z, - VectorXd(0) /*Rd - not using*/); - } /*make*/ - - VectorXi - KalmanFilterInput::make_kept_index() const - { - scope log(XO_DEBUG(false /*!debug_flag*/)); - - log && log(xtag("presence", matrix(presence_))); - - /* count truth values */ - uint32_t mstar = 0; - - for (uint32_t j = 0, m = this->presence_.rows(); jpresence_[j]) - ++mstar; - } - - log && log(xtag("m*", mstar)); - - VectorXi keep(mstar); - - /* 2nd pass, populate keep[] */ - - uint32_t jstar = 0; - - for (uint32_t j = 0, m = this->presence_.rows(); jpresence_[j]) { - keep[jstar] = j; - ++jstar; - } - } - - log && log("keep", matrix(keep)); - - return keep; - } /*make_kept_index*/ - - void - KalmanFilterInput::reflect_self() - { - StructReflector sr; - - if (sr.is_incomplete()) { - REFLECT_MEMBER(sr, tkp1); - REFLECT_MEMBER(sr, presence); - REFLECT_MEMBER(sr, z); - REFLECT_MEMBER(sr, Rd); - } - } /*reflect_self*/ - - reflect::TaggedRcptr - KalmanFilterInput::self_tp() - { - return Reflect::make_rctp(this); - } /*self_tp*/ - - void - KalmanFilterInput::display(std::ostream & os) const - { - os << ""; - } /*display*/ - - std::string - KalmanFilterInput::display_string() const - { - std::stringstream ss; - this->display(ss); - return ss.str(); - } /*display_string*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterInput.cpp */ diff --git a/KalmanFilterInput.hpp b/KalmanFilterInput.hpp deleted file mode 100644 index a73a72d0..00000000 --- a/KalmanFilterInput.hpp +++ /dev/null @@ -1,121 +0,0 @@ -/* @file KalmanFilterInput.hpp */ - -#pragma once - -#include "reflect/SelfTagging.hpp" -#include "time/Time.hpp" -#include "refcnt/Refcounted.hpp" -#include -#include - -namespace xo { - /* FIXME. hack here to get member access working for reflection */ - namespace vf { class StrikesetKfinput; } - - namespace kalman { - /* represents a single kalman filter input event - * comprising: - * - time tkp1 - * - observation vector z[] - * - presence bits presence[] for z - * - observation errors Rd[] - */ - class KalmanFilterInput : public reflect::SelfTagging { - public: - using TaggedRcptr = xo::reflect::TaggedRcptr; - using utc_nanos = xo::time::utc_nanos; - using VectorXb = Eigen::Array; - using VectorXi = Eigen::VectorXi; - using VectorXd = Eigen::VectorXd; - using uint32_t = std::uint32_t; - - public: - KalmanFilterInput() = default; - - static ref::rp make(utc_nanos tkp1, - VectorXb const & presence, - VectorXd const & z, - VectorXd const & Rd); - - /* create input, with all presence bits set + not using Rd */ - static ref::rp make_present(utc_nanos tkp1, - VectorXd const & z); - - /* reflect KalmanFilterInput object representation */ - static void reflect_self(); - - /* alt name -- concession to reactor::DirectSource - * when event type is KalmanFilterInput - */ - utc_nanos tm() const { return tkp1_; } - - utc_nanos tkp1() const { return tkp1_; } - uint32_t n_obs() const { return z_.size(); } - VectorXb const & presence() const { return presence_; } - VectorXd const & z() const { return z_; } - VectorXd const & Rd() const { return Rd_; } - - /* computes reindexer keep[]: - * .presence[keep[j*]] - * is the j*'th true value in .presence - */ - VectorXi make_kept_index() const; - - virtual void display(std::ostream & os) const; - std::string display_string() const; - - // ----- inherited from SelfTagging ----- - - virtual TaggedRcptr self_tp() override; - - protected: - KalmanFilterInput(utc_nanos tkp1, VectorXb presence, VectorXd z, VectorXd Rd) - : tkp1_(tkp1), - presence_{std::move(presence)}, - z_{std::move(z)}, - Rd_{std::move(Rd)} {} - - friend class xo::vf::StrikesetKfinput; - - private: - /* t(k+1) - asof time for observations .z */ - utc_nanos tkp1_ = xo::time::timeutil::epoch(); - /* [m x 1] presence vector. - * an observation z[j] is present iff .presence[j] is true. - * rows/columns for an absent observation are removed from filter matrices - */ - VectorXb presence_; - /* [m x 1] observation vector z(k) */ - VectorXd z_; - - /* [m x 1] observation error vector Rd(k). - * This represents a side channel for passing the diagonal of - * observation matrix R(k), when both: - * (a) error of different observations are assumed to be uncorrelated (likely) - * (b) error variance is derived from input data, e.g. because of - * some input preprocessing. - * - * It's up to KalmanFilterSpec::MkStepFn to opt-in to using this information, - * which requires agreement with any input preparation step. - * - * This variable could just as well provide observation error matrix R - * - * NOTE: perhaps-cleaner alternative would be to inherit KalmanFilterInput to - * introduce additional state, then MkStepFn can dynamic_cast - */ - VectorXd Rd_; - }; /*KalmanFilterInput*/ - - using KalmanFilterInputPtr = ref::rp; - - inline std::ostream & - operator<<(std::ostream & os, KalmanFilterInput const & x) - { - x.display(os); - return os; - } /*operator<<*/ - - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterInput.hpp */ diff --git a/KalmanFilterInputCallback.hpp b/KalmanFilterInputCallback.hpp deleted file mode 100644 index d9b2f213..00000000 --- a/KalmanFilterInputCallback.hpp +++ /dev/null @@ -1,14 +0,0 @@ -/* @file KalmanFilterInputCallback.hpp */ - -#pragma once - -#include "refcnt/Refcounted.hpp" -#include "filter/KalmanFilter.hpp" - -namespace xo { - namespace kalman { - using KalmanFilterInputCallback = reactor::Sink1>; - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterInputCallback.hpp */ diff --git a/KalmanFilterInputSource.hpp b/KalmanFilterInputSource.hpp deleted file mode 100644 index 30c5cbf1..00000000 --- a/KalmanFilterInputSource.hpp +++ /dev/null @@ -1,28 +0,0 @@ -/* @file KalmanFilterInputSource.hpp */ - -#pragma once - -#include "reactor/EventSource.hpp" -#include "filter/KalmanFilterInputCallback.hpp" - -namespace xo { - namespace kalman { - /* Use: - * rp src = ...; - * rp in_cb = ...; - * - * src->add_callback(in_cb); - * ... - * // src invokes in_cb->notify_input( - * src->remove_callback(in_cb); - */ - using KalmanFilterInputSource = reactor::EventSource< - /*KalmanFilterInput,*/ - KalmanFilterInputCallback - /*&KalmanFilterInputCallback::notify_filter*/ - >; - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterInputSource.hpp */ - diff --git a/KalmanFilterInputToConsole.hpp b/KalmanFilterInputToConsole.hpp deleted file mode 100644 index 7d48ef7e..00000000 --- a/KalmanFilterInputToConsole.hpp +++ /dev/null @@ -1,30 +0,0 @@ -/* @file KalmanFilterInputToConsole.hpp */ - -#pragma once - -#include "reactor/Sink.hpp" -#include "filter/KalmanFilterInput.hpp" - -namespace xo { - namespace kalman { - class KalmanFilterInputToConsole - : public xo::reactor::SinkToConsole> - { - public: - KalmanFilterInputToConsole() = default; - - static ref::rp make(); - - virtual void display(std::ostream & os) const; - //virtual std::string display_string() const; - }; /*KalmanFilterInputToConsole*/ - - inline std::ostream & - operator<<(std::ostream & os, KalmanFilterInputToConsole const & x) { - x.display(os); - return os; - } /*operator<<*/ - } /*namespace option*/ -} /*namespace xo*/ - -/* end KalmanFilterInputToConsole.hpp */ diff --git a/KalmanFilterObservable.hpp b/KalmanFilterObservable.hpp deleted file mode 100644 index 87c4f9d1..00000000 --- a/KalmanFilterObservable.hpp +++ /dev/null @@ -1,110 +0,0 @@ -/* @file KalmanFilterObservable.hpp */ - -#pragma once - -#include "time/Time.hpp" -#include -#include - -namespace xo { - namespace kalman { - class KalmanFilterObservable { - public: - using MatrixXd = Eigen::MatrixXd; - using VectorXi = Eigen::VectorXi; - - public: - KalmanFilterObservable() = default; - - /* keep maps back to canonical observations z(j). - * H, R have been re-indexed - * - * If all m observations are included, then keep will be: - * [0, .., m-1] - */ - KalmanFilterObservable(VectorXi keep, MatrixXd H, MatrixXd R) - : keep_{std::move(keep)}, H_{std::move(H)}, R_{std::move(R)} { - assert(this->check_ok()); - } /*ctor*/ - - /* build KF observable object where keeping all observations */ - static KalmanFilterObservable keep_all(MatrixXd H, MatrixXd R); - - /* build KF observable object. replace H, R with reindexed versions H', R' - * according to indexing vector keep[]. keep[] indexes members of - * observation vector z(k)[j]. Reindexed z', H', R' as follows: - * - * z'[j*] = z[keep[j*]] - * H'[j*, i] = H[keep[j*], i] - * R'[j1*, j2*] = R[keep[j1*], keep[j2*]] - */ - static KalmanFilterObservable reindex(VectorXi keep, MatrixXd H, MatrixXd R); - - uint32_t n_state() const { return H_.cols(); } - uint32_t n_observable() const { return H_.rows(); } - VectorXi const & keep() const { return keep_; } - MatrixXd const & observable() const { return H_; } - MatrixXd const & observable_cov() const { return R_; } - - bool check_ok() const { - uint32_t m = H_.rows(); - bool keep_is_mx1 = (keep_.rows() == m); - bool r_is_mxm = ((R_.cols() == m) && (R_.rows() == m)); - - bool keep_is_well_ordered = true; - - /* members of .keep are non-negative integers, - * in strictly increasing order - */ - int64_t keep_jm1 = -1; - - for (uint32_t j = 0; j < keep_.rows(); ++j) { - if (keep_[j] < 0) - keep_is_well_ordered = false; - if (keep_[j] <= keep_jm1) - keep_is_well_ordered = false; - } - - /* also would like to require: R is +ve definite */ - - return keep_is_mx1 && keep_is_well_ordered && r_is_mxm; - } /*check_ok*/ - - void display(std::ostream & os) const; - std::string display_string() const; - - private: - - private: - /* m: #of observations that survived sanity/error checks - * - * H, R here will have been re-indexed to exclude rejected observations. - * observations z will also have been re-indexed. - * - * If an observation z[j] is excluded, then also exclude: - * - j'th row of H - * - j'th row and j'th column of R - * - j'th element of z - * - * Given re-indexed H, R, the j*'th row of H goes with z[keep[j*]] - */ - - /* [m x 1] maps back to indices in original observation vector */ - VectorXi keep_; - /* [m x n] observation matrix */ - MatrixXd H_; - /* [m x m] covariance matrix for observation noise */ - MatrixXd R_; - }; /*KalmanFilterObservable*/ - - inline std::ostream & - operator<<(std::ostream & os, KalmanFilterObservable const & x) - { - x.display(os); - return os; - } /*operator<<*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterObservable.hpp */ - diff --git a/KalmanFilterOutputCallback.hpp b/KalmanFilterOutputCallback.hpp deleted file mode 100644 index 69fb8484..00000000 --- a/KalmanFilterOutputCallback.hpp +++ /dev/null @@ -1,15 +0,0 @@ -/* @file KalmanFilterOutputCallback.hpp */ - -#pragma once - -#include "reactor/Sink.hpp" -#include "filter/KalmanFilter.hpp" - -namespace xo { - namespace kalman { - /* callback for consuming kalman filter output */ - using KalmanFilterOutputCallback = reactor::Sink1>; - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterOutputCallback.hpp */ diff --git a/KalmanFilterSpec.cpp b/KalmanFilterSpec.cpp deleted file mode 100644 index 296aeb99..00000000 --- a/KalmanFilterSpec.cpp +++ /dev/null @@ -1,27 +0,0 @@ -/* @file KalmanFilterSpec.cpp */ - -#include "KalmanFilterSpec.hpp" -#include "indentlog/scope.hpp" - -namespace xo { - using xo::tostr; - using xo::xtag; - - namespace kalman { - void - KalmanFilterSpec::display(std::ostream & os) const - { - os << ""; - } /*display*/ - - std::string - KalmanFilterSpec::display_string() const - { - return tostr(*this); - } /*display_string*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterSpec.cpp */ diff --git a/KalmanFilterSpec.hpp b/KalmanFilterSpec.hpp deleted file mode 100644 index 56ffd783..00000000 --- a/KalmanFilterSpec.hpp +++ /dev/null @@ -1,86 +0,0 @@ -/* @file KalmanFilterSpec.hpp */ - -#pragma once - -#include "filter/KalmanFilterStep.hpp" - -namespace xo { - namespace kalman { - /* full specification for a kalman filter. - * - * For a textbook linear filter, expect a KalmanFilterStep - * instance to be independent of KalmanFilterState/KalmanFilterInput. - * - * Relaxing this requirement for two reasons: - * 1. (proper) want to allow filter with variable timing between observations, - * expecially if observations are event-driven. - * In that case it's likely that state transition matrices are a function - * of elapsed time between observations. Providing filter state sk - * allows MkStepFn to use sk.tm() - * 2. (sketchy) when observations represent market data, - * desirable to treat an observation as giving one-sided information - * about true value. For example treat a bid price as evidence - * true value is higher than that bid, but don't want to constrain - * "how much higher". Certainly no reason to think that - * bid price is normally distributed around fair value. - * Allow for hack in which we - * and modulate error distribution "as if it were normal" to assess - * a non-gaussian error distribution - */ - class KalmanFilterSpec { - public: - /* typical implementation will look something like: - * mk_step(KalmanFilterState const & sk, - * KalmanFilterInputPtr const & zkp1) - * { - * KalmanFilterTransition model = ...; - * KalmanFilterObservable obs = ...; - * - * return KalmanFilterStep(sk, model, obs, zkp1); - * } - */ - using MkStepFn = std::function const & sk, - KalmanFilterInputPtr const & zkp1)>; - - public: - explicit KalmanFilterSpec(ref::rp s0, MkStepFn mkstepfn) - : start_ext_{std::move(s0)}, - mk_step_fn_{std::move(mkstepfn)} {} - - ref::rp const & start_ext() const { return start_ext_; } - /* get step parameters (i.e. matrices F, Q, H, R) - * for step t(k) -> t(k+1). - * - * We supply t(k) state s and t(k+1) observation z(k+1): - * - to allow time stepping to be observation-driven - * - to allow for selective outlier removal - */ - KalmanFilterStep make_step(ref::rp const & sk, - ref::rp const & zkp1) { - return this->mk_step_fn_(sk, zkp1); - } /*make_step*/ - - void display(std::ostream & os) const; - std::string display_string() const; - - private: - /* starting state */ - ref::rp start_ext_; - - /* creates kalman filter step object on demand; - * step object specifies inputs to 1 step in discrete - * linear kalman filter - */ - MkStepFn mk_step_fn_; - }; /*KalmanFilterSpec*/ - - inline std::ostream & - operator<<(std::ostream & os, KalmanFilterSpec const & x) { - x.display(os); - return os; - } /*operator<<*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterSpec.hpp */ diff --git a/KalmanFilterState.cpp b/KalmanFilterState.cpp deleted file mode 100644 index 7656908c..00000000 --- a/KalmanFilterState.cpp +++ /dev/null @@ -1,231 +0,0 @@ -/* @file KalmanFilterState.cpp */ - -#include "KalmanFilterState.hpp" -#include "print_eigen.hpp" -#include "reflect/StructReflector.hpp" -#include "reflect/TaggedPtr.hpp" -#include "indentlog/scope.hpp" -#include "Eigen/src/Core/Matrix.h" -#include -#include - -namespace xo { - using xo::reflect::Reflect; - using xo::reflect::TaggedRcptr; - using xo::reflect::StructReflector; - using xo::time::utc_nanos; - using xo::ref::rp; - using logutil::matrix; - using logutil::vector; - //using xo::scope; - using xo::xtag; - using xo::tostr; - //using Eigen::LDLT; - using Eigen::MatrixXd; - using Eigen::VectorXd; - - namespace kalman { - // ----- KalmanFilterState ----- - - rp - KalmanFilterState::make() - { - return new KalmanFilterState(); - } /*make*/ - - rp - KalmanFilterState::make(uint32_t k, - utc_nanos tk, - VectorXd x, - MatrixXd P, - KalmanFilterTransition transition) - { - return new KalmanFilterState(k, tk, - std::move(x), - std::move(P), - std::move(transition)); - } /*make*/ - - void - KalmanFilterState::reflect_self() - { - StructReflector sr; - - if (sr.is_incomplete()) { - REFLECT_MEMBER(sr, k); - REFLECT_MEMBER(sr, tk); - REFLECT_MEMBER(sr, x); - REFLECT_MEMBER(sr, P); - } - } /*reflect_self*/ - - KalmanFilterState::KalmanFilterState() = default; - - KalmanFilterState::KalmanFilterState(uint32_t k, - utc_nanos tk, - VectorXd x, - MatrixXd P, - KalmanFilterTransition transition) - : k_{k}, tk_{tk}, - x_{std::move(x)}, P_{std::move(P)}, - transition_{std::move(transition)} - {} - - TaggedRcptr - KalmanFilterState::self_tp() - { - return Reflect::make_rctp(this); - } /*self_tp*/ - - // ----- KalmanFilterExt ----- - - rp - KalmanFilterStateExt::make() - { - return new KalmanFilterStateExt(); - } /*make*/ - - rp - KalmanFilterStateExt::make(uint32_t k, - utc_nanos tk, - VectorXd x, - MatrixXd P, - KalmanFilterTransition transition, - MatrixXd K, - int32_t j, - rp zk) - { - return new KalmanFilterStateExt(k, - tk, - std::move(x), - std::move(P), - std::move(transition), - std::move(K), - j, - std::move(zk)); - } /*make*/ - - void - KalmanFilterStateExt::reflect_self() - { - StructReflector sr; - - if (sr.is_incomplete()) { - /* TODO: use sr.adopt_ancestors() */ - - REFLECT_EXPLICIT_MEMBER(sr, "k", &KalmanFilterState::k_); - REFLECT_EXPLICIT_MEMBER(sr, "tk", &KalmanFilterState::tk_); - REFLECT_EXPLICIT_MEMBER(sr, "x", &KalmanFilterState::x_); - REFLECT_EXPLICIT_MEMBER(sr, "P", &KalmanFilterState::P_); - REFLECT_EXPLICIT_MEMBER(sr, "transition", &KalmanFilterState::transition_); - REFLECT_MEMBER(sr, j); - REFLECT_MEMBER(sr, K); - REFLECT_MEMBER(sr, zk); - } - } /*reflect_self*/ - - KalmanFilterStateExt::KalmanFilterStateExt(uint32_t k, - utc_nanos tk, - VectorXd x, - MatrixXd P, - KalmanFilterTransition transition, - MatrixXd K, - int32_t j, - rp zk) - : KalmanFilterState(k, tk, - std::move(x), - std::move(P), - std::move(transition)), - j_{j}, - K_{std::move(K)}, - zk_{std::move(zk)} - { - uint32_t n = x.size(); - - if (n != P.rows() || n != P.cols()) { - std::string err_msg - = tostr("with n=x.size expect [n x n] covar matrix P", - xtag("n", x.size()), - xtag("P.rows", P.rows()), - xtag("P.cols", P.cols())); - - throw std::runtime_error(err_msg); - } - - if ((K.rows() > 0) && (K.rows() > 0)) { - if (n != K.rows()) { - std::string err_msg - = tostr("with n=x.size expect [m x n] gain matrix K", - xtag("n", x.size()), - xtag("K.rows", K.rows()), - xtag("K.cols", K.cols())); - - throw std::runtime_error(err_msg); - } - } else { - /* bypass test with [0 x 0] matrix K; - * normal for initial filter state - */ - } - } /*ctor*/ - - void - KalmanFilterState::display(std::ostream & os) const - { - os << ""; - } /*display*/ - - std::string - KalmanFilterState::display_string() const - { - std::stringstream ss; - ss << *this; - return ss.str(); - } /*display_string*/ - - // ----- KalmanFilterStateExt ----- - - ref::rp - KalmanFilterStateExt::initial(utc_nanos t0, - VectorXd x0, - MatrixXd P0) - { - return KalmanFilterStateExt::make - (0 /*k*/, - t0, - std::move(x0), - std::move(P0), - KalmanFilterTransition(MatrixXd() /*F - not used for initial step*/, - MatrixXd() /*Q - not used for initial step*/), - MatrixXd() /*K - not used for initial step*/, - -1 /*j - not used for initial step*/, - nullptr /*zk - not defined for initial step*/); - } /*initial*/ - - void - KalmanFilterStateExt::display(std::ostream & os) const - { - os << "step_no()) - << xtag("tk", this->tm()) - << xtag("x", matrix(this->state_v())) - << xtag("P", matrix(this->state_cov())) - << xtag("K", matrix(K_)) - << xtag("j", j_) - << ">"; - } /*display*/ - - TaggedRcptr - KalmanFilterStateExt::self_tp() - { - return Reflect::make_rctp(this); - } /*self_tp*/ - } /*namespace filter*/ -} /*namespace xo*/ - -/* end KalmanFilterState.cpp */ diff --git a/KalmanFilterState.hpp b/KalmanFilterState.hpp deleted file mode 100644 index 113c4c6b..00000000 --- a/KalmanFilterState.hpp +++ /dev/null @@ -1,163 +0,0 @@ -/* @file KalmanFilterState.hpp */ - -#pragma once - -#include "reflect/SelfTagging.hpp" -#include "filter/KalmanFilterInput.hpp" -#include "filter/KalmanFilterTransition.hpp" -#include "time/Time.hpp" -#include -#include -#include - -namespace xo { - namespace kalman { - /* encapsulate state (i.e. initial state, and output after each step) - * for a kalman filter - */ - class KalmanFilterState : public reflect::SelfTagging { - public: - using TaggedRcptr = reflect::TaggedRcptr; - using utc_nanos = xo::time::utc_nanos; - using VectorXd = Eigen::VectorXd; - using MatrixXd = Eigen::MatrixXd; - using uint32_t = std::uint32_t; - - public: - static ref::rp make(); - static ref::rp make(uint32_t k, - utc_nanos tk, - VectorXd x, - MatrixXd P, - KalmanFilterTransition transition); - virtual ~KalmanFilterState() = default; - - /* reflect KalmanFilterState object representation */ - static void reflect_self(); - - uint32_t step_no() const { return k_; } - utc_nanos tm() const { return tk_; } - /* with n = .n_state(): - * x_ is [n x 1] vector - * P_ is [n x n] matrix, - */ - uint32_t n_state() const { return x_.size(); } - VectorXd const & state_v() const { return x_; } - MatrixXd const & state_cov() const { return P_; } - - KalmanFilterTransition const & transition() const { return transition_; } - - virtual void display(std::ostream & os) const; - std::string display_string() const; - - // ----- inherited from SelfTagging ----- - - virtual TaggedRcptr self_tp() override; - - private: - KalmanFilterState(); - KalmanFilterState(uint32_t k, - utc_nanos tk, - VectorXd x, - MatrixXd P, - KalmanFilterTransition transition); - - friend class KalmanFilterStateExt; - - private: - /* step# k, advances by +1 on each filter step */ - uint32_t k_ = 0; - /* time t(k) */ - utc_nanos tk_; - /* [n x 1] (estimated) system state xk = x(k) */ - VectorXd x_; - /* [n x n] covariance matrix for error assoc'd with with x(k) - * P(i,j) is the covariance of (ek[i], ek[j]), - * where ex(k) is the difference (x(k) - x_(k)) - * between estimated state x(k) - * (= this->x_) and model state x_(k) - */ - MatrixXd P_; - - /* F, Q matrices driving .x, .P */ - KalmanFilterTransition transition_; - }; /*KalmanFilterState*/ - - inline std::ostream & operator<<(std::ostream & os, - KalmanFilterState const & s) - { - s.display(os); - return os; - } /*operator<<*/ - - /* KalmanFilterStateExt: - * adds additional details from filter step to KalmanFilterState - */ - class KalmanFilterStateExt : public KalmanFilterState { - public: - using TaggedRcptr = reflect::TaggedRcptr; - using MatrixXd = Eigen::MatrixXd; - using int32_t = std::int32_t; - - public: - static ref::rp make(); - static ref::rp make(uint32_t k, - utc_nanos tk, - VectorXd x, - MatrixXd P, - KalmanFilterTransition transition, - MatrixXd K, - int32_t j, - ref::rp zk); - - /* create state object for initial filter state */ - static ref::rp initial(utc_nanos t0, - VectorXd x0, - MatrixXd P0); - - /* reflect KalmanFilterStateExt object representation */ - static void reflect_self(); - - int32_t observable() const { return j_; } - MatrixXd const & gain() const { return K_; } - ref::rp const & zk() const { return zk_; } - - virtual void display(std::ostream & os) const override; - - // ----- inherited from SelfTagging ----- - - virtual TaggedRcptr self_tp() override; - - private: - KalmanFilterStateExt() = default; - KalmanFilterStateExt(uint32_t k, - utc_nanos tk, - VectorXd x, - MatrixXd P, - KalmanFilterTransition transition, - MatrixXd K, - int32_t j, - ref::rp zk); - - private: - /* if -1: not used; - * if >= 0: identifies j'th of m observables; - * gain .K applies just to information obtainable from - * observing that scalar variable - */ - int32_t j_ = -1; - /* if .j is -1: - * [n x n] kalman gain - * if .j >= 0: - * [n x 1] kalman gain for observable #j - */ - MatrixXd K_; - /* input leading to state k. - * empty for initial state (i.e. when .k is 0) - */ - ref::rp zk_; - }; /*KalamnFilterStateExt*/ - } /*namespace filter*/ -} /*namespace xo*/ - -/* end KalmanFilterState.hpp */ diff --git a/KalmanFilterStateToConsole.hpp b/KalmanFilterStateToConsole.hpp deleted file mode 100644 index 0d525cea..00000000 --- a/KalmanFilterStateToConsole.hpp +++ /dev/null @@ -1,30 +0,0 @@ -/* @file KalmanFilterStateToConsole.hpp */ - -#pragma once - -#include "reactor/Sink.hpp" -#include "filter/KalmanFilterState.hpp" - -namespace xo { - namespace kalman { - class KalmanFilterStateToConsole - : public xo::reactor::SinkToConsole - { - public: - KalmanFilterStateToConsole() = default; - - static ref::rp make(); - - virtual void display(std::ostream & os) const; - //virtual std::string display_string() const; - }; /*KalmanFilterStateToConsole*/ - - inline std::ostream & - operator<<(std::ostream & os, KalmanFilterStateToConsole const & x) { - x.display(os); - return os; - } /*operator<<*/ - } /*namespace option*/ -} /*namespace xo*/ - -/* end KalmanFilterStateToConsole.hpp */ diff --git a/KalmanFilterStep.cpp b/KalmanFilterStep.cpp deleted file mode 100644 index f62f2d21..00000000 --- a/KalmanFilterStep.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/* @file KalmanFilterStep.cpp */ - -#include "KalmanFilterStep.hpp" -#include "filter/KalmanFilterEngine.hpp" -#include "filter/KalmanFilterState.hpp" -#include "indentlog/scope.hpp" - -namespace xo { - using xo::scope; - using xo::tostr; - using xo::xtag; - using Eigen::MatrixXd; - using Eigen::VectorXd; - - namespace kalman { - ref::rp - KalmanFilterStep::extrapolate() const - { - return KalmanFilterEngine::extrapolate(this->tkp1(), - this->state(), - this->model() /*transition*/); - } /*extrapolate*/ - - MatrixXd - KalmanFilterStep::gain(ref::rp const & skp1_ext) const - { - return KalmanFilterEngine::kalman_gain(skp1_ext, - this->obs()); - } /*gain*/ - - VectorXd - KalmanFilterStep::gain1(ref::rp const & skp1_ext, - uint32_t j) const - { - return KalmanFilterEngine::kalman_gain1(skp1_ext, - this->obs(), - j); - - } /*gain1*/ - - ref::rp - KalmanFilterStep::correct(ref::rp const & skp1_ext) - { - return KalmanFilterEngine::correct(skp1_ext, - this->obs(), - this->input()); - } /*correct*/ - - ref::rp - KalmanFilterStep::correct1(ref::rp const & skp1_ext, - uint32_t j) - { - return KalmanFilterEngine::correct1(skp1_ext, - this->obs(), - this->input(), - j); - } /*correct1*/ - - void - KalmanFilterStep::display(std::ostream & os) const - { - //scope lscope("KalmanFilterStep::display"); - - os << "model()); - //lscope.log("obs:"); - os << xtag("obs", this->obs()); - //lscope.log("input:"); - os << xtag("input", this->input()); - os << ">"; - } /*display*/ - - std::string - KalmanFilterStep::display_string() const - { - return tostr(*this); - } /*display_string*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterStep.cpp */ diff --git a/KalmanFilterSvc.hpp b/KalmanFilterSvc.hpp deleted file mode 100644 index 33d25514..00000000 --- a/KalmanFilterSvc.hpp +++ /dev/null @@ -1,64 +0,0 @@ -/* @file KalmanFilterSvc.hpp */ - -#include "reactor/Sink.hpp" -#include "reactor/DirectSourcePtr.hpp" -#include "filter/KalmanFilter.hpp" -#include "filter/KalmanFilterInputSource.hpp" -#include "filter/KalmanFilterOutputCallback.hpp" -#include "callback/CallbackSet.hpp" - -namespace xo { - namespace kalman { - /* encapsulate a passive KalmanFilter - * instance as an active event consumer+producer - * - * sinks that want to consume KalmanFilterSvc events will use - * .attach_sink() (or .add_callback()) - */ - class KalmanFilterSvc : public xo::reactor::Sink1>, - public xo::reactor::DirectSourcePtr> { - public: - using AbstractSource = xo::reactor::AbstractSource; - - public: - /* named ctor idiom */ - static ref::rp make(KalmanFilterSpec spec); - - KalmanFilter const & filter() const { return filter_; } - - /* notify incoming observations; will trigger kalman filter step */ - void notify_ev(ref::rp const & input_kp1) override; - - // ----- inherited from reactor::AbstractSink ----- - - /* filter captures KF input pointer */ - virtual bool allow_volatile_source() const override { return false; } - virtual uint32_t n_in_ev() const override { return n_in_ev_; } - virtual void display(std::ostream & os) const override; - - // ----- inherited from reactor::AbstractSource ----- - - /* note: correct since KalmanFilterEngine.extrapolate() - * always creates new state object - */ - virtual bool is_volatile() const override { return false; } - - // ----- Inherited from AbstractEventProcessor ----- - - private: - KalmanFilterSvc(KalmanFilterSpec spec); - - private: - /* passive kalman filter */ - KalmanFilter filter_; - /* receive filter input from this source; see .attach_input() */ - ref::rp input_src_; - /* counts lifetime #of input events (see .notify_ev()) */ - uint32_t n_in_ev_ = 0; - /* publish filter state updates to these callbacks */ - fn::RpCallbackSet pub_; - }; /*KalmanFilterSvc*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* KalmanFilterSvc.hpp */ diff --git a/KalmanFilterTransition.cpp b/KalmanFilterTransition.cpp deleted file mode 100644 index 65be627b..00000000 --- a/KalmanFilterTransition.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/* @file KalmanFilterTransition.cpp */ - -#include "KalmanFilterTransition.hpp" -#include "reflect/StructReflector.hpp" -#include "print_eigen.hpp" -#include "indentlog/scope.hpp" - -namespace xo { - using xo::reflect::StructReflector; - using logutil::matrix; - using xo::xtag; - - namespace kalman { - void - KalmanFilterTransition::reflect_self() - { - StructReflector sr; - - if (sr.is_incomplete()) { - REFLECT_MEMBER(sr, F); - REFLECT_MEMBER(sr, Q); - } - } /*reflect_self*/ - - uint32_t - KalmanFilterTransition::n_state() const - { - /* we know F.rows() == F.cols() = Q.cols() == Q.rows(), - * see .check_ok() - */ - - return F_.rows(); - } /*n_state*/ - - void - KalmanFilterTransition::display(std::ostream & os) const - { - os << ""; - } /*display*/ - - std::string - KalmanFilterTransition::display_string() const - { - std::stringstream ss; - this->display(ss); - return ss.str(); - } /*display_string*/ - - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterTransition.cpp */ diff --git a/KalmanFilterTransition.hpp b/KalmanFilterTransition.hpp deleted file mode 100644 index fc777c00..00000000 --- a/KalmanFilterTransition.hpp +++ /dev/null @@ -1,62 +0,0 @@ -/* @file KalmanFilterTransition.hpp */ - -#pragma once - -#include "time/Time.hpp" -#include -#include - -namespace xo { - namespace kalman { - - /* encapsulate transition behavior for a kalman filter - * before taking observations into account - */ - class KalmanFilterTransition { - public: - using MatrixXd = Eigen::MatrixXd; - using uint32_t = std::uint32_t; - - public: - KalmanFilterTransition() = default; - KalmanFilterTransition(MatrixXd F, - MatrixXd Q) - : F_{std::move(F)}, Q_{std::move(Q)} { assert(this->check_ok()); } - - static void reflect_self(); - - /* n: cardinality of state vector */ - uint32_t n_state() const; - - MatrixXd const & transition_mat() const { return F_; } - MatrixXd const & transition_cov() const { return Q_; } - - bool check_ok() const { - uint32_t n = F_.rows(); - bool f_is_nxn = ((F_.rows() == n) && (F_.cols() == n)); - bool q_is_nxn = ((Q_.rows() == n) && (Q_.cols() == n)); - - /* also would like to require: Q is +ve definite */ - - return f_is_nxn && q_is_nxn; - } /*check_ok*/ - - void display(std::ostream & os) const; - std::string display_string() const; - - private: - /* [n x n] state transition matrix */ - MatrixXd F_; - /* [n x n] covariance matrix for system noise */ - MatrixXd Q_; - }; /*KalmanFilterTransition*/ - - inline std::ostream & - operator<<(std::ostream & os, KalmanFilterTransition const & x) { - x.display(os); - return os; - } /*operator<<*/ - } /*namespace kalman*/ -} /*namespace xo*/ - -/* end KalmanFilterTransition.hpp */ diff --git a/cmake/xo_kalmanfilterConfig.cmake.in b/cmake/xo_kalmanfilterConfig.cmake.in new file mode 100644 index 00000000..366e45ba --- /dev/null +++ b/cmake/xo_kalmanfilterConfig.cmake.in @@ -0,0 +1,17 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) + +# note: changes to find_dependency() calls here +# must coordinate with xo_dependency() calls +# in xo-reactor/src/reactor/CMakeLists.txt +# +find_dependency(reactor) +find_dependnecy(eigen3) +#find_dependency(reflect) +#find_dependency(webutil) +#find_dependency(printjson) +#find_dependency(callback) + +include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") +check_required_components("@PROJECT_NAME@") diff --git a/EigenUtil.hpp b/include/xo/kalmanfilter/EigenUtil.hpp similarity index 100% rename from EigenUtil.hpp rename to include/xo/kalmanfilter/EigenUtil.hpp diff --git a/include/xo/kalmanfilter/KalmanFilter.hpp b/include/xo/kalmanfilter/KalmanFilter.hpp new file mode 100644 index 00000000..0df87a61 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilter.hpp @@ -0,0 +1,127 @@ +/* @file KalmanFilter.hpp */ + +#pragma once + +#include "KalmanFilterSpec.hpp" + +namespace xo { + namespace kalman { + /* Specification for an ordinary discrete linear kalman filter. + * + * The filter generates estimates for a process observed at a discrete + * set of times tk in {t0, t1, .., tn} + * + * At each time tk we have the following: + * + * 0. x(0) initial estimate at t(0) + * P(0) initial priors: error covariance matrix for x(0) + * + * 1. x_(k), [n x 1] vector: + * system state, denoted by vector. + * (state is not directly observable, + * filter will attempt to estimate it) + * + * 2. w_(k), [n x 1] vector + * Q(k), [n x n] matrix + * + * w_(k) denotes system noise, + * gaussian with covariance Q(k). + * noise w_(k) is not directly observable. + * + * 3. z(k), [m x 1] vector: + * + * observation vector for time tk + * + * 4. v_(k), [m x 1] vector + * R(k), [m x m] matrix + * + * v_(k) denotes observation errors, + * gaussian with covariance R(k). + * noise v_(k) is not directly observable. + * + * 5. F(k), [n x n] matrix + * state transition matrix + * model system evolves according to: + * + * x_(k+1) = F(x).x_(k) + w_(k) + * + * 6. observations z(k) depend on system state: + * + * z(k) = H(k).x_(k) + v_(k) + * + * 7. Kalman filter outputs: + * x(k), [n x 1] vector + * Q(k), [n x n] matrix + * + * x(k) is optimal estimate for system state x_(k) + * P(k) is covariance matrix specifying confidence intervals + * for pairs (x(k)[i], x(k)[j]) + * + * filter specification consists of: + * n, m, x(0), P(0), F(k), Q(k), H(k), R(k) + * The cardinality of observations z(k) can vary over time, + * so to be precise, m can vary with tk; write as m(k) + * + * More details: + * - avoid having to specify t(k) in advance; + * instead defer until observation available + * so t(k) can be taken from polling timestamp + */ + + /* encapsulate a (linear) kalman filter + * together with event publishing + */ + class KalmanFilter { + public: + using MatrixXd = Eigen::MatrixXd; + using VectorXd = Eigen::VectorXd; + using utc_nanos = xo::time::utc_nanos; + + public: + /* create filter with specification given by spec, and initial state s0 */ + explicit KalmanFilter(KalmanFilterSpec spec); + + uint32_t step_no() const { return state_ext_->step_no(); } + utc_nanos tm() const { return state_ext_->tm(); } + KalmanFilterSpec const & filter_spec() const { return filter_spec_; } + KalmanFilterStep const & step() const { return step_; } + ref::rp const & state_ext() const { return state_ext_; } + + /* notify kalman filter with input for time t(k+1) = input_kp1.tkp1() + * Require: input.tkp1() >= .current_tm() + * Promise: + * - .tm() = input_kp1.tkp1() + * - .step_no() = old .step_no() + 1 + * - .filter_spec_k, .step_k, .state_k updated + * for observations in input_kp1 + */ + void notify_input(ref::rp const & input_kp1); + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + /* specification for kalman filter; + * produces process/observation matrices on demand + */ + KalmanFilterSpec filter_spec_; + + /* filter step for most recent observation */ + KalmanFilterStep step_; + + /* filter state as of most recent observation; + * result of applying KalmanFilterEngine::step() to contents of .step + */ + ref::rp state_ext_; + }; /*KalmanFilter*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilter const & x) { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilter.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterEngine.hpp b/include/xo/kalmanfilter/KalmanFilterEngine.hpp new file mode 100644 index 00000000..ce21bf3c --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterEngine.hpp @@ -0,0 +1,185 @@ +/* @file KalmanFilterEngine.hpp */ + +#pragma once + +#include "KalmanFilterStep.hpp" +#include "KalmanFilterState.hpp" +#include "KalmanFilterTransition.hpp" +#include "KalmanFilterObservable.hpp" +#include "KalmanFilterInput.hpp" + +namespace xo { + namespace kalman { + class KalmanFilterEngine { + public: + using MatrixXd = Eigen::MatrixXd; + using VectorXd = Eigen::VectorXd; + using utc_nanos = xo::time::utc_nanos; + + public: + /* evolution of system state + account for system noise, + * before incorporating effect of observations z(k+1) + * x(k) --> x(k+1|k) + * P(k) --> P(k+1|k) + * + * tkp1. time t(k+1) assoc'd with step k+1 + * sk. filter state at time tk: + * sk.k = k step # (starts from 0) + * sk.tk = t(k) time t(k) assoc'd with step #k + * sk.x = x(k) estimated state at time tk + * sk.P = P(k) quality of state estimate x(k) + * (error covariance matrix) + * Fk. state transition: + * Fk.F = F(k) state transition matrix + * Fk.Q = Q(k) covariance matrix for system noise + * + * returns propagated state estimate for t(k+1): + * retval.k = k+1 + * retval.tk = t(k+1) = tkp1 + * retval.x = x(k+1|k) + * retval.P = P(k+1|k) + */ + static ref::rp extrapolate(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk); + + /* compute kalman gain matrix for filter step t(k) -> t(k+1) + * Expensive implementation using matrix inversion + * + * T + * M(k+1) = H(k).P(k+1|k).H(k) + R(k) + * + * T -1 + * K(k+1) = P(k+1|k).H(k) .M(k+1) + * + * Require: + * - skp1_ext.n_state() = Hkp1.n_state() + * + * skp1_ext. extrapolated filter state at t(k+1) + * Hkp1. relates model state to observable variables, + * for step t(k) -> t(k+1) + */ + static MatrixXd kalman_gain(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1); + + /* compute kalman gain for a single observation z(k)[j]. + * This is useful iff the observation error matrix R is diagonal. + * For diagonal R we can present a set of observations z(k) serially + * instead of all at once, with lower time complexity + * + * Kalman Filter specifies some space with m observables. + * j identifies one of those observables, indexing from 0. + * This corresponds to row #j of H(k), and element R[j,j] of R. + * + * Effectively, we are projecting the kalman filter assoc'd with + * {skp1_ext, Hkp1} to a filter with a single observable variable z(k)[j], + * then computing the (scalar) kalman gain for this 1-variable filter + * + * The gain vector tells us for each member of filter state, + * how much to adjust our optimal estimate for that member for a unit + * amount of innovation in observable #j, i.e. for difference between + * expected and actual value for that observable. + */ + static VectorXd kalman_gain1(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + uint32_t j); + + /* correct extrapolated state+cov estimate; + * also computes kalman gain + * + * Require: + * - skp1_ext.n_state() = Hkp1.n_state() + * - zkp1.n_obs() == Hkp1.n_observable() + * + * skp1_ext. extrapolated kalman state + covaraince at t(k+1) + * Hkp1. relates model state to observable variables + * for step t(k) -> t(k+1) + * zkp1. observations for time t(k+1) + * + * return new filter state+cov + */ + static ref::rp correct(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1); + + /* correct extrapolated filter state for observation + * of j'th filter observable z(k+1)[j] + * + * Can use this when observation errors are uncorrelated + * (i.e. observation error matrix R is diagonal) + */ + static ref::rp correct1(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1, + uint32_t j); + + /* step filter from t(k) -> t(k+1) + * + * sk. filter state from previous step: + * x (state vector), P (state covar matrix) + * Fk. transition-related params: + * F (transition matrix), Q (system noise covar matrix) + * Hkp1. observation-related params: + * H (coupling matrix), R (error covar matrix) + * zkp1. observations z(k+1) for time t(k+1) + */ + static ref::rp step(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1); + + /* step filter from t(k) -> tk(k+1) + * same as + * .step(tkp1, sk, step_spec.model(), step_spec.obs(), zkp1); + * + * step_spec. encapsulates Fk (transition-related params) + * and Q (system noise covar matrix) + */ + static ref::rp step(KalmanFilterStep const & step_spec); + + /* step filter from t(k) -> t(k+1) + * + * sk. filter state from previous step: + * x (state vector), P (state covar matrix) + * Fk. transition-related params: + * F (transition matrix), Q (system noise covar matrix) + * Hkp1. observation-related params: + * H (coupling matrix), R (error covar matrix) + * zkp1. observations z(k+1) for time t(k+1) + * j. identifies a single filter observable -- + * step will only consume observation z(k+1)[j] + */ + static ref::rp step1(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1, + uint32_t j); + + /* step filter from t(k) -> t(k+1) + * + * same as + * .step1(step_spec.tkp1(), + * step_spec.state(), + * step_spec.model(), + * step_spec.obs(), + * step_spec.input(), + * j); + * + * step_spec. encapsulates + * x (state vector), + * P (state covar matrix), + * Fk (transition-related params), + * Q (system noise covar matrix) + * z (z(k+1), observations at time t(k+1)) + * j. identifies a single filter observable -- + * step will only consume observation z(k+1)[j] + */ + static ref::rp step1(KalmanFilterStep const & step_spec, + uint32_t j); + }; /*KalmanFilterEngine*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterEngine.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterInput.hpp b/include/xo/kalmanfilter/KalmanFilterInput.hpp new file mode 100644 index 00000000..70fee5af --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterInput.hpp @@ -0,0 +1,121 @@ +/* @file KalmanFilterInput.hpp */ + +#pragma once + +#include "xo/reflect/SelfTagging.hpp" +//#include "time/Time.hpp" +//#include "xo/refcnt/Refcounted.hpp" +#include +#include + +namespace xo { + /* FIXME. hack here to get member access working for reflection */ + namespace vf { class StrikesetKfinput; } + + namespace kalman { + /* represents a single kalman filter input event + * comprising: + * - time tkp1 + * - observation vector z[] + * - presence bits presence[] for z + * - observation errors Rd[] + */ + class KalmanFilterInput : public reflect::SelfTagging { + public: + using TaggedRcptr = xo::reflect::TaggedRcptr; + using utc_nanos = xo::time::utc_nanos; + using VectorXb = Eigen::Array; + using VectorXi = Eigen::VectorXi; + using VectorXd = Eigen::VectorXd; + using uint32_t = std::uint32_t; + + public: + KalmanFilterInput() = default; + + static ref::rp make(utc_nanos tkp1, + VectorXb const & presence, + VectorXd const & z, + VectorXd const & Rd); + + /* create input, with all presence bits set + not using Rd */ + static ref::rp make_present(utc_nanos tkp1, + VectorXd const & z); + + /* reflect KalmanFilterInput object representation */ + static void reflect_self(); + + /* alt name -- concession to reactor::DirectSource + * when event type is KalmanFilterInput + */ + utc_nanos tm() const { return tkp1_; } + + utc_nanos tkp1() const { return tkp1_; } + uint32_t n_obs() const { return z_.size(); } + VectorXb const & presence() const { return presence_; } + VectorXd const & z() const { return z_; } + VectorXd const & Rd() const { return Rd_; } + + /* computes reindexer keep[]: + * .presence[keep[j*]] + * is the j*'th true value in .presence + */ + VectorXi make_kept_index() const; + + virtual void display(std::ostream & os) const; + std::string display_string() const; + + // ----- inherited from SelfTagging ----- + + virtual TaggedRcptr self_tp() override; + + protected: + KalmanFilterInput(utc_nanos tkp1, VectorXb presence, VectorXd z, VectorXd Rd) + : tkp1_(tkp1), + presence_{std::move(presence)}, + z_{std::move(z)}, + Rd_{std::move(Rd)} {} + + friend class xo::vf::StrikesetKfinput; + + private: + /* t(k+1) - asof time for observations .z */ + utc_nanos tkp1_ = xo::time::timeutil::epoch(); + /* [m x 1] presence vector. + * an observation z[j] is present iff .presence[j] is true. + * rows/columns for an absent observation are removed from filter matrices + */ + VectorXb presence_; + /* [m x 1] observation vector z(k) */ + VectorXd z_; + + /* [m x 1] observation error vector Rd(k). + * This represents a side channel for passing the diagonal of + * observation matrix R(k), when both: + * (a) error of different observations are assumed to be uncorrelated (likely) + * (b) error variance is derived from input data, e.g. because of + * some input preprocessing. + * + * It's up to KalmanFilterSpec::MkStepFn to opt-in to using this information, + * which requires agreement with any input preparation step. + * + * This variable could just as well provide observation error matrix R + * + * NOTE: perhaps-cleaner alternative would be to inherit KalmanFilterInput to + * introduce additional state, then MkStepFn can dynamic_cast + */ + VectorXd Rd_; + }; /*KalmanFilterInput*/ + + using KalmanFilterInputPtr = ref::rp; + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterInput const & x) + { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInput.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp b/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp new file mode 100644 index 00000000..df86a16b --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterInputCallback.hpp @@ -0,0 +1,14 @@ +/* @file KalmanFilterInputCallback.hpp */ + +#pragma once + +#include "xo/refcnt/Refcounted.hpp" +#include "KalmanFilter.hpp" + +namespace xo { + namespace kalman { + using KalmanFilterInputCallback = reactor::Sink1>; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInputCallback.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterInputSource.hpp b/include/xo/kalmanfilter/KalmanFilterInputSource.hpp new file mode 100644 index 00000000..7e801936 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterInputSource.hpp @@ -0,0 +1,27 @@ +/* @file KalmanFilterInputSource.hpp */ + +#pragma once + +#include "xo/reactor/EventSource.hpp" +#include "KalmanFilterInputCallback.hpp" + +namespace xo { + namespace kalman { + /* Use: + * rp src = ...; + * rp in_cb = ...; + * + * src->add_callback(in_cb); + * ... + * // src invokes in_cb->notify_input( + * src->remove_callback(in_cb); + */ + using KalmanFilterInputSource = reactor::EventSource< + /*KalmanFilterInput,*/ + KalmanFilterInputCallback + /*&KalmanFilterInputCallback::notify_filter*/ + >; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInputSource.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp b/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp new file mode 100644 index 00000000..7d81226a --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp @@ -0,0 +1,30 @@ +/* @file KalmanFilterInputToConsole.hpp */ + +#pragma once + +#include "xo/reactor/Sink.hpp" +#include "KalmanFilterInput.hpp" + +namespace xo { + namespace kalman { + class KalmanFilterInputToConsole + : public xo::reactor::SinkToConsole> + { + public: + KalmanFilterInputToConsole() = default; + + static ref::rp make(); + + virtual void display(std::ostream & os) const; + //virtual std::string display_string() const; + }; /*KalmanFilterInputToConsole*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterInputToConsole const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace option*/ +} /*namespace xo*/ + +/* end KalmanFilterInputToConsole.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterObservable.hpp b/include/xo/kalmanfilter/KalmanFilterObservable.hpp new file mode 100644 index 00000000..c3981095 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterObservable.hpp @@ -0,0 +1,109 @@ +/* @file KalmanFilterObservable.hpp */ + +#pragma once + +//#include "time/Time.hpp" +#include +#include + +namespace xo { + namespace kalman { + class KalmanFilterObservable { + public: + using MatrixXd = Eigen::MatrixXd; + using VectorXi = Eigen::VectorXi; + + public: + KalmanFilterObservable() = default; + + /* keep maps back to canonical observations z(j). + * H, R have been re-indexed + * + * If all m observations are included, then keep will be: + * [0, .., m-1] + */ + KalmanFilterObservable(VectorXi keep, MatrixXd H, MatrixXd R) + : keep_{std::move(keep)}, H_{std::move(H)}, R_{std::move(R)} { + assert(this->check_ok()); + } /*ctor*/ + + /* build KF observable object where keeping all observations */ + static KalmanFilterObservable keep_all(MatrixXd H, MatrixXd R); + + /* build KF observable object. replace H, R with reindexed versions H', R' + * according to indexing vector keep[]. keep[] indexes members of + * observation vector z(k)[j]. Reindexed z', H', R' as follows: + * + * z'[j*] = z[keep[j*]] + * H'[j*, i] = H[keep[j*], i] + * R'[j1*, j2*] = R[keep[j1*], keep[j2*]] + */ + static KalmanFilterObservable reindex(VectorXi keep, MatrixXd H, MatrixXd R); + + uint32_t n_state() const { return H_.cols(); } + uint32_t n_observable() const { return H_.rows(); } + VectorXi const & keep() const { return keep_; } + MatrixXd const & observable() const { return H_; } + MatrixXd const & observable_cov() const { return R_; } + + bool check_ok() const { + uint32_t m = H_.rows(); + bool keep_is_mx1 = (keep_.rows() == m); + bool r_is_mxm = ((R_.cols() == m) && (R_.rows() == m)); + + bool keep_is_well_ordered = true; + + /* members of .keep are non-negative integers, + * in strictly increasing order + */ + int64_t keep_jm1 = -1; + + for (uint32_t j = 0; j < keep_.rows(); ++j) { + if (keep_[j] < 0) + keep_is_well_ordered = false; + if (keep_[j] <= keep_jm1) + keep_is_well_ordered = false; + } + + /* also would like to require: R is +ve definite */ + + return keep_is_mx1 && keep_is_well_ordered && r_is_mxm; + } /*check_ok*/ + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + + private: + /* m: #of observations that survived sanity/error checks + * + * H, R here will have been re-indexed to exclude rejected observations. + * observations z will also have been re-indexed. + * + * If an observation z[j] is excluded, then also exclude: + * - j'th row of H + * - j'th row and j'th column of R + * - j'th element of z + * + * Given re-indexed H, R, the j*'th row of H goes with z[keep[j*]] + */ + + /* [m x 1] maps back to indices in original observation vector */ + VectorXi keep_; + /* [m x n] observation matrix */ + MatrixXd H_; + /* [m x m] covariance matrix for observation noise */ + MatrixXd R_; + }; /*KalmanFilterObservable*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterObservable const & x) + { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterObservable.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp b/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp new file mode 100644 index 00000000..cd858606 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp @@ -0,0 +1,15 @@ +/* @file KalmanFilterOutputCallback.hpp */ + +#pragma once + +#include "xo/reactor/Sink.hpp" +#include "KalmanFilter.hpp" + +namespace xo { + namespace kalman { + /* callback for consuming kalman filter output */ + using KalmanFilterOutputCallback = reactor::Sink1>; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterOutputCallback.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterSpec.hpp b/include/xo/kalmanfilter/KalmanFilterSpec.hpp new file mode 100644 index 00000000..306f298a --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterSpec.hpp @@ -0,0 +1,86 @@ +/* @file KalmanFilterSpec.hpp */ + +#pragma once + +#include "KalmanFilterStep.hpp" + +namespace xo { + namespace kalman { + /* full specification for a kalman filter. + * + * For a textbook linear filter, expect a KalmanFilterStep + * instance to be independent of KalmanFilterState/KalmanFilterInput. + * + * Relaxing this requirement for two reasons: + * 1. (proper) want to allow filter with variable timing between observations, + * expecially if observations are event-driven. + * In that case it's likely that state transition matrices are a function + * of elapsed time between observations. Providing filter state sk + * allows MkStepFn to use sk.tm() + * 2. (sketchy) when observations represent market data, + * desirable to treat an observation as giving one-sided information + * about true value. For example treat a bid price as evidence + * true value is higher than that bid, but don't want to constrain + * "how much higher". Certainly no reason to think that + * bid price is normally distributed around fair value. + * Allow for hack in which we + * and modulate error distribution "as if it were normal" to assess + * a non-gaussian error distribution + */ + class KalmanFilterSpec { + public: + /* typical implementation will look something like: + * mk_step(KalmanFilterState const & sk, + * KalmanFilterInputPtr const & zkp1) + * { + * KalmanFilterTransition model = ...; + * KalmanFilterObservable obs = ...; + * + * return KalmanFilterStep(sk, model, obs, zkp1); + * } + */ + using MkStepFn = std::function const & sk, + KalmanFilterInputPtr const & zkp1)>; + + public: + explicit KalmanFilterSpec(ref::rp s0, MkStepFn mkstepfn) + : start_ext_{std::move(s0)}, + mk_step_fn_{std::move(mkstepfn)} {} + + ref::rp const & start_ext() const { return start_ext_; } + /* get step parameters (i.e. matrices F, Q, H, R) + * for step t(k) -> t(k+1). + * + * We supply t(k) state s and t(k+1) observation z(k+1): + * - to allow time stepping to be observation-driven + * - to allow for selective outlier removal + */ + KalmanFilterStep make_step(ref::rp const & sk, + ref::rp const & zkp1) { + return this->mk_step_fn_(sk, zkp1); + } /*make_step*/ + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + /* starting state */ + ref::rp start_ext_; + + /* creates kalman filter step object on demand; + * step object specifies inputs to 1 step in discrete + * linear kalman filter + */ + MkStepFn mk_step_fn_; + }; /*KalmanFilterSpec*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterSpec const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterSpec.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterState.hpp b/include/xo/kalmanfilter/KalmanFilterState.hpp new file mode 100644 index 00000000..504def9c --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterState.hpp @@ -0,0 +1,163 @@ +/* @file KalmanFilterState.hpp */ + +#pragma once + +#include "xo/reflect/SelfTagging.hpp" +#include "KalmanFilterInput.hpp" +#include "KalmanFilterTransition.hpp" +//#include "time/Time.hpp" +#include +#include +#include + +namespace xo { + namespace kalman { + /* encapsulate state (i.e. initial state, and output after each step) + * for a kalman filter + */ + class KalmanFilterState : public reflect::SelfTagging { + public: + using TaggedRcptr = reflect::TaggedRcptr; + using utc_nanos = xo::time::utc_nanos; + using VectorXd = Eigen::VectorXd; + using MatrixXd = Eigen::MatrixXd; + using uint32_t = std::uint32_t; + + public: + static ref::rp make(); + static ref::rp make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition); + virtual ~KalmanFilterState() = default; + + /* reflect KalmanFilterState object representation */ + static void reflect_self(); + + uint32_t step_no() const { return k_; } + utc_nanos tm() const { return tk_; } + /* with n = .n_state(): + * x_ is [n x 1] vector + * P_ is [n x n] matrix, + */ + uint32_t n_state() const { return x_.size(); } + VectorXd const & state_v() const { return x_; } + MatrixXd const & state_cov() const { return P_; } + + KalmanFilterTransition const & transition() const { return transition_; } + + virtual void display(std::ostream & os) const; + std::string display_string() const; + + // ----- inherited from SelfTagging ----- + + virtual TaggedRcptr self_tp() override; + + private: + KalmanFilterState(); + KalmanFilterState(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition); + + friend class KalmanFilterStateExt; + + private: + /* step# k, advances by +1 on each filter step */ + uint32_t k_ = 0; + /* time t(k) */ + utc_nanos tk_; + /* [n x 1] (estimated) system state xk = x(k) */ + VectorXd x_; + /* [n x n] covariance matrix for error assoc'd with with x(k) + * P(i,j) is the covariance of (ek[i], ek[j]), + * where ex(k) is the difference (x(k) - x_(k)) + * between estimated state x(k) + * (= this->x_) and model state x_(k) + */ + MatrixXd P_; + + /* F, Q matrices driving .x, .P */ + KalmanFilterTransition transition_; + }; /*KalmanFilterState*/ + + inline std::ostream & operator<<(std::ostream & os, + KalmanFilterState const & s) + { + s.display(os); + return os; + } /*operator<<*/ + + /* KalmanFilterStateExt: + * adds additional details from filter step to KalmanFilterState + */ + class KalmanFilterStateExt : public KalmanFilterState { + public: + using TaggedRcptr = reflect::TaggedRcptr; + using MatrixXd = Eigen::MatrixXd; + using int32_t = std::int32_t; + + public: + static ref::rp make(); + static ref::rp make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + ref::rp zk); + + /* create state object for initial filter state */ + static ref::rp initial(utc_nanos t0, + VectorXd x0, + MatrixXd P0); + + /* reflect KalmanFilterStateExt object representation */ + static void reflect_self(); + + int32_t observable() const { return j_; } + MatrixXd const & gain() const { return K_; } + ref::rp const & zk() const { return zk_; } + + virtual void display(std::ostream & os) const override; + + // ----- inherited from SelfTagging ----- + + virtual TaggedRcptr self_tp() override; + + private: + KalmanFilterStateExt() = default; + KalmanFilterStateExt(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + ref::rp zk); + + private: + /* if -1: not used; + * if >= 0: identifies j'th of m observables; + * gain .K applies just to information obtainable from + * observing that scalar variable + */ + int32_t j_ = -1; + /* if .j is -1: + * [n x n] kalman gain + * if .j >= 0: + * [n x 1] kalman gain for observable #j + */ + MatrixXd K_; + /* input leading to state k. + * empty for initial state (i.e. when .k is 0) + */ + ref::rp zk_; + }; /*KalamnFilterStateExt*/ + } /*namespace filter*/ +} /*namespace xo*/ + +/* end KalmanFilterState.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp b/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp new file mode 100644 index 00000000..9d072035 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp @@ -0,0 +1,30 @@ +/* @file KalmanFilterStateToConsole.hpp */ + +#pragma once + +#include "xo/reactor/Sink.hpp" +#include "KalmanFilterState.hpp" + +namespace xo { + namespace kalman { + class KalmanFilterStateToConsole + : public xo::reactor::SinkToConsole + { + public: + KalmanFilterStateToConsole() = default; + + static ref::rp make(); + + virtual void display(std::ostream & os) const; + //virtual std::string display_string() const; + }; /*KalmanFilterStateToConsole*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterStateToConsole const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace option*/ +} /*namespace xo*/ + +/* end KalmanFilterStateToConsole.hpp */ diff --git a/KalmanFilterStep.hpp b/include/xo/kalmanfilter/KalmanFilterStep.hpp similarity index 100% rename from KalmanFilterStep.hpp rename to include/xo/kalmanfilter/KalmanFilterStep.hpp diff --git a/include/xo/kalmanfilter/KalmanFilterSvc.hpp b/include/xo/kalmanfilter/KalmanFilterSvc.hpp new file mode 100644 index 00000000..94702a52 --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterSvc.hpp @@ -0,0 +1,64 @@ +/* @file KalmanFilterSvc.hpp */ + +#include "xo/reactor/Sink.hpp" +#include "xo/reactor/DirectSourcePtr.hpp" +#include "KalmanFilter.hpp" +#include "KalmanFilterInputSource.hpp" +#include "KalmanFilterOutputCallback.hpp" +#include "xo/callback/CallbackSet.hpp" + +namespace xo { + namespace kalman { + /* encapsulate a passive KalmanFilter + * instance as an active event consumer+producer + * + * sinks that want to consume KalmanFilterSvc events will use + * .attach_sink() (or .add_callback()) + */ + class KalmanFilterSvc : public xo::reactor::Sink1>, + public xo::reactor::DirectSourcePtr> { + public: + using AbstractSource = xo::reactor::AbstractSource; + + public: + /* named ctor idiom */ + static ref::rp make(KalmanFilterSpec spec); + + KalmanFilter const & filter() const { return filter_; } + + /* notify incoming observations; will trigger kalman filter step */ + void notify_ev(ref::rp const & input_kp1) override; + + // ----- inherited from reactor::AbstractSink ----- + + /* filter captures KF input pointer */ + virtual bool allow_volatile_source() const override { return false; } + virtual uint32_t n_in_ev() const override { return n_in_ev_; } + virtual void display(std::ostream & os) const override; + + // ----- inherited from reactor::AbstractSource ----- + + /* note: correct since KalmanFilterEngine.extrapolate() + * always creates new state object + */ + virtual bool is_volatile() const override { return false; } + + // ----- Inherited from AbstractEventProcessor ----- + + private: + KalmanFilterSvc(KalmanFilterSpec spec); + + private: + /* passive kalman filter */ + KalmanFilter filter_; + /* receive filter input from this source; see .attach_input() */ + ref::rp input_src_; + /* counts lifetime #of input events (see .notify_ev()) */ + uint32_t n_in_ev_ = 0; + /* publish filter state updates to these callbacks */ + fn::RpCallbackSet pub_; + }; /*KalmanFilterSvc*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* KalmanFilterSvc.hpp */ diff --git a/include/xo/kalmanfilter/KalmanFilterTransition.hpp b/include/xo/kalmanfilter/KalmanFilterTransition.hpp new file mode 100644 index 00000000..22d3de5b --- /dev/null +++ b/include/xo/kalmanfilter/KalmanFilterTransition.hpp @@ -0,0 +1,62 @@ +/* @file KalmanFilterTransition.hpp */ + +#pragma once + +//#include "time/Time.hpp" +#include +#include + +namespace xo { + namespace kalman { + + /* encapsulate transition behavior for a kalman filter + * before taking observations into account + */ + class KalmanFilterTransition { + public: + using MatrixXd = Eigen::MatrixXd; + using uint32_t = std::uint32_t; + + public: + KalmanFilterTransition() = default; + KalmanFilterTransition(MatrixXd F, + MatrixXd Q) + : F_{std::move(F)}, Q_{std::move(Q)} { assert(this->check_ok()); } + + static void reflect_self(); + + /* n: cardinality of state vector */ + uint32_t n_state() const; + + MatrixXd const & transition_mat() const { return F_; } + MatrixXd const & transition_cov() const { return Q_; } + + bool check_ok() const { + uint32_t n = F_.rows(); + bool f_is_nxn = ((F_.rows() == n) && (F_.cols() == n)); + bool q_is_nxn = ((Q_.rows() == n) && (Q_.cols() == n)); + + /* also would like to require: Q is +ve definite */ + + return f_is_nxn && q_is_nxn; + } /*check_ok*/ + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + /* [n x n] state transition matrix */ + MatrixXd F_; + /* [n x n] covariance matrix for system noise */ + MatrixXd Q_; + }; /*KalmanFilterTransition*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterTransition const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterTransition.hpp */ diff --git a/include/xo/kalmanfilter/init_filter.hpp b/include/xo/kalmanfilter/init_filter.hpp new file mode 100644 index 00000000..292a63ff --- /dev/null +++ b/include/xo/kalmanfilter/init_filter.hpp @@ -0,0 +1,20 @@ +/* file init_kalmanfilter.hpp + * + * author: Roland Conybeare, Aug 2022 + */ + +#pragma once + +#include "xo/subsys/Subsystem.hpp" + +namespace xo { + enum S_kalmanfilter_tag {}; + + template<> + struct InitSubsys { + static void init(); + static InitEvidence require(); + }; +} /*namespace xo*/ + +/* end init_kalmanfilter.hpp */ diff --git a/include/xo/kalmanfilter/print_eigen.hpp b/include/xo/kalmanfilter/print_eigen.hpp new file mode 100644 index 00000000..ea6f7375 --- /dev/null +++ b/include/xo/kalmanfilter/print_eigen.hpp @@ -0,0 +1,41 @@ +/* @file print_eigen.hpp */ + +#include +#include + +namespace logutil { + template + class matrix { + public: + matrix(T x) : x_{std::move(x)} {} + + /* print this value */ + T x_; + }; /*matrix*/ + + template + using vector = matrix; + + template + inline std::ostream & + operator<<(std::ostream & s, matrix const & mat) + { + s << "["; + for(std::uint32_t i = 0, m = mat.x_.rows(); i 0) + s << "; "; + + for(std::uint32_t j = 0, n = mat.x_.cols(); j 0) + s << ' '; + + s << mat.x_(i, j); + } + } + s << "]"; + + return s; + } /*operator<<*/ +} /*namespace logutil*/ + +/* end print_eigen.hpp */ diff --git a/init_filter.cpp b/init_filter.cpp deleted file mode 100644 index 80407cc4..00000000 --- a/init_filter.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/* file init_filter.cpp - * - * author: Roland Conybeare, Aug 2022 - */ - -#include "init_filter.hpp" -#include "reactor/init_reactor.hpp" -#include "KalmanFilterState.hpp" -#include "EigenUtil.hpp" -#include "printjson/PrintJson.hpp" - -namespace xo { - using xo::kalman::KalmanFilterInput; - using xo::kalman::KalmanFilterTransition; - using xo::kalman::KalmanFilterState; - using xo::kalman::KalmanFilterStateExt; - using xo::eigen::EigenUtil; - using xo::json::PrintJsonSingleton; - using xo::json::PrintJson; - - void - InitSubsys::init() - { - PrintJson * pjson = PrintJsonSingleton::instance().get(); - - EigenUtil::reflect_eigen(); - EigenUtil::provide_json_printers(pjson); - - KalmanFilterInput::reflect_self(); - KalmanFilterTransition::reflect_self(); - KalmanFilterState::reflect_self(); - KalmanFilterStateExt::reflect_self(); - - } /*init*/ - - InitEvidence - InitSubsys::require() - { - InitEvidence retval; - - /* subsystem dependencies for filter/ */ - retval ^= InitSubsys::require(); - - /* filter/'s own initialization code */ - retval ^= Subsystem::provide("filter", &init); - - return retval; - } /*require*/ -} /*namespace xo*/ - -/* end init_filter.cpp */ diff --git a/init_filter.hpp b/init_filter.hpp deleted file mode 100644 index 79cfc6d0..00000000 --- a/init_filter.hpp +++ /dev/null @@ -1,20 +0,0 @@ -/* file init_filter.hpp - * - * author: Roland Conybeare, Aug 2022 - */ - -#pragma once - -#include "subsys/Subsystem.hpp" - -namespace xo { - enum S_filter_tag {}; - - template<> - struct InitSubsys { - static void init(); - static InitEvidence require(); - }; -} /*namespace xo*/ - -/* end init_filter.hpp */ diff --git a/print_eigen.hpp b/print_eigen.hpp deleted file mode 100644 index 953c5904..00000000 --- a/print_eigen.hpp +++ /dev/null @@ -1,41 +0,0 @@ -/* @file print_eigen.hpp */ - -#include -#include - -namespace logutil { - template - class matrix { - public: - matrix(T x) : x_{std::move(x)} {} - - /* print this value */ - T x_; - }; /*matrix*/ - - template - using vector = matrix; - - template - inline std::ostream & - operator<<(std::ostream & s, matrix const & mat) - { - s << "["; - for(std::uint32_t i = 0, m = mat.x_.rows(); i 0) - s << "; "; - - for(std::uint32_t j = 0, n = mat.x_.cols(); j 0) - s << ' '; - - s << mat.x_(i, j); - } - } - s << "]"; - - return s; - } /*operator<<*/ -} /*namespace logutil*/ - -/* end print_eigen.hpp */ diff --git a/src/kalmanfilter/CMakeLists.txt b/src/kalmanfilter/CMakeLists.txt new file mode 100644 index 00000000..30ca8c02 --- /dev/null +++ b/src/kalmanfilter/CMakeLists.txt @@ -0,0 +1,31 @@ +# xo-kalmanfilter/src/kalmanfilter/CMakeLists.txt + +set(SELF_LIB xo_kalmanfilter) + +set(SELF_SRCS + EigenUtil.cpp + KalmanFilterInput.cpp + KalmanFilterInputToConsole.cpp + KalmanFilterState.cpp + KalmanFilterStateToConsole.cpp + KalmanFilterTransition.cpp + KalmanFilterObservable.cpp + KalmanFilterEngine.cpp + KalmanFilter.cpp + KalmanFilterStep.cpp + KalmanFilterSpec.cpp + KalmanFilterSvc.cpp + init_filter.cpp +) + +xo_add_shared_library4(${SELF_LIB} ${PROJECT_NAME}Targets ${PROJECT_VERSION} 1 ${SELF_SRCS}) + +# ---------------------------------------------------------------- +# internal dependencies + +xo_dependency(${SELF_LIB} reactor) + +# ---------------------------------------------------------------- +# external dependencies + +xo_external_target_dependency(${SELF_LIB} eigen3 Eigen3::Eigen) diff --git a/src/kalmanfilter/EigenUtil.cpp b/src/kalmanfilter/EigenUtil.cpp new file mode 100644 index 00000000..b5770440 --- /dev/null +++ b/src/kalmanfilter/EigenUtil.cpp @@ -0,0 +1,157 @@ +/* file EigenUtil.cpp + * + * author: Roland Conybeare, Sep 2022 + */ + +#include "EigenUtil.hpp" +#include "xo/printjson/PrintJson.hpp" +#include "xo/reflect/Reflect.hpp" +#include +#include +#include +#include + +namespace xo { + using xo::json::PrintJson; + using xo::json::JsonPrinter; + using xo::reflect::Reflect; + using xo::reflect::TypeDescr; + using VectorXb = Eigen::Array; + using Eigen::VectorXd; + using Eigen::MatrixXd; + +#ifdef NOT_YET + namespace reflect { + template + using EigenVectorX_Tdx = xo::reflect::StlVectorTdx>; + + /* probably need this to appear before decl for class xo::reflect::Reflect */ + template + class EstablishTdx> { + public: + static std::unique_ptr make() { + return EigenVectorX_Tdx::make(); + } /*make*/ + }; /*EstablishTdx*/ + } /*reflect*/ +#endif + + namespace eigen { + + namespace { + /* prints a VectorXd as json, in the obvious format, e.g. + * [1,2,3] + */ + template + class EigenVectorJsonPrinter : public JsonPrinter { + public: + EigenVectorJsonPrinter(PrintJson const * pjson) : JsonPrinter(pjson) {} + + virtual void print_json(TaggedPtr tp, + std::ostream * p_os) const override + { + EigenVectorType * pv = this->check_recover_native(tp, p_os); + + if (pv) { + /* EigenVectorType (VectorXb, VectorXd, ..) + * is reflected as atomic for now, out of expedience. + * + * as soon as we reflect as mt_vector, will not need this helper. + */ + *p_os << "["; + + for (std::uint32_t i = 0, n = pv->size(); i < n; ++i) { + if (i > 0) + *p_os << ","; + + /* note: need to dispatch via json printer for vector elements, + * to get special treatment for non-finite values + */ + this->pjson()->print((*pv)[i], p_os); + //*p_os << jsonp((*pv)[i], this->pjson()); + } + + *p_os << "]"; + } + } /*print_json*/ + }; /*EigenVectorJsonPrinter*/ + + /* prints a MatrixXd as json, in row-major format, e.g. + * [[1,2,3], [4,5,6], [7,8,9]] + */ + class MatrixXdJsonPrinter : public JsonPrinter { + public: + MatrixXdJsonPrinter(PrintJson const * pjson) : JsonPrinter(pjson) {} + + virtual void print_json(TaggedPtr tp, + std::ostream * p_os) const override + { + MatrixXd * pm = this->check_recover_native(tp, p_os); + + if (pm) { + /* MatrixXd is reflected as atomic for now, out of expedience */ + *p_os << "["; + + for(std::uint32_t i=0, m=pm->rows(); i 0) + *p_os << ", "; + *p_os << "["; + for(std::uint32_t j=0, n=pm->cols(); j 0) + *p_os << ","; + + /* note: need to dispatch via json printer for matrix elements, + * to get special treatment for non-finite values + */ + this->pjson()->print((*pm)(i, j), p_os); + //*p_os << jsonp((*pm)(i, j), this->pjson()); + } + *p_os << "]"; + } + + *p_os << "]"; + } + } /*print_json*/ + }; /*MatrixXdJsonPrinter*/ + + template + void + provide_eigen_vector_printer(PrintJson * p_pjson) + { + TypeDescr td = Reflect::require(); + std::unique_ptr pr(new EigenVectorJsonPrinter(p_pjson)); + + p_pjson->provide_printer(td, std::move(pr)); + } /*provide_eigen_vector_printer*/ + } /*namespace*/ + + void + EigenUtil::reflect_eigen() + { +#ifdef NOT_YET + Reflect::require(); + Reflect::require(); +#endif + } /*reflect_eigen*/ + + void + EigenUtil::provide_json_printers(PrintJson * p_pjson) + { + assert(p_pjson); + + provide_eigen_vector_printer(p_pjson); + provide_eigen_vector_printer(p_pjson); + + { + TypeDescr td = Reflect::require(); + std::unique_ptr pr(new MatrixXdJsonPrinter(p_pjson)); + + p_pjson->provide_printer(td, std::move(pr)); + } + } /*provide_json_printers*/ + } /*namespace eigen*/ +} /*namespace xo*/ + +/* end EigenUtil.cpp */ diff --git a/src/kalmanfilter/KalmanFilter.cpp b/src/kalmanfilter/KalmanFilter.cpp new file mode 100644 index 00000000..56507650 --- /dev/null +++ b/src/kalmanfilter/KalmanFilter.cpp @@ -0,0 +1,78 @@ +/* @file KalmanFilter.cpp */ + +#include "KalmanFilter.hpp" +#include "KalmanFilterEngine.hpp" +#include "print_eigen.hpp" +#include "xo/indentlog/scope.hpp" +#include "Eigen/src/Core/Matrix.h" + +namespace xo { + using xo::time::utc_nanos; + //using logutil::matrix; + using xo::scope; + using xo::tostr; + using xo::xtag; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + // ----- KalmanFilter ----- + + KalmanFilter::KalmanFilter(KalmanFilterSpec spec) + : filter_spec_{std::move(spec)}, + state_ext_{filter_spec_.start_ext()} + {} /*ctor*/ + + void + KalmanFilter::notify_input(ref::rp const & input_kp1) + { + scope log(XO_ENTER0(info)); + + /* on entry: + * .state_ext refers to t(k) + * on exit: + * .step refers to t(k+1) + * .state_ext refers to t(k+1) + */ + + log && log(xtag("step_dt", + input_kp1->tkp1() - this->state_ext_->tm())); + + /* establish step inputs for this filter step: + * F(k+1) (system transition matrix) + * Q(k+1) (system noise covariance matrix) + * H(k+1) (observation coupling matrix) + * R(k+1) (observation noise covariance matrix) + * z(k+1) (observation vector) + */ + this->step_ = this->filter_spec_.make_step(this->state_ext_, input_kp1); + + //if (lscope.enabled()) { lscope.log(xtag("step", this->step_)); } + + /* extrapolate filter state to t(k+1), + * and correct based on z(k+1) + */ + this->state_ext_ = KalmanFilterEngine::step(this->step_); + + //if (lscope.enabled()) { lscope.log(xtag("state_ext", this->state_ext_)); } + } /*notify_input*/ + + void + KalmanFilter::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilter::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilter.cpp */ diff --git a/src/kalmanfilter/KalmanFilterEngine.cpp b/src/kalmanfilter/KalmanFilterEngine.cpp new file mode 100644 index 00000000..e9533549 --- /dev/null +++ b/src/kalmanfilter/KalmanFilterEngine.cpp @@ -0,0 +1,360 @@ +/* @file KalmanFilterEngine.cpp + * + */ + +#include "KalmanFilterEngine.hpp" +#include "print_eigen.hpp" +#include "xo/indentlog/scope.hpp" +#include "Eigen/src/Core/Matrix.h" + +namespace xo { + using xo::time::utc_nanos; + using logutil::matrix; + using xo::scope; + using xo::xtag; + using Eigen::LDLT; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + // ----- KalmanFilterEngine ----- + + ref::rp + KalmanFilterEngine::extrapolate(utc_nanos tkp1, + ref::rp const & s, + KalmanFilterTransition const & f) + { + //constexpr char const * c_self_name + // = "KalmanFilterEngine::extrapolate"; + + /* prior estimates at t(k) */ + VectorXd const & x = s->state_v(); + MatrixXd const & P = s->state_cov(); + + /* model change from t(k) -> t(k+1) */ + MatrixXd const & F = f.transition_mat(); + MatrixXd const & Q = f.transition_cov(); + + if(F.cols() != x.rows()) { + scope log(XO_DEBUG(true /*debug_flag*/)); + + log("error: F*x: expected F.cols=x.rows", + xtag("F.cols", F.cols()), xtag("x.rows", x.rows())); + } + + /* x(k+1|k) */ + VectorXd x_ext = F * x; + + /* P(k+1|k) */ + MatrixXd P_ext = (F * P * F.transpose()) + Q; + + /* creating new state object here + * allows KalmanFilterSvc.is_volatile()=false + */ + + return KalmanFilterState::make(s->step_no() + 1, + tkp1, + std::move(x_ext), + std::move(P_ext), + f); + } /*extrapolate*/ + + VectorXd + KalmanFilterEngine::kalman_gain1(ref::rp const & skp1_ext, + KalmanFilterObservable const & h, + uint32_t j) + { + constexpr bool c_debug_enabled = false; + + scope log(XO_DEBUG(c_debug_enabled)); + + /* P(k+1|k) :: [n x n] */ + MatrixXd const & P_ext = skp1_ext->state_cov(); + + /* H(k) :: [m x n] */ + MatrixXd const & H = h.observable(); + /* R(k) :: [m x m] */ + MatrixXd const & R = h.observable_cov(); + + /* i'th col of H couples element #i of filter state to each member of input z(k); + * j'th row of H couples filter state to j'th observable + * + * Hj :: [1 x n] Hj is a row-vector + */ + auto Hj = H.row(j); + + /* Rjj is the j'th diagonal element of R */ + double Rjj = R(j, j); + + /* T + * M(k) = Hj * P(k+1|k) * Hj + Rjj + * + * M(k) is a [1 x 1] matrix + */ + double m = Hj * (P_ext * Hj.transpose()) + Rjj; + + /* -1 + * M(k) trivial, since M is [1 x 1] + */ + double m_inv = 1.0 / m; + + /* K :: [n x 1] */ + VectorXd K = P_ext * Hj.transpose() * m_inv; + + log && log("result", + xtag("P(k+1|k)", matrix(P_ext)), + xtag("R", matrix(R)), + xtag("m", m)); + + return K; + } /*kalman_gain1*/ + + MatrixXd + KalmanFilterEngine::kalman_gain(ref::rp const & skp1_ext, + KalmanFilterObservable const & h) + { + scope log(XO_DEBUG(false /*debug_enabled*/)); + + /* P(k+1|k) */ + MatrixXd const & P_ext = skp1_ext->state_cov(); + + MatrixXd const & H = h.observable(); + MatrixXd const & R = h.observable_cov(); + + uint32_t m = H.rows(); + uint32_t n = H.cols(); + + if ((P_ext.rows() != n) || (P_ext.cols() != n)) { + std::string err_msg + = tostr("kalman_gain: with dim(H) = [m x n] expect dim(P) = [n x n]", + xtag("m", m), xtag("n", n), + xtag("P.rows", P_ext.rows()), + xtag("P.cols", P_ext.cols())); + + throw std::runtime_error(err_msg); + } + + if ((R.rows() != m) || (R.cols() != m)) { + std::string err_msg + = tostr("kalman_gain: with dim(H) = [m x n] expect dim(R) = [m x m]", + xtag("m", m), xtag("n", n), + xtag("R.rows", R.rows()), xtag("R.cols", R.cols())); + + throw std::runtime_error(err_msg); + } + + /* kalman gain: + * T -1 + * K(k+1) = P(k+1|k).H(k) .M + * + * T / T \ -1 + * = P(k+1|k).H(k) .| H(k).P(k+1|k).H(k) + R(k) | + * \ / + * + * Notes: + * 1. the matrix M being inverted is symmetric, since represents covariances. + * 2. if diagonal of R(k) has no zeroes (i.e. all measurements are subject to error), + * then it must be non-negative definite + * 3. unless observation errors are perfectly correlated, M(k) + * is positive definite. + * 4. even though 3. holds, there may be a nearby non-positive-definite matrix M+dM, + * Factoring M with finite-precision arithmetic solves for M+dM instead of M; + * which may run into difficulty if M is only 'slighlty' +ve definite. + * If necessary add small diagonal correction D to M, + * sufficient to make M+D positive definite. + * This is equivalent to introducing additional + * uncorrelated observation error, so benign from a robustness perspective + * 5. In generally we usually want to avoid fully realizing a matrix inverse. + * In this case need to explicitly compute K as ingredient used to + * correct state covariance later. + * 6. However, if R is diagonal (which is in practice quite likely), + * then it's easy to decompose a suite of vector observations z(k+1) = [z1, ..zm]T + * into separate zi, with dt=0 separating them. + * Can use this to avoid computing the inverse. + * See .kalman_gain1(), .correct1() + * 7. .kalman_gain() works unaltered when H, R have been reindexed + * to exclude outliers/errors; this is true because .kalman_gain() does not + * use the observation vector z[], i.e. operates entirely in the reduced + * reindexed space. + */ + + MatrixXd M = H * P_ext * H.transpose() + R; + + /* will use to write M as: + * + * T T + * M = P .L.D.L .P + * + * where: + * P is a permutation matrix + * L is lower triangular, with unit diagonal + * D is diagonal + */ + LDLT ldlt = M.ldlt(); + + /* solve for the identity matrix to realize the inverse this way */ + MatrixXd I = MatrixXd::Identity(M.rows(), M.cols()); + + /* -1 + * M + */ + MatrixXd M_inv = ldlt.solve(I); + + /* K(k+1) */ + MatrixXd K = P_ext * H.transpose() * M_inv; + + log && log("result", + xtag("k", skp1_ext->step_no()), + xtag("P(k+1|k)", matrix(P_ext)), + xtag("H", matrix(H)), + xtag("R", matrix(R)), + xtag("M", matrix(M)), + xtag("K", matrix(K))); + + return K; + } /*kalman_gain*/ + + ref::rp + KalmanFilterEngine::correct1(ref::rp const & skp1_ext, + KalmanFilterObservable const & h, + ref::rp const & zkp1, + uint32_t j) + { + uint32_t n = skp1_ext->n_state(); + /* Kj :: [n x 1] */ + VectorXd Kj = kalman_gain1(skp1_ext, h, j); + /* H :: [m x n] */ + MatrixXd const & H = h.observable(); + VectorXd const & z = zkp1->z(); + + /* Hj :: [1 x n] the j'th row of H */ + auto const & Hj = H.row(j); + + + /* x(k+1|x) :: [n x 1] */ + VectorXd const & x_ext = skp1_ext->state_v(); + + /* P(k+1|k) :: [n x n] */ + MatrixXd const & P_ext = skp1_ext->state_cov(); + + /* innovj : difference between jth 'actual observation' + * and jth 'predicted observation' + */ + double innovj = z[j] - (Hj * x_ext); + + /* x(k+1) */ + VectorXd xkp1 = x_ext + (Kj * innovj); + + MatrixXd I = MatrixXd::Identity(n, n); + /* note: Kj [n x 1], Hj [1 x n], + * so Kj * Hj [n x n], with rank 1 + */ + MatrixXd Pkp1 = (I - (Kj * Hj)) * P_ext; + + return KalmanFilterStateExt::make(skp1_ext->step_no(), + skp1_ext->tm(), + xkp1, + Pkp1, + skp1_ext->transition(), + Kj, + j, + zkp1); + } /*correct1*/ + + ref::rp + KalmanFilterEngine::correct(ref::rp const & skp1_ext, + KalmanFilterObservable const & h, + ref::rp const & zkp1) + { + uint32_t n = skp1_ext->n_state(); + /* K :: [n x m] */ + MatrixXd K = kalman_gain(skp1_ext, h); + MatrixXd const & H = h.observable(); + /* z_orig[] is original observation vector before reindexing */ + VectorXd const & z_orig = zkp1->z(); + /* reindex z_orig, keeping only elements that appear in + */ + VectorXd z = z_orig(h.keep()); + + /* 'ext' short for 'extrapolated' */ + VectorXd const & x_ext = skp1_ext->state_v(); + MatrixXd const & P_ext = skp1_ext->state_cov(); + + /* innov: difference between 'actual observations' + * and 'predicted observations' + */ + VectorXd innov = z - (H * x_ext); + + /* x(k+1) :: [n x 1] */ + VectorXd xkp1 = x_ext + K * innov; + MatrixXd I = MatrixXd::Identity(n, n); + MatrixXd Pkp1 = (I - K * H) * P_ext; + + return KalmanFilterStateExt::make(skp1_ext->step_no(), + skp1_ext->tm(), + xkp1, + Pkp1, + skp1_ext->transition(), + K, + -1 /*j: not used*/, + zkp1); + } /*correct*/ + + ref::rp + KalmanFilterEngine::step(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1) + { + ref::rp skp1_ext + = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); + + ref::rp skp1 + = KalmanFilterEngine::correct(skp1_ext, Hkp1, zkp1); + + return skp1; + } /*step*/ + + ref::rp + KalmanFilterEngine::step(KalmanFilterStep const & step_spec) + { + return step(step_spec.tkp1(), + step_spec.state(), + step_spec.model(), + step_spec.obs(), + step_spec.input()); + } /*step*/ + + ref::rp + KalmanFilterEngine::step1(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1, + uint32_t j) + { + ref::rp skp1_ext + = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); + + ref::rp skp1 + = KalmanFilterEngine::correct1(skp1_ext, Hkp1, zkp1, j); + + return skp1; + } /*step1*/ + + ref::rp + KalmanFilterEngine::step1(KalmanFilterStep const & step_spec, + uint32_t j) + { + return step1(step_spec.tkp1(), + step_spec.state(), + step_spec.model(), + step_spec.obs(), + step_spec.input(), + j); + } /*step1*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterEngine.cpp */ diff --git a/src/kalmanfilter/KalmanFilterInput.cpp b/src/kalmanfilter/KalmanFilterInput.cpp new file mode 100644 index 00000000..96f0d6ca --- /dev/null +++ b/src/kalmanfilter/KalmanFilterInput.cpp @@ -0,0 +1,118 @@ +/* @file KalmanFilterInput.cpp */ + +#include "KalmanFilterInput.hpp" +#include "xo/reflect/StructReflector.hpp" +#include "Eigen/src/Core/Matrix.h" +#include "print_eigen.hpp" +#include "xo/indentlog/scope.hpp" +#include "xo/reflect/TaggedRcptr.hpp" + +namespace xo { + using xo::reflect::Reflect; + using xo::reflect::TaggedRcptr; + using xo::reflect::StructReflector; + using xo::scope; + using logutil::matrix; + using xo::xtag; + using Eigen::MatrixXd; + using Eigen::VectorXi; + using std::uint32_t; + + namespace kalman { + ref::rp + KalmanFilterInput::make(utc_nanos tkp1, + VectorXb const & presence, + VectorXd const & z, + VectorXd const & Rd) + { + return new KalmanFilterInput(tkp1, presence, z, Rd); + } /*make*/ + + ref::rp + KalmanFilterInput::make_present(utc_nanos tkp1, + VectorXd const & z) + { + VectorXb presence = VectorXb::Ones(z.cols()); + + return new KalmanFilterInput(tkp1, + presence, + z, + VectorXd(0) /*Rd - not using*/); + } /*make*/ + + VectorXi + KalmanFilterInput::make_kept_index() const + { + scope log(XO_DEBUG(false /*!debug_flag*/)); + + log && log(xtag("presence", matrix(presence_))); + + /* count truth values */ + uint32_t mstar = 0; + + for (uint32_t j = 0, m = this->presence_.rows(); jpresence_[j]) + ++mstar; + } + + log && log(xtag("m*", mstar)); + + VectorXi keep(mstar); + + /* 2nd pass, populate keep[] */ + + uint32_t jstar = 0; + + for (uint32_t j = 0, m = this->presence_.rows(); jpresence_[j]) { + keep[jstar] = j; + ++jstar; + } + } + + log && log("keep", matrix(keep)); + + return keep; + } /*make_kept_index*/ + + void + KalmanFilterInput::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + REFLECT_MEMBER(sr, tkp1); + REFLECT_MEMBER(sr, presence); + REFLECT_MEMBER(sr, z); + REFLECT_MEMBER(sr, Rd); + } + } /*reflect_self*/ + + reflect::TaggedRcptr + KalmanFilterInput::self_tp() + { + return Reflect::make_rctp(this); + } /*self_tp*/ + + void + KalmanFilterInput::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterInput::display_string() const + { + std::stringstream ss; + this->display(ss); + return ss.str(); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInput.cpp */ diff --git a/KalmanFilterInputToConsole.cpp b/src/kalmanfilter/KalmanFilterInputToConsole.cpp similarity index 93% rename from KalmanFilterInputToConsole.cpp rename to src/kalmanfilter/KalmanFilterInputToConsole.cpp index a0a848ca..8fadf031 100644 --- a/KalmanFilterInputToConsole.cpp +++ b/src/kalmanfilter/KalmanFilterInputToConsole.cpp @@ -1,7 +1,7 @@ /* @file KalmanFilterInputToConsole.cpp */ #include "KalmanFilterInputToConsole.hpp" -#include "indentlog/print/tag.hpp" +#include "xo/indentlog/print/tag.hpp" namespace xo { using xo::xtag; diff --git a/KalmanFilterObservable.cpp b/src/kalmanfilter/KalmanFilterObservable.cpp similarity index 98% rename from KalmanFilterObservable.cpp rename to src/kalmanfilter/KalmanFilterObservable.cpp index 90f31460..ad90c1f1 100644 --- a/KalmanFilterObservable.cpp +++ b/src/kalmanfilter/KalmanFilterObservable.cpp @@ -2,7 +2,7 @@ #include "KalmanFilterObservable.hpp" #include "print_eigen.hpp" -#include "indentlog/scope.hpp" +#include "xo/indentlog/scope.hpp" namespace xo { using xo::scope; diff --git a/src/kalmanfilter/KalmanFilterSpec.cpp b/src/kalmanfilter/KalmanFilterSpec.cpp new file mode 100644 index 00000000..45185551 --- /dev/null +++ b/src/kalmanfilter/KalmanFilterSpec.cpp @@ -0,0 +1,27 @@ +/* @file KalmanFilterSpec.cpp */ + +#include "KalmanFilterSpec.hpp" +#include "xo/indentlog/scope.hpp" + +namespace xo { + using xo::tostr; + using xo::xtag; + + namespace kalman { + void + KalmanFilterSpec::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterSpec::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterSpec.cpp */ diff --git a/src/kalmanfilter/KalmanFilterState.cpp b/src/kalmanfilter/KalmanFilterState.cpp new file mode 100644 index 00000000..4b5679e3 --- /dev/null +++ b/src/kalmanfilter/KalmanFilterState.cpp @@ -0,0 +1,231 @@ +/* @file KalmanFilterState.cpp */ + +#include "KalmanFilterState.hpp" +#include "print_eigen.hpp" +#include "xo/reflect/StructReflector.hpp" +#include "xo/reflect/TaggedPtr.hpp" +#include "xo/indentlog/scope.hpp" +#include "Eigen/src/Core/Matrix.h" +#include +#include + +namespace xo { + using xo::reflect::Reflect; + using xo::reflect::TaggedRcptr; + using xo::reflect::StructReflector; + using xo::time::utc_nanos; + using xo::ref::rp; + using logutil::matrix; + using logutil::vector; + //using xo::scope; + using xo::xtag; + using xo::tostr; + //using Eigen::LDLT; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + // ----- KalmanFilterState ----- + + rp + KalmanFilterState::make() + { + return new KalmanFilterState(); + } /*make*/ + + rp + KalmanFilterState::make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition) + { + return new KalmanFilterState(k, tk, + std::move(x), + std::move(P), + std::move(transition)); + } /*make*/ + + void + KalmanFilterState::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + REFLECT_MEMBER(sr, k); + REFLECT_MEMBER(sr, tk); + REFLECT_MEMBER(sr, x); + REFLECT_MEMBER(sr, P); + } + } /*reflect_self*/ + + KalmanFilterState::KalmanFilterState() = default; + + KalmanFilterState::KalmanFilterState(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition) + : k_{k}, tk_{tk}, + x_{std::move(x)}, P_{std::move(P)}, + transition_{std::move(transition)} + {} + + TaggedRcptr + KalmanFilterState::self_tp() + { + return Reflect::make_rctp(this); + } /*self_tp*/ + + // ----- KalmanFilterExt ----- + + rp + KalmanFilterStateExt::make() + { + return new KalmanFilterStateExt(); + } /*make*/ + + rp + KalmanFilterStateExt::make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + rp zk) + { + return new KalmanFilterStateExt(k, + tk, + std::move(x), + std::move(P), + std::move(transition), + std::move(K), + j, + std::move(zk)); + } /*make*/ + + void + KalmanFilterStateExt::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + /* TODO: use sr.adopt_ancestors() */ + + REFLECT_EXPLICIT_MEMBER(sr, "k", &KalmanFilterState::k_); + REFLECT_EXPLICIT_MEMBER(sr, "tk", &KalmanFilterState::tk_); + REFLECT_EXPLICIT_MEMBER(sr, "x", &KalmanFilterState::x_); + REFLECT_EXPLICIT_MEMBER(sr, "P", &KalmanFilterState::P_); + REFLECT_EXPLICIT_MEMBER(sr, "transition", &KalmanFilterState::transition_); + REFLECT_MEMBER(sr, j); + REFLECT_MEMBER(sr, K); + REFLECT_MEMBER(sr, zk); + } + } /*reflect_self*/ + + KalmanFilterStateExt::KalmanFilterStateExt(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + rp zk) + : KalmanFilterState(k, tk, + std::move(x), + std::move(P), + std::move(transition)), + j_{j}, + K_{std::move(K)}, + zk_{std::move(zk)} + { + uint32_t n = x.size(); + + if (n != P.rows() || n != P.cols()) { + std::string err_msg + = tostr("with n=x.size expect [n x n] covar matrix P", + xtag("n", x.size()), + xtag("P.rows", P.rows()), + xtag("P.cols", P.cols())); + + throw std::runtime_error(err_msg); + } + + if ((K.rows() > 0) && (K.rows() > 0)) { + if (n != K.rows()) { + std::string err_msg + = tostr("with n=x.size expect [m x n] gain matrix K", + xtag("n", x.size()), + xtag("K.rows", K.rows()), + xtag("K.cols", K.cols())); + + throw std::runtime_error(err_msg); + } + } else { + /* bypass test with [0 x 0] matrix K; + * normal for initial filter state + */ + } + } /*ctor*/ + + void + KalmanFilterState::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterState::display_string() const + { + std::stringstream ss; + ss << *this; + return ss.str(); + } /*display_string*/ + + // ----- KalmanFilterStateExt ----- + + ref::rp + KalmanFilterStateExt::initial(utc_nanos t0, + VectorXd x0, + MatrixXd P0) + { + return KalmanFilterStateExt::make + (0 /*k*/, + t0, + std::move(x0), + std::move(P0), + KalmanFilterTransition(MatrixXd() /*F - not used for initial step*/, + MatrixXd() /*Q - not used for initial step*/), + MatrixXd() /*K - not used for initial step*/, + -1 /*j - not used for initial step*/, + nullptr /*zk - not defined for initial step*/); + } /*initial*/ + + void + KalmanFilterStateExt::display(std::ostream & os) const + { + os << "step_no()) + << xtag("tk", this->tm()) + << xtag("x", matrix(this->state_v())) + << xtag("P", matrix(this->state_cov())) + << xtag("K", matrix(K_)) + << xtag("j", j_) + << ">"; + } /*display*/ + + TaggedRcptr + KalmanFilterStateExt::self_tp() + { + return Reflect::make_rctp(this); + } /*self_tp*/ + } /*namespace filter*/ +} /*namespace xo*/ + +/* end KalmanFilterState.cpp */ diff --git a/KalmanFilterStateToConsole.cpp b/src/kalmanfilter/KalmanFilterStateToConsole.cpp similarity index 93% rename from KalmanFilterStateToConsole.cpp rename to src/kalmanfilter/KalmanFilterStateToConsole.cpp index 8e76ce84..9559d339 100644 --- a/KalmanFilterStateToConsole.cpp +++ b/src/kalmanfilter/KalmanFilterStateToConsole.cpp @@ -1,7 +1,7 @@ /* @file KalmanFilterStateToConsole.cpp */ #include "KalmanFilterStateToConsole.hpp" -#include "indentlog/print/tag.hpp" +#include "xo/indentlog/print/tag.hpp" namespace xo { using xo::xtag; diff --git a/src/kalmanfilter/KalmanFilterStep.cpp b/src/kalmanfilter/KalmanFilterStep.cpp new file mode 100644 index 00000000..8ca90211 --- /dev/null +++ b/src/kalmanfilter/KalmanFilterStep.cpp @@ -0,0 +1,84 @@ +/* @file KalmanFilterStep.cpp */ + +#include "KalmanFilterStep.hpp" +#include "KalmanFilterEngine.hpp" +#include "KalmanFilterState.hpp" +#include "xo/indentlog/scope.hpp" + +namespace xo { + using xo::scope; + using xo::tostr; + using xo::xtag; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + ref::rp + KalmanFilterStep::extrapolate() const + { + return KalmanFilterEngine::extrapolate(this->tkp1(), + this->state(), + this->model() /*transition*/); + } /*extrapolate*/ + + MatrixXd + KalmanFilterStep::gain(ref::rp const & skp1_ext) const + { + return KalmanFilterEngine::kalman_gain(skp1_ext, + this->obs()); + } /*gain*/ + + VectorXd + KalmanFilterStep::gain1(ref::rp const & skp1_ext, + uint32_t j) const + { + return KalmanFilterEngine::kalman_gain1(skp1_ext, + this->obs(), + j); + + } /*gain1*/ + + ref::rp + KalmanFilterStep::correct(ref::rp const & skp1_ext) + { + return KalmanFilterEngine::correct(skp1_ext, + this->obs(), + this->input()); + } /*correct*/ + + ref::rp + KalmanFilterStep::correct1(ref::rp const & skp1_ext, + uint32_t j) + { + return KalmanFilterEngine::correct1(skp1_ext, + this->obs(), + this->input(), + j); + } /*correct1*/ + + void + KalmanFilterStep::display(std::ostream & os) const + { + //scope lscope("KalmanFilterStep::display"); + + os << "model()); + //lscope.log("obs:"); + os << xtag("obs", this->obs()); + //lscope.log("input:"); + os << xtag("input", this->input()); + os << ">"; + } /*display*/ + + std::string + KalmanFilterStep::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterStep.cpp */ diff --git a/KalmanFilterSvc.cpp b/src/kalmanfilter/KalmanFilterSvc.cpp similarity index 100% rename from KalmanFilterSvc.cpp rename to src/kalmanfilter/KalmanFilterSvc.cpp diff --git a/src/kalmanfilter/KalmanFilterTransition.cpp b/src/kalmanfilter/KalmanFilterTransition.cpp new file mode 100644 index 00000000..a6ec63ae --- /dev/null +++ b/src/kalmanfilter/KalmanFilterTransition.cpp @@ -0,0 +1,55 @@ +/* @file KalmanFilterTransition.cpp */ + +#include "KalmanFilterTransition.hpp" +#include "print_eigen.hpp" +#include "xo/reflect/StructReflector.hpp" +#include "xo/indentlog/scope.hpp" + +namespace xo { + using xo::reflect::StructReflector; + using logutil::matrix; + using xo::xtag; + + namespace kalman { + void + KalmanFilterTransition::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + REFLECT_MEMBER(sr, F); + REFLECT_MEMBER(sr, Q); + } + } /*reflect_self*/ + + uint32_t + KalmanFilterTransition::n_state() const + { + /* we know F.rows() == F.cols() = Q.cols() == Q.rows(), + * see .check_ok() + */ + + return F_.rows(); + } /*n_state*/ + + void + KalmanFilterTransition::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterTransition::display_string() const + { + std::stringstream ss; + this->display(ss); + return ss.str(); + } /*display_string*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterTransition.cpp */ diff --git a/src/kalmanfilter/init_filter.cpp b/src/kalmanfilter/init_filter.cpp new file mode 100644 index 00000000..3417214b --- /dev/null +++ b/src/kalmanfilter/init_filter.cpp @@ -0,0 +1,52 @@ +/* file init_filter.cpp + * + * author: Roland Conybeare, Aug 2022 + */ + +#include "init_filter.hpp" +#include "xo/reactor/init_reactor.hpp" + +#include "KalmanFilterState.hpp" +#include "EigenUtil.hpp" +#include "xo/printjson/PrintJson.hpp" + +namespace xo { + using xo::kalman::KalmanFilterInput; + using xo::kalman::KalmanFilterTransition; + using xo::kalman::KalmanFilterState; + using xo::kalman::KalmanFilterStateExt; + using xo::eigen::EigenUtil; + using xo::json::PrintJsonSingleton; + using xo::json::PrintJson; + + void + InitSubsys::init() + { + PrintJson * pjson = PrintJsonSingleton::instance().get(); + + EigenUtil::reflect_eigen(); + EigenUtil::provide_json_printers(pjson); + + KalmanFilterInput::reflect_self(); + KalmanFilterTransition::reflect_self(); + KalmanFilterState::reflect_self(); + KalmanFilterStateExt::reflect_self(); + + } /*init*/ + + InitEvidence + InitSubsys::require() + { + InitEvidence retval; + + /* subsystem dependencies for filter/ */ + retval ^= InitSubsys::require(); + + /* filter/'s own initialization code */ + retval ^= Subsystem::provide("kalmanfilter", &init); + + return retval; + } /*require*/ +} /*namespace xo*/ + +/* end init_filter.cpp */ diff --git a/utest/CMakeLists.txt b/utest/CMakeLists.txt new file mode 100644 index 00000000..9f21bfa4 --- /dev/null +++ b/utest/CMakeLists.txt @@ -0,0 +1,54 @@ +# xo-kalmanfilter/utest/CMakeLists.txt + +set(SELF_EXE utest.filter) + +set(SELF_SRCS + KalmanFilter.test.cpp + filter_utest_main.cpp) + +add_executable(${SELF_EXE} ${SELF_SRCS}) +xo_include_options2(${SELF_EXE}) + +add_test(NAME ${SELF_EXE} COMMAND ${SELF_EXE}) +target_code_coverage(${SELF_EXE} AUTO ALL) + +# ---------------------------------------------------------------- +# create convenience symlink to canned data + +#create_symlink("${CMAKE_SOURCE_DIR}/utestdata/" "src/filter/utest/utestdata") + +# ---------------------------------------------------------------- +# generic project dependency + +# PROJECT_SOURCE_DIR: +# so we can for example write +# #include "logutil/scope.hpp" +# from anywhere in the project +# PROJECT_BINARY_DIR: +# since version file will be in build directory, need that directory +# to also be included in compiler's include path +# +xo_self_dependency(${SELF_EXE} xo_kalmanfilter) + +# ---------------------------------------------------------------- +# internal dependencies: logutil, ... + +xo_dependency(${SELF_EXE} xo_statistics) + +# ---------------------------------------------------------------- +# external dependencies: catch2.. + +xo_external_target_dependency(${SELF_EXE} Catch2 Catch2::Catch2) + +# ---------------------------------------------------------------- +# make standard directories for std:: includes explicit +# so that +# (1) they appear in compile_commands.json. +# (2) clangd (run from emacs lsp-mode) can find them +# +if(CMAKE_EXPORT_COMPILE_COMMANDS) + set(CMAKE_CXX_STANDARD_INCLUDE_DIRECTORIES + ${CMAKE_CXX_IMPLICIT_INCLUDE_DIRECTORIES}) +endif() + +# end CMakeLists.txt diff --git a/utest/KalmanFilter.test.cpp b/utest/KalmanFilter.test.cpp new file mode 100644 index 00000000..679f461f --- /dev/null +++ b/utest/KalmanFilter.test.cpp @@ -0,0 +1,690 @@ +/* @file KalmanFilter.test.cpp */ + +#include "xo/kalmanfilter/KalmanFilter.hpp" +#include "xo/kalmanfilter/KalmanFilterEngine.hpp" +#include "xo/kalmanfilter/print_eigen.hpp" +#include "statistics/SampleStatistics.hpp" +#include "xo/randomgen/normalgen.hpp" +#include "xo/randomgen/xoshiro256.hpp" +#include "xo/indentlog/scope.hpp" +#include "xo/indentlog/log_level.hpp" +#include +#include + +namespace xo { + using xo::kalman::KalmanFilterSpec; + using xo::kalman::KalmanFilterStep; + using xo::kalman::KalmanFilterEngine; + using xo::kalman::KalmanFilterStateExt; + using xo::kalman::KalmanFilterState; + using xo::kalman::KalmanFilterTransition; + using xo::kalman::KalmanFilterObservable; + using xo::kalman::KalmanFilterInput; + using xo::statistics::SampleStatistics; + using xo::rng::normalgen; + using xo::rng::xoshiro256ss; + using xo::time::timeutil; + using xo::time::utc_nanos; + using xo::time::seconds; + using xo::ref::rp; + using xo::log_level; + using logutil::matrix; + //using logutil::scope; + //using logutil::tostr; + //using logutil::xtag; + using xo::print::ccs; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace ut { + namespace { + /* step for kalman filter with: + * - single state variable x[0] + * - identity process model - x(k+1) = F(k).x(k), with F(k) = | 1 | + * - no process noise + * - single observation z[0] + * - identity coupling matrix - z(k) = H(k).x(k) + w(k), with H(k) = | 1 | + */ + KalmanFilterSpec::MkStepFn + kalman_identity1_mkstep_fn() + { + /* kalman state transition matrix: use identity <--> state is constant */ + MatrixXd F = MatrixXd::Identity(1, 1); + + /* state transition noise: set this to zero; + * measuring something that's known to be constant + */ + MatrixXd Q = MatrixXd::Zero(1, 1); + + /* single direct observation */ + MatrixXd H = MatrixXd::Identity(1, 1); + + /* observation errors understood to have + * mean 0, sdev 1 + * + * This is consistent with normal_rng below, + * so R is correctly specified + */ + MatrixXd R = MatrixXd::Identity(1, 1); + + return [F, Q, H, R](rp const & sk, + rp const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk = KalmanFilterObservable::keep_all(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_identity1_mkstep_fn*/ + } /*namespace*/ + + /* example 1. + * repeated direct observation of a scalar + * use rng to generate observations + */ + TEST_CASE("kalman-identity", "[kalmanfilter]") { + /* setting up trivial filter for repeated indept + * measurements of a constant. + * + * True value of unknown set to 10, + * utest observes filter converging toward that value + */ + + /* seed for rng */ + uint64_t seed = 14950319842636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on 'measurements', + * use as reference implementation for filter + */ + SampleStatistics z_stats; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0) = [0] */ + VectorXd x0(1); + x0 << 10.0 + normal_rng(); + + INFO(tostr("x0=", x0)); + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0 = 1.0 * MatrixXd::Identity(1, 1); + + /* F, Q, K, j, zk not used for initial state */ + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1 /*j*/, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_identity1_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + + rp sk = spec.start_ext(); + + for(uint32_t i_step = 1; i_step < 100; ++i_step) { + /* note: for this filter, measurement time doesn't matter */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(1); + z << 10.0 + normal_rng(); + + INFO(tostr("z=", z)); + + z_stats.include_sample(z[0]); + + ref::rp inputk + = KalmanFilterInput::make_present(tkp1, z); + + KalmanFilterStep step_spec = spec.make_step(sk, inputk); + + rp skp1 + = KalmanFilterEngine::step(step_spec); + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 1); + REQUIRE(skp1->state_v().size() == 1); + REQUIRE(skp1->state_v()[0] == Approx(z_stats.mean()).epsilon(1e-6)); + REQUIRE(skp1->state_cov().rows() == 1); + REQUIRE(skp1->state_cov().cols() == 1); + REQUIRE(skp1->gain().rows() == 1); + REQUIRE(skp1->gain().cols() == 1); + REQUIRE(skp1->observable() == -1); + + /* z_stats reflects k = z_stats.n_sample() N(0,1) 'random' vars; + * variance of sum (i.e. z_stats.mean() * k) is proportional to k; + * variance of mean like 1/k + * + * kalman filter also should compute covariance estimate like 1/k + */ + + REQUIRE(skp1->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + REQUIRE(skp1->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + /* estimate at each step should be (approximately) + * average of measurements taken so far. + * approximate because also affected by prior + */ + + sk = skp1; + } + + REQUIRE(sk->state_v()[0] == Approx(10.0).epsilon(1e-2)); + REQUIRE(sk->state_cov()(0, 0) == Approx(0.01).epsilon(1e-6)); + REQUIRE(sk->gain()(0, 0) == Approx(0.01).epsilon(1e-6)); + } /*TEST_CASE(kalman-identity)*/ + + /* example 2. + * like example 1, but using "separate observation" variants: + * KalmanGain::correct1() // per observation + * instead of + * KalmanGain::correct() // per observation set + */ + TEST_CASE("kalman-identity1", "[kalmanfilter]") { + /* setting up trivial filter for repeated indept + * measurements of a constant. + * + * True value of unknown set to 10, + * utest observes filter converging toward that value + * + */ + + /* seed for rng */ + uint64_t seed = 14950319842636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on 'measurements', + * use as reference implementation for filter + */ + SampleStatistics z_stats; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0) = [0] */ + VectorXd x0(1); + x0 << 10.0 + normal_rng(); + + INFO(tostr("x0=", x0)); + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0 = 1.0 * MatrixXd::Identity(1, 1); + + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_identity1_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + + rp sk = spec.start_ext(); + + for(uint32_t i_step = 1; i_step < 100; ++i_step) { + /* note: for this filter, measurement time doesn't matter */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(1); + z << 10.0 + normal_rng(); + + INFO(tostr("z=", z)); + + z_stats.include_sample(z[0]); + + ref::rp inputk + = KalmanFilterInput::make_present(tkp1, z); + + KalmanFilterStep step_spec + = spec.make_step(sk, inputk); + + rp skp1 + = KalmanFilterEngine::step1(step_spec, 0 /*j*/); + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 1); + REQUIRE(skp1->state_v().size() == 1); + REQUIRE(skp1->state_v()[0] == Approx(z_stats.mean()).epsilon(1e-6)); + REQUIRE(skp1->state_cov().rows() == 1); + REQUIRE(skp1->state_cov().cols() == 1); + REQUIRE(skp1->gain().rows() == 1); + REQUIRE(skp1->gain().cols() == 1); + REQUIRE(skp1->observable() == 0); + + /* z_stats reflects k = z_stats.n_sample() N(0,1) 'random' vars; + * variance of sum (i.e. z_stats.mean() * k) is proportional to k; + * variance of mean like 1/k + * + * kalman filter also should compute covariance estimate like 1/k + */ + + REQUIRE(skp1->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + REQUIRE(skp1->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + /* estimate at each step should be (approximately) + * average of measurements taken so far. + * approximate because also affected by prior + */ + + sk = skp1; + } + + REQUIRE(sk->state_v()[0] == Approx(10.0).epsilon(1e-2)); + REQUIRE(sk->state_cov()(0, 0) == Approx(0.01).epsilon(1e-6)); + REQUIRE(sk->gain()(0, 0) == Approx(0.01).epsilon(1e-6)); + } /*TEST_CASE(kalman-identity1)*/ + + namespace { + /* step for kalman filter with: + * - single state variable x[0] + * - identity process model: x(k+1) = F(k).x(k), with F(k) = | 1 | + * - no process noise + * - two observations z[0], z[1] + * - identity coupling matrix: z(k) = H(k).x(k) + w(k), with + * H(k) = | 1 | + * | 1 | + * + * w(k) = | w1 | with w1 ~ N(0,1) + * | w2 | + */ + KalmanFilterSpec::MkStepFn + kalman_identity2_mkstep_fn() + { + /* kalman state transition matrix: use identity <-> state is constant */ + MatrixXd F = MatrixXd::Identity(1, 1); + + /* state transition noise: set to 0 */ + MatrixXd Q = MatrixXd::Zero(1, 1); + + /* two direct observations */ + MatrixXd H = MatrixXd::Constant(2 /*#rows*/, 1 /*#cols*/, 1.0 /*M(i,j)*/); + + /* observation errors: N(0,1) */ + MatrixXd R = MatrixXd::Identity(2, 2); + + return [F, Q, H, R](rp const & sk, + rp const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk = KalmanFilterObservable::keep_all(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_identity2_mkstep_fn*/ + } /*namespace*/ + + TEST_CASE("kalman-identity2", "[kalmanfilter]") { + /* variation on filter in kalman-identity1 utest above; + * this time make 2 observations per step + */ + + /* seed for rng */ + uint64_t seed = 14950319842636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on 'measurements', + * use as reference implementation for filter + */ + SampleStatistics z_stats; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0) = [0] */ + VectorXd x0(1); + x0 << 10.0 + normal_rng(); + + INFO(tostr("x0=", x0)); + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0 = 1.0 * MatrixXd::Identity(1, 1); + + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1 /*j*/, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_identity2_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + rp sk = spec.start_ext(); + + /* need 1/2 as many filter steps to reach same confidence + * as in test "kalman-identity" + */ + for(uint32_t i_step = 1; i_step < 51; ++i_step) { + INFO(tostr(xtag("i_step", i_step))); + + /* note: for this filter, measurement time doesn't affect behavior */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(2); + z << 10.0 + normal_rng(), 10.0 + normal_rng(); + + z_stats.include_sample(z[0]); + z_stats.include_sample(z[1]); + + INFO(tostr(xtag("i_step", i_step), xtag("z", z))); + + ref::rp inputk + = KalmanFilterInput::make_present(tkp1, z); + + KalmanFilterStep step_spec = spec.make_step(sk, inputk); + + rp skp1 = KalmanFilterEngine::step(step_spec); + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 1); + REQUIRE(skp1->state_v().size() == 1); + REQUIRE(skp1->state_v()[0] == Approx(z_stats.mean()).epsilon(1e-6)); + REQUIRE(skp1->state_cov().rows() == 1); + REQUIRE(skp1->state_cov().cols() == 1); + REQUIRE(skp1->gain().rows() == 1); + REQUIRE(skp1->gain().cols() == 2); + REQUIRE(skp1->observable() == -1); + /* z_stats reflects 2*k = z_stats.n_sample() N(0,1) 'random' vars + * (since 2 vars per step) + * variance of sum (i.e. z_stats.mean() * k) is proportional to k; + * variance of mean like 1/k + * + * kalman filter also should compute covariance estimate like 1/k + * + */ + + REQUIRE(skp1->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + REQUIRE(skp1->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + REQUIRE(skp1->gain()(0, 1) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-6)); + + /* estimate at each step should be (approximately) + * average of measurements taken so far. + * approximate because also affected by prior + */ + + sk = skp1; + } + + REQUIRE(sk->state_v()[0] == Approx(10.0).epsilon(1e-2)); + REQUIRE(sk->state_cov()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-3)); + /* result is close but not identical, + * because initial confidence P0 counts as one sample, + * so have odd #of samples + */ + REQUIRE(sk->gain()(0, 0) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-3)); + REQUIRE(sk->gain()(0, 1) == Approx(1.0 / z_stats.n_sample()).epsilon(1e-3)); + } /*TEST_CASE(kalman-identity2)*/ + + namespace { + /* step for kalman filter with: + * - two state variables x[0], x[1] + * x[0] subject to random disturbances, reverts towards mean 1 + * x[1] is 1 + * - process model: x(k+1) = F(k).x(k) + v(k) with + * F(k) = | 0.95 0.05 | v(k) = | v1 |, v ~ N(0, 0.25) + * | 0 1 | | 0 | + * - one observation z[0] + * - coupling matrix: z(k) = H(k).x(k) + w(k), with + * H(k) = | 1 0 | + * + * w(k) ~ N(0,1) + */ + KalmanFilterSpec::MkStepFn + kalman_revert1_mkstep_fn() + { + /* kalman state transition matrix */ + MatrixXd F(2,2); + F << 0.95, 0.05, 0, 1; + + /* state transition noise */ + MatrixXd Q(2,2); + Q << 0.0001, 0.0, 0.0, 0.0; + + /* coupling matrix */ + MatrixXd H(1,2); + H << 1.0, 0.0; + + /* observation errors */ + MatrixXd R(1,1); + R << 0.25; + + return [F, Q, H, R](rp const & sk, + rp const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk = KalmanFilterObservable::keep_all(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_revert1_mkstep_fn*/ + } /*namespace*/ + + TEST_CASE("kalman-revert1", "[kalmanfilter]") { + /* like test "kalman-identity", + * but introduce some system noise. + */ + + constexpr bool c_debug_enabled = false; + scope lscope(XO_DEBUG2(c_debug_enabled, "TEST(kalman_revert)")); + + /* seed for rng */ + uint64_t seed = 14950139742636922572UL; + + /* N(0,1) random numbers */ + auto normal_rng + = (normalgen::make + (seed, + std::normal_distribution(0.0 /*mean*/, + 1.0 /*sdev*/))); + + /* accumulate statistics on observations, + * use as reference when assessing filter behavior + */ + SampleStatistics z_stats; + + /* write output to file - use as baseline for regression testing */ + std::string self_test_name = Catch::getResultCapture().getCurrentTestName(); + + /* write space-delimited output, suitable for gnuplot + * omit always-constant values, rely on unit test verifying these + */ + std::ofstream out(self_test_name); + out << "step z0 x0 P00 K0" << std::endl; + + utc_nanos t0 = timeutil::ymd_midnight(20220707); + + /* estimate x(0). + * x(0)[1] is constant 1, used to achieve mean reversion + */ + VectorXd x0(2); + x0 << 1.0 + normal_rng(), 1.0; + + z_stats.include_sample(x0[0]); + + /* kalman prior : Variance = 1, sdev = 1 */ + MatrixXd P0(2,2); + P0 << 1.0, 0.0, 0.0, 0.0; + + rp s0 + = KalmanFilterStateExt::make(0 /*step#*/, + t0, + x0, + P0, + KalmanFilterTransition(MatrixXd::Zero(1, 1) /*F*/, + MatrixXd::Zero(1, 1) /*Q*/), + MatrixXd::Zero(1, 1) /*K*/, + -1 /*j*/, + nullptr /*zk*/); + + auto mk_step_fn + = kalman_revert1_mkstep_fn(); + + KalmanFilterSpec spec(s0, mk_step_fn); + rp sk = spec.start_ext(); + + for(uint32_t i_step = 1; i_step < 100; ++i_step) { + /* note: for this filter, measurement time doesn't affect behavior */ + utc_nanos tkp1 = sk->tm() + seconds(1); + + VectorXd z(1); + z << 1.0 + normal_rng(); + + z_stats.include_sample(z[0]); + + ref::rp inputk + = KalmanFilterInput::make_present(tkp1, z); + KalmanFilterStep step_spec = spec.make_step(sk, inputk); + rp skp1 = KalmanFilterEngine::step(step_spec); + + if (c_debug_enabled) { + lscope.log("filter", + xtag("step", i_step), + xtag("z", matrix(z)), + xtag("x", matrix(skp1->state_v())), + xtag("P", matrix(skp1->state_cov())), + xtag("K", matrix(skp1->gain()))); + } + + /* headings: step z0 x0 P00 K0 */ + out << i_step + << " " << z(0) + << " " << skp1->state_v()(0) + << " " << skp1->state_cov()(0, 0) + << " " << skp1->gain()(0, 0) + << "\n"; + + REQUIRE(skp1->step_no() == i_step); + REQUIRE(skp1->tm() == tkp1); + REQUIRE(skp1->n_state() == 2); + // + REQUIRE(skp1->state_v().size() == 2); + REQUIRE(skp1->state_v()(1) == 1.0); + // + REQUIRE(skp1->state_cov().rows() == 2); + REQUIRE(skp1->state_cov().cols() == 2); + // test skp1->state_cov()(0,0) vs baseline + REQUIRE(skp1->state_cov()(0, 0) >= 0.0); + REQUIRE(skp1->state_cov()(1, 0) == 0.0); + REQUIRE(skp1->state_cov()(0, 1) == 0.0); + REQUIRE(skp1->state_cov()(1, 1) == 0.0); + // + REQUIRE(skp1->gain().rows() == 2); + REQUIRE(skp1->gain().cols() == 1); + REQUIRE(skp1->gain()(0, 0) > 0.0); + REQUIRE(skp1->gain()(1, 0) == 0.0); + // + REQUIRE(skp1->observable() == -1); + // + + sk = skp1; + } + + out << std::flush; + out.close(); + + /* compare output with regression baseline. + * command like: + * diff kalman-revert1 utestdata/filter/kalman-revert1 + */ + char cmd_buf[256]; + snprintf(cmd_buf, sizeof(cmd_buf), + "diff %s utestdata/filter/%s\n", + self_test_name.c_str(), + self_test_name.c_str()); + + INFO(tostr(self_test_name, xtag("cmd", ccs(cmd_buf)))); + + std::int32_t err = ::system(cmd_buf); + + REQUIRE(err == 0); + } /*TEST_CASE(kalman-drift)*/ + +#ifdef NOT_IN_USE + namespace { + /* step for kalman filter with: + * - two state variables x[0], x[1] + * - identity process model: x(k+1) = F(k).x(k), with + * F(k) = | 1 0 | + * | 0 1 | + * - no process noise + * - two observations z[0], z[1] + * - simple coupling matrix: z(k) = H(k).x(k) + w(k), with + * H(k) = | 1 0 | + * | 0 -1 | + * (so sign of z[1] is reversed w.r.t x[1]) + * + * w(k) = | w1 | with w1 ~ N(0,1) + * | w2 | + */ + KalmanFilterSpec::MkStepFn + kalman_identity2x2_mkstep_fn() + { + /* kalman state transition matrix: use identity <-> state is constant */ + MatrixXd F = MatrixXd::Identity(2, 2); + + /* state transition noise: set to 0 */ + MatrixXd Q = MatrixXd::Zero(2, 2); + + /* two direct observations */ + MatrixXd H = MatrixXd::Constant(2 /*#rows*/, 1 /*#cols*/, 1.0 /*M(i,j)*/); + + /* observation errors: N(0,1) */ + MatrixXd R = MatrixXd::Identity(2, 2); + + return [F, Q, H, R](KalmanFilterState const & sk, + KalmanFilterInput const & zkp1) { + KalmanFilterTransition Fk(F, Q); + KalmanFilterObservable Hk(H, R); + + return KalmanFilterStep(sk, Fk, Hk, zkp1); + }; + } /*kalman_identity2_mkstep_fn*/ + } /*namespace*/ +#endif + } /*namespace ut*/ +} /*namespace xo*/ + +/* end KalmanFilter.test.cpp */ diff --git a/utest/filter_utest_main.cpp b/utest/filter_utest_main.cpp new file mode 100644 index 00000000..9ad0266a --- /dev/null +++ b/utest/filter_utest_main.cpp @@ -0,0 +1,6 @@ +/* @file filter_utest_main.cpp */ + +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" + +/* end filter_utest_main.cpp */