commit 8454b48956dcaf7b8884588595ef52d230067b4f Author: Roland Conybeare Date: Sun Oct 22 20:51:11 2023 -0400 initial implementation diff --git a/CMakeLists.txt.old b/CMakeLists.txt.old new file mode 100644 index 00000000..e85d245f --- /dev/null +++ b/CMakeLists.txt.old @@ -0,0 +1,37 @@ +# filter/CMakeLists.txt + +set(SELF_LIBRARY_NAME filter) + +set(SELF_SOURCE_FILES KalmanFilterSvc.cpp KalmanFilter.cpp KalmanFilterState.cpp KalmanFilterTransition.cpp KalmanFilterObservable.cpp KalmanFilterInput.cpp KalmanFilterStep.cpp KalmanFilterSpec.cpp KalmanFilterEngine.cpp KalmanFilterInputToConsole.cpp KalmanFilterStateToConsole.cpp EigenUtil.cpp init_filter.cpp) + +# build shared liburary 'filter' +add_library(${SELF_LIBRARY_NAME} SHARED ${SELF_SOURCE_FILES}) + +set_target_properties(${SELF_LIBRARY_NAME} + PROPERTIES + VERSION ${PROJECT_VERSION} + SOVERSION 1 + PUBLIC_HEADER init_filter.hpp) + +# ---------------------------------------------------------------- +# all the warnings! +# +xo_compile_options(${SELF_LIBRARY_NAME}) +xo_include_options(${SELF_LIBRARY_NAME}) + +# ---------------------------------------------------------------- +# internal dependencies: reactor, ... + +#target_link_libraries(${SELF_LIBRARY_NAME} PUBLIC process) +target_link_libraries(${SELF_LIBRARY_NAME} PUBLIC reactor) +target_link_libraries(${SELF_LIBRARY_NAME} PUBLIC printjson) +target_link_libraries(${SELF_LIBRARY_NAME} PUBLIC refcnt) + +# ---------------------------------------------------------------- +# 3rd party dependency: eigen: + +xo_eigen_dependency(${SELF_LIBRARY_NAME}) + +xo_install_library(${SELF_LIBRARY_NAME}) + +# end CMakeLists.txt diff --git a/EXAMPLES b/EXAMPLES new file mode 100644 index 00000000..8f69ea73 --- /dev/null +++ b/EXAMPLES @@ -0,0 +1,164 @@ +scaffold kalman filter implementation here + +notation: + +x_(k) : n x 1 : true system state that we want to estimate +F(k) : n x n : state transition matrix at time t(k) +w_(k) : n x 1 : system noise, with mean 0, covariance Q(k) +Q(k) : n x n : covariance of systen noise + +v_(k) : m x 1 : observation errors at time t(k) +R(k) : m x m : observation error covariance matrix at time t(k) + +x(k) : n x 1 : state vector estimate for time t(k) +P(k) : n x n : covariance matrix for x(k) +z(k) : m x 1 : observation vector at time t(k) +H(k) : m x n : observation matrix at time t(k) + +K(k) : : kalman gain - measures information gain from observation z(k) + relative to prior P(k | k-1) + +use shorthand: + xT, F(k)T for transpose(x), transpose(F(k)) + x(k+1 | k) for "estimate x(k+1) given information known at t(k)" + +A. Linear Kalman Filter +----------------------- + +1. system model: + x_(k+1) = F(k).x_(k) + w_(k), w_(k) ~ N(0,Q) + + i.e. expected behavior of system from t(k) -> t(k+1), + absent system noise, is given by linear transformation F(k) + + ofc model is not directly observable, + since we don't know x_(k) or w_(k), + instead we will be estimating it. + +2. measurement model: + z(k+1) = H(k+1).x_(k+1) + v_(k), v_(k) ~ N(0, R) + +3. prior: + x(0), P(0) + + must be supplied as initial input + +4. pre-estimate for t(k+1) system state: + (before incorporating z(k+1) and accounting for system noise) + x(k+1|k) := F(k).x(k) + + in other words propagate t(k) estimate to t(k+1), + using F(k) + +5. pre-estimate for t(k+1) estimate covariance + (before incorporating z(k+1) and accounting for system noise) + P(k+1|k) := F(k).P(k).F(k)T + Q(k) + +6. kalman gain matrix + K(k+1) := P(k+1|k).H(k)T.inverse(H(k).P(k+1|k).H(k)T + R(k)) + + note that the matrix-to-be-inverted in the gain expression + is symmetric and positive-definite (so can use cholesky decomposition) + +7. innovation: difference between actual observation vector and + observation predicted from state estimate x(k+1|k): + z(k+1) - H(k+1).x(k+1|k) + +8. corrected state estimate for t(k+1): + x(k+1) := x(k+1|k) + K(k+1)[z(k+1) - H(k+1).x(k+1|k)] + +9. correct state covariance for t(k+1): + P(k+1) := [I - K(k+1).H(k+1)].P(k+1|k) + + +B. Extended Kalman Filter +------------------------- +Must write in continuous form, to deal with non-linearities + +notation: + +x_(t) : n x 1 : true system state that we want to estimate, + continuously evolves with t +f(x_(t),t) : n x 1 : expected derivative (d/dt)(x_(t)) +h(x_(t),t) : m x 1 : observation function +x(k) : n x 1 : state vector estimate for x(t) at time t=t(k) +F(x(t),t) : n x n : jacobian of f(x_(t),t) evaluated at x_(t)=x(t) +H(x(t),t) : m x n : jacobian of h(x_(t),t) evaluated at x_(t)=x(t) + +1. system model: + (d/dt)x_(t) = f(x_(t),t) + w(t), + where + w(t) is a white noise with + + / t2 + | + | w(t).dt ~ N(0, Q(t1,t2)) + | + / t1 + + editorial: + note that we exclude more general model + (d/dt)x_(t) = f(x_(t),t)) + G(x_(t),t).w(t) + because would be unable to assume RHS mean-square integrable. + The form chosen effectively band-limits the frequencies at which + w(t) acts, since it's independent of x_(t). This allows mean-square + rules to be applied as in linear kalman filter + +2. measurement model: + z(k+1) = h(x_(t(k+1)), t(k+1)) + v(k), v(k) ~ N(0, Rk) + +3. prior: + x(0), P(0) + + (same as for linear kalman filter) + +(3a). state estimate propagation model: + (d/dt)x(t) = f(x(t),t) + + editorial: + this step is *sketchy*: x(t) is intended to be minimum-variance + estimate for system state, but that property is lost when solving + differential equation (d/dt)x(t) = f(x(t),t) with non-linear f. + +(3b). covariance estimate propagation model: + (d/dt)P(t) = F(x(t),t).P(t) + P(t).F(x(t),t)T + Q(t) + +4. pre-estimate (extrapolation) for t(k+1) system state: + + can use first order approx: + x(k+1|k) = x(k) + F(x(tk),tk).(t(k+1) - t(k)) + or multiple timesteps if t(k+1) - t(k) is too large + + i.e. approximately solving (3a) + +5. pre-estimate (extrapolation) for t(k+1) error covariance: + + P(k+1|k) = P(k) + [F(x(tk),tk).P(k) + P(k)F(x(tk),tk)T + Q(tk)].(t(k+1) - t(k)) + or multiple timesteps if t(k+1) - t(k) is too large + + i.e. approximately solving (3b) + +6. kalman gain matrix + K(k+1) = P(k+1|k) + .H(k+1)T(x(k+1|k)) + .inverse[H(k+1)(x(k+1|k)).P(k+1|k).H(k+1)T(x(k+1|k)) + R(k+1)] + +7. innovation: + z(k+1) - h(x(k+1|k), t(k+1)) + +8. corrected state estimate for t(k+1): + x(k+1) = x(k+1|k) + K(k+1).(z(k+1) - h(k+1)(x(k+1|k)) + +9. corrected error covariance for t(k+1): + P(k+1) = [I - K(k+1).H(k+1)(x(k+1|k))].P(k+1|k) + + editorial: + This step is *sketchy*, to the extend h() is non-linear, linearizing around + x(k+1|k) may give poor estimate of state covariance + +10. linearization (1st order term for taylor series of f() around x(t)) + jacobian: + F(x(t),t) = (df/dx_) evaluated at x_(t)=x(t) + + H(k+1)(x(k+1|k)) = (dh/dx_) evaluated at x_(t)=x(k+1|k) + diff --git a/EigenUtil.cpp b/EigenUtil.cpp new file mode 100644 index 00000000..f1d3c4a3 --- /dev/null +++ b/EigenUtil.cpp @@ -0,0 +1,157 @@ +/* file EigenUtil.cpp + * + * author: Roland Conybeare, Sep 2022 + */ + +#include "EigenUtil.hpp" +#include "printjson/PrintJson.hpp" +#include "reflect/Reflect.hpp" +#include +#include +#include +#include + +namespace xo { + using xo::json::PrintJson; + using xo::json::JsonPrinter; + using xo::reflect::Reflect; + using xo::reflect::TypeDescr; + using VectorXb = Eigen::Array; + using Eigen::VectorXd; + using Eigen::MatrixXd; + +#ifdef NOT_YET + namespace reflect { + template + using EigenVectorX_Tdx = xo::reflect::StlVectorTdx>; + + /* probably need this to appear before decl for class xo::reflect::Reflect */ + template + class EstablishTdx> { + public: + static std::unique_ptr make() { + return EigenVectorX_Tdx::make(); + } /*make*/ + }; /*EstablishTdx*/ + } /*reflect*/ +#endif + + namespace eigen { + + namespace { + /* prints a VectorXd as json, in the obvious format, e.g. + * [1,2,3] + */ + template + class EigenVectorJsonPrinter : public JsonPrinter { + public: + EigenVectorJsonPrinter(PrintJson const * pjson) : JsonPrinter(pjson) {} + + virtual void print_json(TaggedPtr tp, + std::ostream * p_os) const override + { + EigenVectorType * pv = this->check_recover_native(tp, p_os); + + if (pv) { + /* EigenVectorType (VectorXb, VectorXd, ..) + * is reflected as atomic for now, out of expedience. + * + * as soon as we reflect as mt_vector, will not need this helper. + */ + *p_os << "["; + + for (std::uint32_t i = 0, n = pv->size(); i < n; ++i) { + if (i > 0) + *p_os << ","; + + /* note: need to dispatch via json printer for vector elements, + * to get special treatment for non-finite values + */ + this->pjson()->print((*pv)[i], p_os); + //*p_os << jsonp((*pv)[i], this->pjson()); + } + + *p_os << "]"; + } + } /*print_json*/ + }; /*EigenVectorJsonPrinter*/ + + /* prints a MatrixXd as json, in row-major format, e.g. + * [[1,2,3], [4,5,6], [7,8,9]] + */ + class MatrixXdJsonPrinter : public JsonPrinter { + public: + MatrixXdJsonPrinter(PrintJson const * pjson) : JsonPrinter(pjson) {} + + virtual void print_json(TaggedPtr tp, + std::ostream * p_os) const override + { + MatrixXd * pm = this->check_recover_native(tp, p_os); + + if (pm) { + /* MatrixXd is reflected as atomic for now, out of expedience */ + *p_os << "["; + + for(std::uint32_t i=0, m=pm->rows(); i 0) + *p_os << ", "; + *p_os << "["; + for(std::uint32_t j=0, n=pm->cols(); j 0) + *p_os << ","; + + /* note: need to dispatch via json printer for matrix elements, + * to get special treatment for non-finite values + */ + this->pjson()->print((*pm)(i, j), p_os); + //*p_os << jsonp((*pm)(i, j), this->pjson()); + } + *p_os << "]"; + } + + *p_os << "]"; + } + } /*print_json*/ + }; /*MatrixXdJsonPrinter*/ + + template + void + provide_eigen_vector_printer(PrintJson * p_pjson) + { + TypeDescr td = Reflect::require(); + std::unique_ptr pr(new EigenVectorJsonPrinter(p_pjson)); + + p_pjson->provide_printer(td, std::move(pr)); + } /*provide_eigen_vector_printer*/ + } /*namespace*/ + + void + EigenUtil::reflect_eigen() + { +#ifdef NOT_YET + Reflect::require(); + Reflect::require(); +#endif + } /*reflect_eigen*/ + + void + EigenUtil::provide_json_printers(PrintJson * p_pjson) + { + assert(p_pjson); + + provide_eigen_vector_printer(p_pjson); + provide_eigen_vector_printer(p_pjson); + + { + TypeDescr td = Reflect::require(); + std::unique_ptr pr(new MatrixXdJsonPrinter(p_pjson)); + + p_pjson->provide_printer(td, std::move(pr)); + } + } /*provide_json_printers*/ + } /*namespace eigen*/ +} /*namespace xo*/ + +/* end EigenUtil.cpp */ diff --git a/EigenUtil.hpp b/EigenUtil.hpp new file mode 100644 index 00000000..07a86d48 --- /dev/null +++ b/EigenUtil.hpp @@ -0,0 +1,29 @@ +/* file EigenUtil.hpp + * + * author: Roland Conybeare, Sep 2022 + */ + +#pragma once + +namespace xo { + namespace json { class PrintJson; } + + namespace eigen { + class EigenUtil { + public: + /* reflection for + * Eigen::VectorXd + * VectorXb (= Eigen::Array; by analogy with Eigen::VectorXd) + */ + static void reflect_eigen(); + + /* json printers for: + * Eigen::VectorXd + * Eigen::MatrixXd + */ + static void provide_json_printers(json::PrintJson * pjson); + }; /*EigenUtil*/ + } /*namespace eigen*/ +} /*namespace xo*/ + +/* end EigenUtil.hpp */ diff --git a/KalmanFilter.cpp b/KalmanFilter.cpp new file mode 100644 index 00000000..bf94fde2 --- /dev/null +++ b/KalmanFilter.cpp @@ -0,0 +1,78 @@ +/* @file KalmanFilter.cpp */ + +#include "KalmanFilter.hpp" +#include "KalmanFilterEngine.hpp" +#include "print_eigen.hpp" +#include "indentlog/scope.hpp" +#include "Eigen/src/Core/Matrix.h" + +namespace xo { + using xo::time::utc_nanos; + //using logutil::matrix; + using xo::scope; + using xo::tostr; + using xo::xtag; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + // ----- KalmanFilter ----- + + KalmanFilter::KalmanFilter(KalmanFilterSpec spec) + : filter_spec_{std::move(spec)}, + state_ext_{filter_spec_.start_ext()} + {} /*ctor*/ + + void + KalmanFilter::notify_input(ref::rp const & input_kp1) + { + scope log(XO_ENTER0(info)); + + /* on entry: + * .state_ext refers to t(k) + * on exit: + * .step refers to t(k+1) + * .state_ext refers to t(k+1) + */ + + log && log(xtag("step_dt", + input_kp1->tkp1() - this->state_ext_->tm())); + + /* establish step inputs for this filter step: + * F(k+1) (system transition matrix) + * Q(k+1) (system noise covariance matrix) + * H(k+1) (observation coupling matrix) + * R(k+1) (observation noise covariance matrix) + * z(k+1) (observation vector) + */ + this->step_ = this->filter_spec_.make_step(this->state_ext_, input_kp1); + + //if (lscope.enabled()) { lscope.log(xtag("step", this->step_)); } + + /* extrapolate filter state to t(k+1), + * and correct based on z(k+1) + */ + this->state_ext_ = KalmanFilterEngine::step(this->step_); + + //if (lscope.enabled()) { lscope.log(xtag("state_ext", this->state_ext_)); } + } /*notify_input*/ + + void + KalmanFilter::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilter::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilter.cpp */ diff --git a/KalmanFilter.hpp b/KalmanFilter.hpp new file mode 100644 index 00000000..295a4f7a --- /dev/null +++ b/KalmanFilter.hpp @@ -0,0 +1,128 @@ +/* @file KalmanFilter.hpp */ + +#pragma once + +#include "filter/KalmanFilterSpec.hpp" + +namespace xo { + namespace kalman { + /* Specification for an ordinary discrete linear kalman filter. + * + * The filter generates estimates for a process observed at a discrete + * set of times tk in {t0, t1, .., tn} + * + * At each time tk we have the following: + * + * 0. x(0) initial estimate at t(0) + * P(0) initial priors: error covariance matrix for x(0) + * + * 1. x_(k), [n x 1] vector: + * system state, denoted by vector. + * (state is not directly observable, + * filter will attempt to estimate it) + * + * 2. w_(k), [n x 1] vector + * Q(k), [n x n] matrix + * + * w_(k) denotes system noise, + * gaussian with covariance Q(k). + * noise w_(k) is not directly observable. + * + * 3. z(k), [m x 1] vector: + * + * observation vector for time tk + * + * 4. v_(k), [m x 1] vector + * R(k), [m x m] matrix + * + * v_(k) denotes observation errors, + * gaussian with covariance R(k). + * noise v_(k) is not directly observable. + * + * 5. F(k), [n x n] matrix + * state transition matrix + * model system evolves according to: + * + * x_(k+1) = F(x).x_(k) + w_(k) + * + * 6. observations z(k) depend on system state: + * + * z(k) = H(k).x_(k) + v_(k) + * + * 7. Kalman filter outputs: + * x(k), [n x 1] vector + * Q(k), [n x n] matrix + * + * x(k) is optimal estimate for system state x_(k) + * P(k) is covariance matrix specifying confidence intervals + * for pairs (x(k)[i], x(k)[j]) + * + * filter specification consists of: + * n, m, x(0), P(0), F(k), Q(k), H(k), R(k) + * The cardinality of observations z(k) can vary over time, + * so to be precise, m can vary with tk; write as m(k) + * + * More details: + * - avoid having to specify t(k) in advance; + * instead defer until observation available + * so t(k) can be taken from polling timestamp + */ + + /* encapsulate a (linear) kalman filter + * together with event publishing + */ + class KalmanFilter { + public: + using MatrixXd = Eigen::MatrixXd; + using VectorXd = Eigen::VectorXd; + using utc_nanos = xo::time::utc_nanos; + + public: + /* create filter with specification given by spec, and initial state s0 */ + explicit KalmanFilter(KalmanFilterSpec spec); + + uint32_t step_no() const { return state_ext_->step_no(); } + utc_nanos tm() const { return state_ext_->tm(); } + KalmanFilterSpec const & filter_spec() const { return filter_spec_; } + KalmanFilterStep const & step() const { return step_; } + ref::rp const & state_ext() const { return state_ext_; } + + /* notify kalman filter with input for time t(k+1) = input_kp1.tkp1() + * Require: input.tkp1() >= .current_tm() + * Promise: + * - .tm() = input_kp1.tkp1() + * - .step_no() = old .step_no() + 1 + * - .filter_spec_k, .step_k, .state_k updated + * for observations in input_kp1 + */ + void notify_input(ref::rp const & input_kp1); + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + /* specification for kalman filter; + * produces process/observation matrices on demand + */ + KalmanFilterSpec filter_spec_; + + /* filter step for most recent observation */ + KalmanFilterStep step_; + + /* filter state as of most recent observation; + * result of applying KalmanFilterEngine::step() to contents of .step + */ + ref::rp state_ext_; + }; /*KalmanFilter*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilter const & x) { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + + +/* end KalmanFilter.hpp */ diff --git a/KalmanFilterEngine.cpp b/KalmanFilterEngine.cpp new file mode 100644 index 00000000..e80f7497 --- /dev/null +++ b/KalmanFilterEngine.cpp @@ -0,0 +1,360 @@ +/* @file KalmanFilterEngine.cpp + * + */ + +#include "KalmanFilterEngine.hpp" +#include "print_eigen.hpp" +#include "indentlog/scope.hpp" +#include "Eigen/src/Core/Matrix.h" + +namespace xo { + using xo::time::utc_nanos; + using logutil::matrix; + using xo::scope; + using xo::xtag; + using Eigen::LDLT; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + // ----- KalmanFilterEngine ----- + + ref::rp + KalmanFilterEngine::extrapolate(utc_nanos tkp1, + ref::rp const & s, + KalmanFilterTransition const & f) + { + //constexpr char const * c_self_name + // = "KalmanFilterEngine::extrapolate"; + + /* prior estimates at t(k) */ + VectorXd const & x = s->state_v(); + MatrixXd const & P = s->state_cov(); + + /* model change from t(k) -> t(k+1) */ + MatrixXd const & F = f.transition_mat(); + MatrixXd const & Q = f.transition_cov(); + + if(F.cols() != x.rows()) { + scope log(XO_DEBUG(true /*debug_flag*/)); + + log("error: F*x: expected F.cols=x.rows", + xtag("F.cols", F.cols()), xtag("x.rows", x.rows())); + } + + /* x(k+1|k) */ + VectorXd x_ext = F * x; + + /* P(k+1|k) */ + MatrixXd P_ext = (F * P * F.transpose()) + Q; + + /* creating new state object here + * allows KalmanFilterSvc.is_volatile()=false + */ + + return KalmanFilterState::make(s->step_no() + 1, + tkp1, + std::move(x_ext), + std::move(P_ext), + f); + } /*extrapolate*/ + + VectorXd + KalmanFilterEngine::kalman_gain1(ref::rp const & skp1_ext, + KalmanFilterObservable const & h, + uint32_t j) + { + constexpr bool c_debug_enabled = false; + + scope log(XO_DEBUG(c_debug_enabled)); + + /* P(k+1|k) :: [n x n] */ + MatrixXd const & P_ext = skp1_ext->state_cov(); + + /* H(k) :: [m x n] */ + MatrixXd const & H = h.observable(); + /* R(k) :: [m x m] */ + MatrixXd const & R = h.observable_cov(); + + /* i'th col of H couples element #i of filter state to each member of input z(k); + * j'th row of H couples filter state to j'th observable + * + * Hj :: [1 x n] Hj is a row-vector + */ + auto Hj = H.row(j); + + /* Rjj is the j'th diagonal element of R */ + double Rjj = R(j, j); + + /* T + * M(k) = Hj * P(k+1|k) * Hj + Rjj + * + * M(k) is a [1 x 1] matrix + */ + double m = Hj * (P_ext * Hj.transpose()) + Rjj; + + /* -1 + * M(k) trivial, since M is [1 x 1] + */ + double m_inv = 1.0 / m; + + /* K :: [n x 1] */ + VectorXd K = P_ext * Hj.transpose() * m_inv; + + log && log("result", + xtag("P(k+1|k)", matrix(P_ext)), + xtag("R", matrix(R)), + xtag("m", m)); + + return K; + } /*kalman_gain1*/ + + MatrixXd + KalmanFilterEngine::kalman_gain(ref::rp const & skp1_ext, + KalmanFilterObservable const & h) + { + scope log(XO_DEBUG(false /*debug_enabled*/)); + + /* P(k+1|k) */ + MatrixXd const & P_ext = skp1_ext->state_cov(); + + MatrixXd const & H = h.observable(); + MatrixXd const & R = h.observable_cov(); + + uint32_t m = H.rows(); + uint32_t n = H.cols(); + + if ((P_ext.rows() != n) || (P_ext.cols() != n)) { + std::string err_msg + = tostr("kalman_gain: with dim(H) = [m x n] expect dim(P) = [n x n]", + xtag("m", m), xtag("n", n), + xtag("P.rows", P_ext.rows()), + xtag("P.cols", P_ext.cols())); + + throw std::runtime_error(err_msg); + } + + if ((R.rows() != m) || (R.cols() != m)) { + std::string err_msg + = tostr("kalman_gain: with dim(H) = [m x n] expect dim(R) = [m x m]", + xtag("m", m), xtag("n", n), + xtag("R.rows", R.rows()), xtag("R.cols", R.cols())); + + throw std::runtime_error(err_msg); + } + + /* kalman gain: + * T -1 + * K(k+1) = P(k+1|k).H(k) .M + * + * T / T \ -1 + * = P(k+1|k).H(k) .| H(k).P(k+1|k).H(k) + R(k) | + * \ / + * + * Notes: + * 1. the matrix M being inverted is symmetric, since represents covariances. + * 2. if diagonal of R(k) has no zeroes (i.e. all measurements are subject to error), + * then it must be non-negative definite + * 3. unless observation errors are perfectly correlated, M(k) + * is positive definite. + * 4. even though 3. holds, there may be a nearby non-positive-definite matrix M+dM, + * Factoring M with finite-precision arithmetic solves for M+dM instead of M; + * which may run into difficulty if M is only 'slighlty' +ve definite. + * If necessary add small diagonal correction D to M, + * sufficient to make M+D positive definite. + * This is equivalent to introducing additional + * uncorrelated observation error, so benign from a robustness perspective + * 5. In generally we usually want to avoid fully realizing a matrix inverse. + * In this case need to explicitly compute K as ingredient used to + * correct state covariance later. + * 6. However, if R is diagonal (which is in practice quite likely), + * then it's easy to decompose a suite of vector observations z(k+1) = [z1, ..zm]T + * into separate zi, with dt=0 separating them. + * Can use this to avoid computing the inverse. + * See .kalman_gain1(), .correct1() + * 7. .kalman_gain() works unaltered when H, R have been reindexed + * to exclude outliers/errors; this is true because .kalman_gain() does not + * use the observation vector z[], i.e. operates entirely in the reduced + * reindexed space. + */ + + MatrixXd M = H * P_ext * H.transpose() + R; + + /* will use to write M as: + * + * T T + * M = P .L.D.L .P + * + * where: + * P is a permutation matrix + * L is lower triangular, with unit diagonal + * D is diagonal + */ + LDLT ldlt = M.ldlt(); + + /* solve for the identity matrix to realize the inverse this way */ + MatrixXd I = MatrixXd::Identity(M.rows(), M.cols()); + + /* -1 + * M + */ + MatrixXd M_inv = ldlt.solve(I); + + /* K(k+1) */ + MatrixXd K = P_ext * H.transpose() * M_inv; + + log && log("result", + xtag("k", skp1_ext->step_no()), + xtag("P(k+1|k)", matrix(P_ext)), + xtag("H", matrix(H)), + xtag("R", matrix(R)), + xtag("M", matrix(M)), + xtag("K", matrix(K))); + + return K; + } /*kalman_gain*/ + + ref::rp + KalmanFilterEngine::correct1(ref::rp const & skp1_ext, + KalmanFilterObservable const & h, + ref::rp const & zkp1, + uint32_t j) + { + uint32_t n = skp1_ext->n_state(); + /* Kj :: [n x 1] */ + VectorXd Kj = kalman_gain1(skp1_ext, h, j); + /* H :: [m x n] */ + MatrixXd const & H = h.observable(); + VectorXd const & z = zkp1->z(); + + /* Hj :: [1 x n] the j'th row of H */ + auto const & Hj = H.row(j); + + + /* x(k+1|x) :: [n x 1] */ + VectorXd const & x_ext = skp1_ext->state_v(); + + /* P(k+1|k) :: [n x n] */ + MatrixXd const & P_ext = skp1_ext->state_cov(); + + /* innovj : difference between jth 'actual observation' + * and jth 'predicted observation' + */ + double innovj = z[j] - (Hj * x_ext); + + /* x(k+1) */ + VectorXd xkp1 = x_ext + (Kj * innovj); + + MatrixXd I = MatrixXd::Identity(n, n); + /* note: Kj [n x 1], Hj [1 x n], + * so Kj * Hj [n x n], with rank 1 + */ + MatrixXd Pkp1 = (I - (Kj * Hj)) * P_ext; + + return KalmanFilterStateExt::make(skp1_ext->step_no(), + skp1_ext->tm(), + xkp1, + Pkp1, + skp1_ext->transition(), + Kj, + j, + zkp1); + } /*correct1*/ + + ref::rp + KalmanFilterEngine::correct(ref::rp const & skp1_ext, + KalmanFilterObservable const & h, + ref::rp const & zkp1) + { + uint32_t n = skp1_ext->n_state(); + /* K :: [n x m] */ + MatrixXd K = kalman_gain(skp1_ext, h); + MatrixXd const & H = h.observable(); + /* z_orig[] is original observation vector before reindexing */ + VectorXd const & z_orig = zkp1->z(); + /* reindex z_orig, keeping only elements that appear in + */ + VectorXd z = z_orig(h.keep()); + + /* 'ext' short for 'extrapolated' */ + VectorXd const & x_ext = skp1_ext->state_v(); + MatrixXd const & P_ext = skp1_ext->state_cov(); + + /* innov: difference between 'actual observations' + * and 'predicted observations' + */ + VectorXd innov = z - (H * x_ext); + + /* x(k+1) :: [n x 1] */ + VectorXd xkp1 = x_ext + K * innov; + MatrixXd I = MatrixXd::Identity(n, n); + MatrixXd Pkp1 = (I - K * H) * P_ext; + + return KalmanFilterStateExt::make(skp1_ext->step_no(), + skp1_ext->tm(), + xkp1, + Pkp1, + skp1_ext->transition(), + K, + -1 /*j: not used*/, + zkp1); + } /*correct*/ + + ref::rp + KalmanFilterEngine::step(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1) + { + ref::rp skp1_ext + = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); + + ref::rp skp1 + = KalmanFilterEngine::correct(skp1_ext, Hkp1, zkp1); + + return skp1; + } /*step*/ + + ref::rp + KalmanFilterEngine::step(KalmanFilterStep const & step_spec) + { + return step(step_spec.tkp1(), + step_spec.state(), + step_spec.model(), + step_spec.obs(), + step_spec.input()); + } /*step*/ + + ref::rp + KalmanFilterEngine::step1(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1, + uint32_t j) + { + ref::rp skp1_ext + = KalmanFilterEngine::extrapolate(tkp1, sk, Fk); + + ref::rp skp1 + = KalmanFilterEngine::correct1(skp1_ext, Hkp1, zkp1, j); + + return skp1; + } /*step1*/ + + ref::rp + KalmanFilterEngine::step1(KalmanFilterStep const & step_spec, + uint32_t j) + { + return step1(step_spec.tkp1(), + step_spec.state(), + step_spec.model(), + step_spec.obs(), + step_spec.input(), + j); + } /*step1*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterEngine.cpp */ diff --git a/KalmanFilterEngine.hpp b/KalmanFilterEngine.hpp new file mode 100644 index 00000000..b8c2f949 --- /dev/null +++ b/KalmanFilterEngine.hpp @@ -0,0 +1,185 @@ +/* @file KalmanFilterEngine.hpp */ + +#pragma once + +#include "filter/KalmanFilterStep.hpp" +#include "filter/KalmanFilterState.hpp" +#include "filter/KalmanFilterTransition.hpp" +#include "filter/KalmanFilterObservable.hpp" +#include "filter/KalmanFilterInput.hpp" + +namespace xo { + namespace kalman { + class KalmanFilterEngine { + public: + using MatrixXd = Eigen::MatrixXd; + using VectorXd = Eigen::VectorXd; + using utc_nanos = xo::time::utc_nanos; + + public: + /* evolution of system state + account for system noise, + * before incorporating effect of observations z(k+1) + * x(k) --> x(k+1|k) + * P(k) --> P(k+1|k) + * + * tkp1. time t(k+1) assoc'd with step k+1 + * sk. filter state at time tk: + * sk.k = k step # (starts from 0) + * sk.tk = t(k) time t(k) assoc'd with step #k + * sk.x = x(k) estimated state at time tk + * sk.P = P(k) quality of state estimate x(k) + * (error covariance matrix) + * Fk. state transition: + * Fk.F = F(k) state transition matrix + * Fk.Q = Q(k) covariance matrix for system noise + * + * returns propagated state estimate for t(k+1): + * retval.k = k+1 + * retval.tk = t(k+1) = tkp1 + * retval.x = x(k+1|k) + * retval.P = P(k+1|k) + */ + static ref::rp extrapolate(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk); + + /* compute kalman gain matrix for filter step t(k) -> t(k+1) + * Expensive implementation using matrix inversion + * + * T + * M(k+1) = H(k).P(k+1|k).H(k) + R(k) + * + * T -1 + * K(k+1) = P(k+1|k).H(k) .M(k+1) + * + * Require: + * - skp1_ext.n_state() = Hkp1.n_state() + * + * skp1_ext. extrapolated filter state at t(k+1) + * Hkp1. relates model state to observable variables, + * for step t(k) -> t(k+1) + */ + static MatrixXd kalman_gain(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1); + + /* compute kalman gain for a single observation z(k)[j]. + * This is useful iff the observation error matrix R is diagonal. + * For diagonal R we can present a set of observations z(k) serially + * instead of all at once, with lower time complexity + * + * Kalman Filter specifies some space with m observables. + * j identifies one of those observables, indexing from 0. + * This corresponds to row #j of H(k), and element R[j,j] of R. + * + * Effectively, we are projecting the kalman filter assoc'd with + * {skp1_ext, Hkp1} to a filter with a single observable variable z(k)[j], + * then computing the (scalar) kalman gain for this 1-variable filter + * + * The gain vector tells us for each member of filter state, + * how much to adjust our optimal estimate for that member for a unit + * amount of innovation in observable #j, i.e. for difference between + * expected and actual value for that observable. + */ + static VectorXd kalman_gain1(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + uint32_t j); + + /* correct extrapolated state+cov estimate; + * also computes kalman gain + * + * Require: + * - skp1_ext.n_state() = Hkp1.n_state() + * - zkp1.n_obs() == Hkp1.n_observable() + * + * skp1_ext. extrapolated kalman state + covaraince at t(k+1) + * Hkp1. relates model state to observable variables + * for step t(k) -> t(k+1) + * zkp1. observations for time t(k+1) + * + * return new filter state+cov + */ + static ref::rp correct(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1); + + /* correct extrapolated filter state for observation + * of j'th filter observable z(k+1)[j] + * + * Can use this when observation errors are uncorrelated + * (i.e. observation error matrix R is diagonal) + */ + static ref::rp correct1(ref::rp const & skp1_ext, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1, + uint32_t j); + + /* step filter from t(k) -> t(k+1) + * + * sk. filter state from previous step: + * x (state vector), P (state covar matrix) + * Fk. transition-related params: + * F (transition matrix), Q (system noise covar matrix) + * Hkp1. observation-related params: + * H (coupling matrix), R (error covar matrix) + * zkp1. observations z(k+1) for time t(k+1) + */ + static ref::rp step(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1); + + /* step filter from t(k) -> tk(k+1) + * same as + * .step(tkp1, sk, step_spec.model(), step_spec.obs(), zkp1); + * + * step_spec. encapsulates Fk (transition-related params) + * and Q (system noise covar matrix) + */ + static ref::rp step(KalmanFilterStep const & step_spec); + + /* step filter from t(k) -> t(k+1) + * + * sk. filter state from previous step: + * x (state vector), P (state covar matrix) + * Fk. transition-related params: + * F (transition matrix), Q (system noise covar matrix) + * Hkp1. observation-related params: + * H (coupling matrix), R (error covar matrix) + * zkp1. observations z(k+1) for time t(k+1) + * j. identifies a single filter observable -- + * step will only consume observation z(k+1)[j] + */ + static ref::rp step1(utc_nanos tkp1, + ref::rp const & sk, + KalmanFilterTransition const & Fk, + KalmanFilterObservable const & Hkp1, + ref::rp const & zkp1, + uint32_t j); + + /* step filter from t(k) -> t(k+1) + * + * same as + * .step1(step_spec.tkp1(), + * step_spec.state(), + * step_spec.model(), + * step_spec.obs(), + * step_spec.input(), + * j); + * + * step_spec. encapsulates + * x (state vector), + * P (state covar matrix), + * Fk (transition-related params), + * Q (system noise covar matrix) + * z (z(k+1), observations at time t(k+1)) + * j. identifies a single filter observable -- + * step will only consume observation z(k+1)[j] + */ + static ref::rp step1(KalmanFilterStep const & step_spec, + uint32_t j); + }; /*KalmanFilterEngine*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterEngine.hpp */ diff --git a/KalmanFilterInput.cpp b/KalmanFilterInput.cpp new file mode 100644 index 00000000..03b5330f --- /dev/null +++ b/KalmanFilterInput.cpp @@ -0,0 +1,118 @@ +/* @file KalmanFilterInput.cpp */ + +#include "KalmanFilterInput.hpp" +#include "reflect/StructReflector.hpp" +#include "Eigen/src/Core/Matrix.h" +#include "print_eigen.hpp" +#include "indentlog/scope.hpp" +#include "reflect/TaggedRcptr.hpp" + +namespace xo { + using xo::reflect::Reflect; + using xo::reflect::TaggedRcptr; + using xo::reflect::StructReflector; + using xo::scope; + using logutil::matrix; + using xo::xtag; + using Eigen::MatrixXd; + using Eigen::VectorXi; + using std::uint32_t; + + namespace kalman { + ref::rp + KalmanFilterInput::make(utc_nanos tkp1, + VectorXb const & presence, + VectorXd const & z, + VectorXd const & Rd) + { + return new KalmanFilterInput(tkp1, presence, z, Rd); + } /*make*/ + + ref::rp + KalmanFilterInput::make_present(utc_nanos tkp1, + VectorXd const & z) + { + VectorXb presence = VectorXb::Ones(z.cols()); + + return new KalmanFilterInput(tkp1, + presence, + z, + VectorXd(0) /*Rd - not using*/); + } /*make*/ + + VectorXi + KalmanFilterInput::make_kept_index() const + { + scope log(XO_DEBUG(false /*!debug_flag*/)); + + log && log(xtag("presence", matrix(presence_))); + + /* count truth values */ + uint32_t mstar = 0; + + for (uint32_t j = 0, m = this->presence_.rows(); jpresence_[j]) + ++mstar; + } + + log && log(xtag("m*", mstar)); + + VectorXi keep(mstar); + + /* 2nd pass, populate keep[] */ + + uint32_t jstar = 0; + + for (uint32_t j = 0, m = this->presence_.rows(); jpresence_[j]) { + keep[jstar] = j; + ++jstar; + } + } + + log && log("keep", matrix(keep)); + + return keep; + } /*make_kept_index*/ + + void + KalmanFilterInput::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + REFLECT_MEMBER(sr, tkp1); + REFLECT_MEMBER(sr, presence); + REFLECT_MEMBER(sr, z); + REFLECT_MEMBER(sr, Rd); + } + } /*reflect_self*/ + + reflect::TaggedRcptr + KalmanFilterInput::self_tp() + { + return Reflect::make_rctp(this); + } /*self_tp*/ + + void + KalmanFilterInput::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterInput::display_string() const + { + std::stringstream ss; + this->display(ss); + return ss.str(); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInput.cpp */ diff --git a/KalmanFilterInput.hpp b/KalmanFilterInput.hpp new file mode 100644 index 00000000..a73a72d0 --- /dev/null +++ b/KalmanFilterInput.hpp @@ -0,0 +1,121 @@ +/* @file KalmanFilterInput.hpp */ + +#pragma once + +#include "reflect/SelfTagging.hpp" +#include "time/Time.hpp" +#include "refcnt/Refcounted.hpp" +#include +#include + +namespace xo { + /* FIXME. hack here to get member access working for reflection */ + namespace vf { class StrikesetKfinput; } + + namespace kalman { + /* represents a single kalman filter input event + * comprising: + * - time tkp1 + * - observation vector z[] + * - presence bits presence[] for z + * - observation errors Rd[] + */ + class KalmanFilterInput : public reflect::SelfTagging { + public: + using TaggedRcptr = xo::reflect::TaggedRcptr; + using utc_nanos = xo::time::utc_nanos; + using VectorXb = Eigen::Array; + using VectorXi = Eigen::VectorXi; + using VectorXd = Eigen::VectorXd; + using uint32_t = std::uint32_t; + + public: + KalmanFilterInput() = default; + + static ref::rp make(utc_nanos tkp1, + VectorXb const & presence, + VectorXd const & z, + VectorXd const & Rd); + + /* create input, with all presence bits set + not using Rd */ + static ref::rp make_present(utc_nanos tkp1, + VectorXd const & z); + + /* reflect KalmanFilterInput object representation */ + static void reflect_self(); + + /* alt name -- concession to reactor::DirectSource + * when event type is KalmanFilterInput + */ + utc_nanos tm() const { return tkp1_; } + + utc_nanos tkp1() const { return tkp1_; } + uint32_t n_obs() const { return z_.size(); } + VectorXb const & presence() const { return presence_; } + VectorXd const & z() const { return z_; } + VectorXd const & Rd() const { return Rd_; } + + /* computes reindexer keep[]: + * .presence[keep[j*]] + * is the j*'th true value in .presence + */ + VectorXi make_kept_index() const; + + virtual void display(std::ostream & os) const; + std::string display_string() const; + + // ----- inherited from SelfTagging ----- + + virtual TaggedRcptr self_tp() override; + + protected: + KalmanFilterInput(utc_nanos tkp1, VectorXb presence, VectorXd z, VectorXd Rd) + : tkp1_(tkp1), + presence_{std::move(presence)}, + z_{std::move(z)}, + Rd_{std::move(Rd)} {} + + friend class xo::vf::StrikesetKfinput; + + private: + /* t(k+1) - asof time for observations .z */ + utc_nanos tkp1_ = xo::time::timeutil::epoch(); + /* [m x 1] presence vector. + * an observation z[j] is present iff .presence[j] is true. + * rows/columns for an absent observation are removed from filter matrices + */ + VectorXb presence_; + /* [m x 1] observation vector z(k) */ + VectorXd z_; + + /* [m x 1] observation error vector Rd(k). + * This represents a side channel for passing the diagonal of + * observation matrix R(k), when both: + * (a) error of different observations are assumed to be uncorrelated (likely) + * (b) error variance is derived from input data, e.g. because of + * some input preprocessing. + * + * It's up to KalmanFilterSpec::MkStepFn to opt-in to using this information, + * which requires agreement with any input preparation step. + * + * This variable could just as well provide observation error matrix R + * + * NOTE: perhaps-cleaner alternative would be to inherit KalmanFilterInput to + * introduce additional state, then MkStepFn can dynamic_cast + */ + VectorXd Rd_; + }; /*KalmanFilterInput*/ + + using KalmanFilterInputPtr = ref::rp; + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterInput const & x) + { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInput.hpp */ diff --git a/KalmanFilterInputCallback.hpp b/KalmanFilterInputCallback.hpp new file mode 100644 index 00000000..d9b2f213 --- /dev/null +++ b/KalmanFilterInputCallback.hpp @@ -0,0 +1,14 @@ +/* @file KalmanFilterInputCallback.hpp */ + +#pragma once + +#include "refcnt/Refcounted.hpp" +#include "filter/KalmanFilter.hpp" + +namespace xo { + namespace kalman { + using KalmanFilterInputCallback = reactor::Sink1>; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInputCallback.hpp */ diff --git a/KalmanFilterInputSource.hpp b/KalmanFilterInputSource.hpp new file mode 100644 index 00000000..30c5cbf1 --- /dev/null +++ b/KalmanFilterInputSource.hpp @@ -0,0 +1,28 @@ +/* @file KalmanFilterInputSource.hpp */ + +#pragma once + +#include "reactor/EventSource.hpp" +#include "filter/KalmanFilterInputCallback.hpp" + +namespace xo { + namespace kalman { + /* Use: + * rp src = ...; + * rp in_cb = ...; + * + * src->add_callback(in_cb); + * ... + * // src invokes in_cb->notify_input( + * src->remove_callback(in_cb); + */ + using KalmanFilterInputSource = reactor::EventSource< + /*KalmanFilterInput,*/ + KalmanFilterInputCallback + /*&KalmanFilterInputCallback::notify_filter*/ + >; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInputSource.hpp */ + diff --git a/KalmanFilterInputToConsole.cpp b/KalmanFilterInputToConsole.cpp new file mode 100644 index 00000000..a0a848ca --- /dev/null +++ b/KalmanFilterInputToConsole.cpp @@ -0,0 +1,25 @@ +/* @file KalmanFilterInputToConsole.cpp */ + +#include "KalmanFilterInputToConsole.hpp" +#include "indentlog/print/tag.hpp" + +namespace xo { + using xo::xtag; + + namespace kalman { + ref::rp + KalmanFilterInputToConsole::make() { + return new KalmanFilterInputToConsole(); + } /*make*/ + + void + KalmanFilterInputToConsole::display(std::ostream & os) const + { + os << ""; + } /*display*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterInputToConsole.cpp */ diff --git a/KalmanFilterInputToConsole.hpp b/KalmanFilterInputToConsole.hpp new file mode 100644 index 00000000..7d48ef7e --- /dev/null +++ b/KalmanFilterInputToConsole.hpp @@ -0,0 +1,30 @@ +/* @file KalmanFilterInputToConsole.hpp */ + +#pragma once + +#include "reactor/Sink.hpp" +#include "filter/KalmanFilterInput.hpp" + +namespace xo { + namespace kalman { + class KalmanFilterInputToConsole + : public xo::reactor::SinkToConsole> + { + public: + KalmanFilterInputToConsole() = default; + + static ref::rp make(); + + virtual void display(std::ostream & os) const; + //virtual std::string display_string() const; + }; /*KalmanFilterInputToConsole*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterInputToConsole const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace option*/ +} /*namespace xo*/ + +/* end KalmanFilterInputToConsole.hpp */ diff --git a/KalmanFilterObservable.cpp b/KalmanFilterObservable.cpp new file mode 100644 index 00000000..90f31460 --- /dev/null +++ b/KalmanFilterObservable.cpp @@ -0,0 +1,71 @@ +/* @file KalmanFilterObservable.cpp */ + +#include "KalmanFilterObservable.hpp" +#include "print_eigen.hpp" +#include "indentlog/scope.hpp" + +namespace xo { + using xo::scope; + using logutil::matrix; + using xo::xtag; + + namespace kalman { + KalmanFilterObservable + KalmanFilterObservable::keep_all(MatrixXd H, + MatrixXd R) + { + VectorXi keep(H.rows()); + + for (uint32_t j=0; j"; + } /*display*/ + + std::string + KalmanFilterObservable::display_string() const + { + std::stringstream ss; + this->display(ss); + return ss.str(); + } /*display_string*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterObservable.cpp */ diff --git a/KalmanFilterObservable.hpp b/KalmanFilterObservable.hpp new file mode 100644 index 00000000..87c4f9d1 --- /dev/null +++ b/KalmanFilterObservable.hpp @@ -0,0 +1,110 @@ +/* @file KalmanFilterObservable.hpp */ + +#pragma once + +#include "time/Time.hpp" +#include +#include + +namespace xo { + namespace kalman { + class KalmanFilterObservable { + public: + using MatrixXd = Eigen::MatrixXd; + using VectorXi = Eigen::VectorXi; + + public: + KalmanFilterObservable() = default; + + /* keep maps back to canonical observations z(j). + * H, R have been re-indexed + * + * If all m observations are included, then keep will be: + * [0, .., m-1] + */ + KalmanFilterObservable(VectorXi keep, MatrixXd H, MatrixXd R) + : keep_{std::move(keep)}, H_{std::move(H)}, R_{std::move(R)} { + assert(this->check_ok()); + } /*ctor*/ + + /* build KF observable object where keeping all observations */ + static KalmanFilterObservable keep_all(MatrixXd H, MatrixXd R); + + /* build KF observable object. replace H, R with reindexed versions H', R' + * according to indexing vector keep[]. keep[] indexes members of + * observation vector z(k)[j]. Reindexed z', H', R' as follows: + * + * z'[j*] = z[keep[j*]] + * H'[j*, i] = H[keep[j*], i] + * R'[j1*, j2*] = R[keep[j1*], keep[j2*]] + */ + static KalmanFilterObservable reindex(VectorXi keep, MatrixXd H, MatrixXd R); + + uint32_t n_state() const { return H_.cols(); } + uint32_t n_observable() const { return H_.rows(); } + VectorXi const & keep() const { return keep_; } + MatrixXd const & observable() const { return H_; } + MatrixXd const & observable_cov() const { return R_; } + + bool check_ok() const { + uint32_t m = H_.rows(); + bool keep_is_mx1 = (keep_.rows() == m); + bool r_is_mxm = ((R_.cols() == m) && (R_.rows() == m)); + + bool keep_is_well_ordered = true; + + /* members of .keep are non-negative integers, + * in strictly increasing order + */ + int64_t keep_jm1 = -1; + + for (uint32_t j = 0; j < keep_.rows(); ++j) { + if (keep_[j] < 0) + keep_is_well_ordered = false; + if (keep_[j] <= keep_jm1) + keep_is_well_ordered = false; + } + + /* also would like to require: R is +ve definite */ + + return keep_is_mx1 && keep_is_well_ordered && r_is_mxm; + } /*check_ok*/ + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + + private: + /* m: #of observations that survived sanity/error checks + * + * H, R here will have been re-indexed to exclude rejected observations. + * observations z will also have been re-indexed. + * + * If an observation z[j] is excluded, then also exclude: + * - j'th row of H + * - j'th row and j'th column of R + * - j'th element of z + * + * Given re-indexed H, R, the j*'th row of H goes with z[keep[j*]] + */ + + /* [m x 1] maps back to indices in original observation vector */ + VectorXi keep_; + /* [m x n] observation matrix */ + MatrixXd H_; + /* [m x m] covariance matrix for observation noise */ + MatrixXd R_; + }; /*KalmanFilterObservable*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterObservable const & x) + { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterObservable.hpp */ + diff --git a/KalmanFilterOutputCallback.hpp b/KalmanFilterOutputCallback.hpp new file mode 100644 index 00000000..69fb8484 --- /dev/null +++ b/KalmanFilterOutputCallback.hpp @@ -0,0 +1,15 @@ +/* @file KalmanFilterOutputCallback.hpp */ + +#pragma once + +#include "reactor/Sink.hpp" +#include "filter/KalmanFilter.hpp" + +namespace xo { + namespace kalman { + /* callback for consuming kalman filter output */ + using KalmanFilterOutputCallback = reactor::Sink1>; + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterOutputCallback.hpp */ diff --git a/KalmanFilterSpec.cpp b/KalmanFilterSpec.cpp new file mode 100644 index 00000000..296aeb99 --- /dev/null +++ b/KalmanFilterSpec.cpp @@ -0,0 +1,27 @@ +/* @file KalmanFilterSpec.cpp */ + +#include "KalmanFilterSpec.hpp" +#include "indentlog/scope.hpp" + +namespace xo { + using xo::tostr; + using xo::xtag; + + namespace kalman { + void + KalmanFilterSpec::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterSpec::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterSpec.cpp */ diff --git a/KalmanFilterSpec.hpp b/KalmanFilterSpec.hpp new file mode 100644 index 00000000..56ffd783 --- /dev/null +++ b/KalmanFilterSpec.hpp @@ -0,0 +1,86 @@ +/* @file KalmanFilterSpec.hpp */ + +#pragma once + +#include "filter/KalmanFilterStep.hpp" + +namespace xo { + namespace kalman { + /* full specification for a kalman filter. + * + * For a textbook linear filter, expect a KalmanFilterStep + * instance to be independent of KalmanFilterState/KalmanFilterInput. + * + * Relaxing this requirement for two reasons: + * 1. (proper) want to allow filter with variable timing between observations, + * expecially if observations are event-driven. + * In that case it's likely that state transition matrices are a function + * of elapsed time between observations. Providing filter state sk + * allows MkStepFn to use sk.tm() + * 2. (sketchy) when observations represent market data, + * desirable to treat an observation as giving one-sided information + * about true value. For example treat a bid price as evidence + * true value is higher than that bid, but don't want to constrain + * "how much higher". Certainly no reason to think that + * bid price is normally distributed around fair value. + * Allow for hack in which we + * and modulate error distribution "as if it were normal" to assess + * a non-gaussian error distribution + */ + class KalmanFilterSpec { + public: + /* typical implementation will look something like: + * mk_step(KalmanFilterState const & sk, + * KalmanFilterInputPtr const & zkp1) + * { + * KalmanFilterTransition model = ...; + * KalmanFilterObservable obs = ...; + * + * return KalmanFilterStep(sk, model, obs, zkp1); + * } + */ + using MkStepFn = std::function const & sk, + KalmanFilterInputPtr const & zkp1)>; + + public: + explicit KalmanFilterSpec(ref::rp s0, MkStepFn mkstepfn) + : start_ext_{std::move(s0)}, + mk_step_fn_{std::move(mkstepfn)} {} + + ref::rp const & start_ext() const { return start_ext_; } + /* get step parameters (i.e. matrices F, Q, H, R) + * for step t(k) -> t(k+1). + * + * We supply t(k) state s and t(k+1) observation z(k+1): + * - to allow time stepping to be observation-driven + * - to allow for selective outlier removal + */ + KalmanFilterStep make_step(ref::rp const & sk, + ref::rp const & zkp1) { + return this->mk_step_fn_(sk, zkp1); + } /*make_step*/ + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + /* starting state */ + ref::rp start_ext_; + + /* creates kalman filter step object on demand; + * step object specifies inputs to 1 step in discrete + * linear kalman filter + */ + MkStepFn mk_step_fn_; + }; /*KalmanFilterSpec*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterSpec const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterSpec.hpp */ diff --git a/KalmanFilterState.cpp b/KalmanFilterState.cpp new file mode 100644 index 00000000..7656908c --- /dev/null +++ b/KalmanFilterState.cpp @@ -0,0 +1,231 @@ +/* @file KalmanFilterState.cpp */ + +#include "KalmanFilterState.hpp" +#include "print_eigen.hpp" +#include "reflect/StructReflector.hpp" +#include "reflect/TaggedPtr.hpp" +#include "indentlog/scope.hpp" +#include "Eigen/src/Core/Matrix.h" +#include +#include + +namespace xo { + using xo::reflect::Reflect; + using xo::reflect::TaggedRcptr; + using xo::reflect::StructReflector; + using xo::time::utc_nanos; + using xo::ref::rp; + using logutil::matrix; + using logutil::vector; + //using xo::scope; + using xo::xtag; + using xo::tostr; + //using Eigen::LDLT; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + // ----- KalmanFilterState ----- + + rp + KalmanFilterState::make() + { + return new KalmanFilterState(); + } /*make*/ + + rp + KalmanFilterState::make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition) + { + return new KalmanFilterState(k, tk, + std::move(x), + std::move(P), + std::move(transition)); + } /*make*/ + + void + KalmanFilterState::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + REFLECT_MEMBER(sr, k); + REFLECT_MEMBER(sr, tk); + REFLECT_MEMBER(sr, x); + REFLECT_MEMBER(sr, P); + } + } /*reflect_self*/ + + KalmanFilterState::KalmanFilterState() = default; + + KalmanFilterState::KalmanFilterState(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition) + : k_{k}, tk_{tk}, + x_{std::move(x)}, P_{std::move(P)}, + transition_{std::move(transition)} + {} + + TaggedRcptr + KalmanFilterState::self_tp() + { + return Reflect::make_rctp(this); + } /*self_tp*/ + + // ----- KalmanFilterExt ----- + + rp + KalmanFilterStateExt::make() + { + return new KalmanFilterStateExt(); + } /*make*/ + + rp + KalmanFilterStateExt::make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + rp zk) + { + return new KalmanFilterStateExt(k, + tk, + std::move(x), + std::move(P), + std::move(transition), + std::move(K), + j, + std::move(zk)); + } /*make*/ + + void + KalmanFilterStateExt::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + /* TODO: use sr.adopt_ancestors() */ + + REFLECT_EXPLICIT_MEMBER(sr, "k", &KalmanFilterState::k_); + REFLECT_EXPLICIT_MEMBER(sr, "tk", &KalmanFilterState::tk_); + REFLECT_EXPLICIT_MEMBER(sr, "x", &KalmanFilterState::x_); + REFLECT_EXPLICIT_MEMBER(sr, "P", &KalmanFilterState::P_); + REFLECT_EXPLICIT_MEMBER(sr, "transition", &KalmanFilterState::transition_); + REFLECT_MEMBER(sr, j); + REFLECT_MEMBER(sr, K); + REFLECT_MEMBER(sr, zk); + } + } /*reflect_self*/ + + KalmanFilterStateExt::KalmanFilterStateExt(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + rp zk) + : KalmanFilterState(k, tk, + std::move(x), + std::move(P), + std::move(transition)), + j_{j}, + K_{std::move(K)}, + zk_{std::move(zk)} + { + uint32_t n = x.size(); + + if (n != P.rows() || n != P.cols()) { + std::string err_msg + = tostr("with n=x.size expect [n x n] covar matrix P", + xtag("n", x.size()), + xtag("P.rows", P.rows()), + xtag("P.cols", P.cols())); + + throw std::runtime_error(err_msg); + } + + if ((K.rows() > 0) && (K.rows() > 0)) { + if (n != K.rows()) { + std::string err_msg + = tostr("with n=x.size expect [m x n] gain matrix K", + xtag("n", x.size()), + xtag("K.rows", K.rows()), + xtag("K.cols", K.cols())); + + throw std::runtime_error(err_msg); + } + } else { + /* bypass test with [0 x 0] matrix K; + * normal for initial filter state + */ + } + } /*ctor*/ + + void + KalmanFilterState::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterState::display_string() const + { + std::stringstream ss; + ss << *this; + return ss.str(); + } /*display_string*/ + + // ----- KalmanFilterStateExt ----- + + ref::rp + KalmanFilterStateExt::initial(utc_nanos t0, + VectorXd x0, + MatrixXd P0) + { + return KalmanFilterStateExt::make + (0 /*k*/, + t0, + std::move(x0), + std::move(P0), + KalmanFilterTransition(MatrixXd() /*F - not used for initial step*/, + MatrixXd() /*Q - not used for initial step*/), + MatrixXd() /*K - not used for initial step*/, + -1 /*j - not used for initial step*/, + nullptr /*zk - not defined for initial step*/); + } /*initial*/ + + void + KalmanFilterStateExt::display(std::ostream & os) const + { + os << "step_no()) + << xtag("tk", this->tm()) + << xtag("x", matrix(this->state_v())) + << xtag("P", matrix(this->state_cov())) + << xtag("K", matrix(K_)) + << xtag("j", j_) + << ">"; + } /*display*/ + + TaggedRcptr + KalmanFilterStateExt::self_tp() + { + return Reflect::make_rctp(this); + } /*self_tp*/ + } /*namespace filter*/ +} /*namespace xo*/ + +/* end KalmanFilterState.cpp */ diff --git a/KalmanFilterState.hpp b/KalmanFilterState.hpp new file mode 100644 index 00000000..113c4c6b --- /dev/null +++ b/KalmanFilterState.hpp @@ -0,0 +1,163 @@ +/* @file KalmanFilterState.hpp */ + +#pragma once + +#include "reflect/SelfTagging.hpp" +#include "filter/KalmanFilterInput.hpp" +#include "filter/KalmanFilterTransition.hpp" +#include "time/Time.hpp" +#include +#include +#include + +namespace xo { + namespace kalman { + /* encapsulate state (i.e. initial state, and output after each step) + * for a kalman filter + */ + class KalmanFilterState : public reflect::SelfTagging { + public: + using TaggedRcptr = reflect::TaggedRcptr; + using utc_nanos = xo::time::utc_nanos; + using VectorXd = Eigen::VectorXd; + using MatrixXd = Eigen::MatrixXd; + using uint32_t = std::uint32_t; + + public: + static ref::rp make(); + static ref::rp make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition); + virtual ~KalmanFilterState() = default; + + /* reflect KalmanFilterState object representation */ + static void reflect_self(); + + uint32_t step_no() const { return k_; } + utc_nanos tm() const { return tk_; } + /* with n = .n_state(): + * x_ is [n x 1] vector + * P_ is [n x n] matrix, + */ + uint32_t n_state() const { return x_.size(); } + VectorXd const & state_v() const { return x_; } + MatrixXd const & state_cov() const { return P_; } + + KalmanFilterTransition const & transition() const { return transition_; } + + virtual void display(std::ostream & os) const; + std::string display_string() const; + + // ----- inherited from SelfTagging ----- + + virtual TaggedRcptr self_tp() override; + + private: + KalmanFilterState(); + KalmanFilterState(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition); + + friend class KalmanFilterStateExt; + + private: + /* step# k, advances by +1 on each filter step */ + uint32_t k_ = 0; + /* time t(k) */ + utc_nanos tk_; + /* [n x 1] (estimated) system state xk = x(k) */ + VectorXd x_; + /* [n x n] covariance matrix for error assoc'd with with x(k) + * P(i,j) is the covariance of (ek[i], ek[j]), + * where ex(k) is the difference (x(k) - x_(k)) + * between estimated state x(k) + * (= this->x_) and model state x_(k) + */ + MatrixXd P_; + + /* F, Q matrices driving .x, .P */ + KalmanFilterTransition transition_; + }; /*KalmanFilterState*/ + + inline std::ostream & operator<<(std::ostream & os, + KalmanFilterState const & s) + { + s.display(os); + return os; + } /*operator<<*/ + + /* KalmanFilterStateExt: + * adds additional details from filter step to KalmanFilterState + */ + class KalmanFilterStateExt : public KalmanFilterState { + public: + using TaggedRcptr = reflect::TaggedRcptr; + using MatrixXd = Eigen::MatrixXd; + using int32_t = std::int32_t; + + public: + static ref::rp make(); + static ref::rp make(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + ref::rp zk); + + /* create state object for initial filter state */ + static ref::rp initial(utc_nanos t0, + VectorXd x0, + MatrixXd P0); + + /* reflect KalmanFilterStateExt object representation */ + static void reflect_self(); + + int32_t observable() const { return j_; } + MatrixXd const & gain() const { return K_; } + ref::rp const & zk() const { return zk_; } + + virtual void display(std::ostream & os) const override; + + // ----- inherited from SelfTagging ----- + + virtual TaggedRcptr self_tp() override; + + private: + KalmanFilterStateExt() = default; + KalmanFilterStateExt(uint32_t k, + utc_nanos tk, + VectorXd x, + MatrixXd P, + KalmanFilterTransition transition, + MatrixXd K, + int32_t j, + ref::rp zk); + + private: + /* if -1: not used; + * if >= 0: identifies j'th of m observables; + * gain .K applies just to information obtainable from + * observing that scalar variable + */ + int32_t j_ = -1; + /* if .j is -1: + * [n x n] kalman gain + * if .j >= 0: + * [n x 1] kalman gain for observable #j + */ + MatrixXd K_; + /* input leading to state k. + * empty for initial state (i.e. when .k is 0) + */ + ref::rp zk_; + }; /*KalamnFilterStateExt*/ + } /*namespace filter*/ +} /*namespace xo*/ + +/* end KalmanFilterState.hpp */ diff --git a/KalmanFilterStateToConsole.cpp b/KalmanFilterStateToConsole.cpp new file mode 100644 index 00000000..8e76ce84 --- /dev/null +++ b/KalmanFilterStateToConsole.cpp @@ -0,0 +1,25 @@ +/* @file KalmanFilterStateToConsole.cpp */ + +#include "KalmanFilterStateToConsole.hpp" +#include "indentlog/print/tag.hpp" + +namespace xo { + using xo::xtag; + + namespace kalman { + ref::rp + KalmanFilterStateToConsole::make() { + return new KalmanFilterStateToConsole(); + } /*make*/ + + void + KalmanFilterStateToConsole::display(std::ostream & os) const + { + os << ""; + } /*display*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterStateToConsole.cpp */ diff --git a/KalmanFilterStateToConsole.hpp b/KalmanFilterStateToConsole.hpp new file mode 100644 index 00000000..0d525cea --- /dev/null +++ b/KalmanFilterStateToConsole.hpp @@ -0,0 +1,30 @@ +/* @file KalmanFilterStateToConsole.hpp */ + +#pragma once + +#include "reactor/Sink.hpp" +#include "filter/KalmanFilterState.hpp" + +namespace xo { + namespace kalman { + class KalmanFilterStateToConsole + : public xo::reactor::SinkToConsole + { + public: + KalmanFilterStateToConsole() = default; + + static ref::rp make(); + + virtual void display(std::ostream & os) const; + //virtual std::string display_string() const; + }; /*KalmanFilterStateToConsole*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterStateToConsole const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace option*/ +} /*namespace xo*/ + +/* end KalmanFilterStateToConsole.hpp */ diff --git a/KalmanFilterStep.cpp b/KalmanFilterStep.cpp new file mode 100644 index 00000000..f62f2d21 --- /dev/null +++ b/KalmanFilterStep.cpp @@ -0,0 +1,84 @@ +/* @file KalmanFilterStep.cpp */ + +#include "KalmanFilterStep.hpp" +#include "filter/KalmanFilterEngine.hpp" +#include "filter/KalmanFilterState.hpp" +#include "indentlog/scope.hpp" + +namespace xo { + using xo::scope; + using xo::tostr; + using xo::xtag; + using Eigen::MatrixXd; + using Eigen::VectorXd; + + namespace kalman { + ref::rp + KalmanFilterStep::extrapolate() const + { + return KalmanFilterEngine::extrapolate(this->tkp1(), + this->state(), + this->model() /*transition*/); + } /*extrapolate*/ + + MatrixXd + KalmanFilterStep::gain(ref::rp const & skp1_ext) const + { + return KalmanFilterEngine::kalman_gain(skp1_ext, + this->obs()); + } /*gain*/ + + VectorXd + KalmanFilterStep::gain1(ref::rp const & skp1_ext, + uint32_t j) const + { + return KalmanFilterEngine::kalman_gain1(skp1_ext, + this->obs(), + j); + + } /*gain1*/ + + ref::rp + KalmanFilterStep::correct(ref::rp const & skp1_ext) + { + return KalmanFilterEngine::correct(skp1_ext, + this->obs(), + this->input()); + } /*correct*/ + + ref::rp + KalmanFilterStep::correct1(ref::rp const & skp1_ext, + uint32_t j) + { + return KalmanFilterEngine::correct1(skp1_ext, + this->obs(), + this->input(), + j); + } /*correct1*/ + + void + KalmanFilterStep::display(std::ostream & os) const + { + //scope lscope("KalmanFilterStep::display"); + + os << "model()); + //lscope.log("obs:"); + os << xtag("obs", this->obs()); + //lscope.log("input:"); + os << xtag("input", this->input()); + os << ">"; + } /*display*/ + + std::string + KalmanFilterStep::display_string() const + { + return tostr(*this); + } /*display_string*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterStep.cpp */ diff --git a/KalmanFilterStep.hpp b/KalmanFilterStep.hpp new file mode 100644 index 00000000..89813cc2 --- /dev/null +++ b/KalmanFilterStep.hpp @@ -0,0 +1,128 @@ +/* @file KalmanFilterStep.hpp */ + +#pragma once + +#include "KalmanFilterState.hpp" +#include "KalmanFilterInput.hpp" +#include "KalmanFilterTransition.hpp" +#include "KalmanFilterObservable.hpp" + +namespace xo { + namespace kalman { + /* encapsulate {state + observation} models for a single time step t(k). + * Emitted by KalmanFilterSpec, q.v. + */ + class KalmanFilterStepBase { + public: + KalmanFilterStepBase() = default; + KalmanFilterStepBase(KalmanFilterTransition model, + KalmanFilterObservable obs) + : model_{std::move(model)}, + obs_{std::move(obs)} {} + + /* aka system_model() */ + KalmanFilterTransition const & model() const { return model_; } + KalmanFilterObservable const & obs() const { return obs_; } + + private: + /* model for process being observed (state transition + noise) */ + KalmanFilterTransition model_; + /* what can be observed (observables + noise) */ + KalmanFilterObservable obs_; + }; /*KalmanFilterStepBase*/ + + /* encapsulate {state + observation} models for a single time step t(k). + * Emitted by KalmanFilterSpec, q.v. + * + * holds: + * x(k) + * P(k) + * F(k) + * H(k+1) + * z(k+1) + * + * contains all the inputs needed to compute: + * x(k+1) + * P(k+1) + * + * does not provide that result + */ + class KalmanFilterStep : public KalmanFilterStepBase { + public: + using utc_nanos = xo::time::utc_nanos; + using MatrixXd = Eigen::MatrixXd; + using VectorXd = Eigen::VectorXd; + + public: + KalmanFilterStep() = default; + KalmanFilterStep(ref::rp state, + KalmanFilterTransition model, + KalmanFilterObservable obs, + ref::rp zkp1) + : KalmanFilterStepBase(model, obs), + state_{std::move(state)}, + input_{std::move(zkp1)} {} + + ref::rp const & state() const { return state_; } + ref::rp const & input() const { return input_; } + + utc_nanos tkp1() const { return input_->tkp1(); } + + /* extrapolate kalman filter state forward to time + * .tkp1() (i.e. to t(k+1)); computes + * x(k+1|k) + * P(k+1|k) + * does not use the t(k+1) observations .input.z + */ + ref::rp extrapolate() const; + + /* compute kalman gain matrix K(k+1) + * given extrapolated t(k+1) state skp1_ext = {x(k+1|k), P(k+1|k)} + * + * note that .state() != skp1_ext; .state() reports {x(k), P(k)} + */ + MatrixXd gain(ref::rp const & skp1_ext) const; + + /* compute kalman gain vector K(k+1) + * given extrapolated t(k+1) state skp1_ext = {x(k+1|k), P(k+1|k)}, + * on behalf of a single observation z[j]. + * actual observation z[j] is not given here, + * just computing the gain vector. i'th member of gain vector + * gives effect of innovation on i'th member of kalman filter state. + */ + VectorXd gain1(ref::rp const & skp1_ext, + uint32_t j) const; + + /* compute correction to extrapolated filter state {x(k+1|k), P(k+1|k)}, + * for observation z(k+1) = .input.z() + */ + ref::rp correct(ref::rp const & skp1_ext); + + /* compute correction to extrapolated filter state skp1_ext = {x(k+1|k), P(k+1|k)}, + * for a single observation z(k+1, j) = .input.z()[j] + */ + ref::rp correct1(ref::rp const & skp1_ext, + uint32_t j); + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + /* system state: timestamp, estimated process state, process covariance + * asof beginning of this step + */ + ref::rp state_; + /* input: observations at time t(k+1) */ + KalmanFilterInputPtr input_; + }; /*KalmanFilterStep*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterStep const & x) { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterStep.hpp */ diff --git a/KalmanFilterSvc.cpp b/KalmanFilterSvc.cpp new file mode 100644 index 00000000..5e32b102 --- /dev/null +++ b/KalmanFilterSvc.cpp @@ -0,0 +1,44 @@ +/* @file KalmanFilterSvc.cpp */ + +#include "KalmanFilterSvc.hpp" + +namespace xo { + using xo::ref::rp; + using xo::scope; + using xo::xtag; + + namespace kalman { + rp + KalmanFilterSvc::make(KalmanFilterSpec spec) + { + return new KalmanFilterSvc(std::move(spec)); + } /*make*/ + + KalmanFilterSvc::KalmanFilterSvc(KalmanFilterSpec spec) + : filter_{std::move(spec)} + {} + + void + KalmanFilterSvc::notify_ev(ref::rp const & input_kp1) + { + this->filter_.notify_input(input_kp1); + + ++(this->n_in_ev_); + this->notify_secondary_event(this->filter_.state_ext()); + } /*notify_input*/ + + void + KalmanFilterSvc::display(std::ostream & os) const + { + os << "name()) + << xtag("n_in_ev", this->n_in_ev()) + << xtag("n_queued_out_ev", this->n_queued_out_ev()) + << xtag("n_out_ev", this->n_out_ev()) + //<< xtag("filter", this->filter_) + << ">"; + } /*display*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterSvc.cpp */ diff --git a/KalmanFilterSvc.hpp b/KalmanFilterSvc.hpp new file mode 100644 index 00000000..33d25514 --- /dev/null +++ b/KalmanFilterSvc.hpp @@ -0,0 +1,64 @@ +/* @file KalmanFilterSvc.hpp */ + +#include "reactor/Sink.hpp" +#include "reactor/DirectSourcePtr.hpp" +#include "filter/KalmanFilter.hpp" +#include "filter/KalmanFilterInputSource.hpp" +#include "filter/KalmanFilterOutputCallback.hpp" +#include "callback/CallbackSet.hpp" + +namespace xo { + namespace kalman { + /* encapsulate a passive KalmanFilter + * instance as an active event consumer+producer + * + * sinks that want to consume KalmanFilterSvc events will use + * .attach_sink() (or .add_callback()) + */ + class KalmanFilterSvc : public xo::reactor::Sink1>, + public xo::reactor::DirectSourcePtr> { + public: + using AbstractSource = xo::reactor::AbstractSource; + + public: + /* named ctor idiom */ + static ref::rp make(KalmanFilterSpec spec); + + KalmanFilter const & filter() const { return filter_; } + + /* notify incoming observations; will trigger kalman filter step */ + void notify_ev(ref::rp const & input_kp1) override; + + // ----- inherited from reactor::AbstractSink ----- + + /* filter captures KF input pointer */ + virtual bool allow_volatile_source() const override { return false; } + virtual uint32_t n_in_ev() const override { return n_in_ev_; } + virtual void display(std::ostream & os) const override; + + // ----- inherited from reactor::AbstractSource ----- + + /* note: correct since KalmanFilterEngine.extrapolate() + * always creates new state object + */ + virtual bool is_volatile() const override { return false; } + + // ----- Inherited from AbstractEventProcessor ----- + + private: + KalmanFilterSvc(KalmanFilterSpec spec); + + private: + /* passive kalman filter */ + KalmanFilter filter_; + /* receive filter input from this source; see .attach_input() */ + ref::rp input_src_; + /* counts lifetime #of input events (see .notify_ev()) */ + uint32_t n_in_ev_ = 0; + /* publish filter state updates to these callbacks */ + fn::RpCallbackSet pub_; + }; /*KalmanFilterSvc*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* KalmanFilterSvc.hpp */ diff --git a/KalmanFilterTransition.cpp b/KalmanFilterTransition.cpp new file mode 100644 index 00000000..65be627b --- /dev/null +++ b/KalmanFilterTransition.cpp @@ -0,0 +1,55 @@ +/* @file KalmanFilterTransition.cpp */ + +#include "KalmanFilterTransition.hpp" +#include "reflect/StructReflector.hpp" +#include "print_eigen.hpp" +#include "indentlog/scope.hpp" + +namespace xo { + using xo::reflect::StructReflector; + using logutil::matrix; + using xo::xtag; + + namespace kalman { + void + KalmanFilterTransition::reflect_self() + { + StructReflector sr; + + if (sr.is_incomplete()) { + REFLECT_MEMBER(sr, F); + REFLECT_MEMBER(sr, Q); + } + } /*reflect_self*/ + + uint32_t + KalmanFilterTransition::n_state() const + { + /* we know F.rows() == F.cols() = Q.cols() == Q.rows(), + * see .check_ok() + */ + + return F_.rows(); + } /*n_state*/ + + void + KalmanFilterTransition::display(std::ostream & os) const + { + os << ""; + } /*display*/ + + std::string + KalmanFilterTransition::display_string() const + { + std::stringstream ss; + this->display(ss); + return ss.str(); + } /*display_string*/ + + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterTransition.cpp */ diff --git a/KalmanFilterTransition.hpp b/KalmanFilterTransition.hpp new file mode 100644 index 00000000..fc777c00 --- /dev/null +++ b/KalmanFilterTransition.hpp @@ -0,0 +1,62 @@ +/* @file KalmanFilterTransition.hpp */ + +#pragma once + +#include "time/Time.hpp" +#include +#include + +namespace xo { + namespace kalman { + + /* encapsulate transition behavior for a kalman filter + * before taking observations into account + */ + class KalmanFilterTransition { + public: + using MatrixXd = Eigen::MatrixXd; + using uint32_t = std::uint32_t; + + public: + KalmanFilterTransition() = default; + KalmanFilterTransition(MatrixXd F, + MatrixXd Q) + : F_{std::move(F)}, Q_{std::move(Q)} { assert(this->check_ok()); } + + static void reflect_self(); + + /* n: cardinality of state vector */ + uint32_t n_state() const; + + MatrixXd const & transition_mat() const { return F_; } + MatrixXd const & transition_cov() const { return Q_; } + + bool check_ok() const { + uint32_t n = F_.rows(); + bool f_is_nxn = ((F_.rows() == n) && (F_.cols() == n)); + bool q_is_nxn = ((Q_.rows() == n) && (Q_.cols() == n)); + + /* also would like to require: Q is +ve definite */ + + return f_is_nxn && q_is_nxn; + } /*check_ok*/ + + void display(std::ostream & os) const; + std::string display_string() const; + + private: + /* [n x n] state transition matrix */ + MatrixXd F_; + /* [n x n] covariance matrix for system noise */ + MatrixXd Q_; + }; /*KalmanFilterTransition*/ + + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterTransition const & x) { + x.display(os); + return os; + } /*operator<<*/ + } /*namespace kalman*/ +} /*namespace xo*/ + +/* end KalmanFilterTransition.hpp */ diff --git a/init_filter.cpp b/init_filter.cpp new file mode 100644 index 00000000..80407cc4 --- /dev/null +++ b/init_filter.cpp @@ -0,0 +1,51 @@ +/* file init_filter.cpp + * + * author: Roland Conybeare, Aug 2022 + */ + +#include "init_filter.hpp" +#include "reactor/init_reactor.hpp" +#include "KalmanFilterState.hpp" +#include "EigenUtil.hpp" +#include "printjson/PrintJson.hpp" + +namespace xo { + using xo::kalman::KalmanFilterInput; + using xo::kalman::KalmanFilterTransition; + using xo::kalman::KalmanFilterState; + using xo::kalman::KalmanFilterStateExt; + using xo::eigen::EigenUtil; + using xo::json::PrintJsonSingleton; + using xo::json::PrintJson; + + void + InitSubsys::init() + { + PrintJson * pjson = PrintJsonSingleton::instance().get(); + + EigenUtil::reflect_eigen(); + EigenUtil::provide_json_printers(pjson); + + KalmanFilterInput::reflect_self(); + KalmanFilterTransition::reflect_self(); + KalmanFilterState::reflect_self(); + KalmanFilterStateExt::reflect_self(); + + } /*init*/ + + InitEvidence + InitSubsys::require() + { + InitEvidence retval; + + /* subsystem dependencies for filter/ */ + retval ^= InitSubsys::require(); + + /* filter/'s own initialization code */ + retval ^= Subsystem::provide("filter", &init); + + return retval; + } /*require*/ +} /*namespace xo*/ + +/* end init_filter.cpp */ diff --git a/init_filter.hpp b/init_filter.hpp new file mode 100644 index 00000000..79cfc6d0 --- /dev/null +++ b/init_filter.hpp @@ -0,0 +1,20 @@ +/* file init_filter.hpp + * + * author: Roland Conybeare, Aug 2022 + */ + +#pragma once + +#include "subsys/Subsystem.hpp" + +namespace xo { + enum S_filter_tag {}; + + template<> + struct InitSubsys { + static void init(); + static InitEvidence require(); + }; +} /*namespace xo*/ + +/* end init_filter.hpp */ diff --git a/print_eigen.hpp b/print_eigen.hpp new file mode 100644 index 00000000..953c5904 --- /dev/null +++ b/print_eigen.hpp @@ -0,0 +1,41 @@ +/* @file print_eigen.hpp */ + +#include +#include + +namespace logutil { + template + class matrix { + public: + matrix(T x) : x_{std::move(x)} {} + + /* print this value */ + T x_; + }; /*matrix*/ + + template + using vector = matrix; + + template + inline std::ostream & + operator<<(std::ostream & s, matrix const & mat) + { + s << "["; + for(std::uint32_t i = 0, m = mat.x_.rows(); i 0) + s << "; "; + + for(std::uint32_t j = 0, n = mat.x_.cols(); j 0) + s << ' '; + + s << mat.x_(i, j); + } + } + s << "]"; + + return s; + } /*operator<<*/ +} /*namespace logutil*/ + +/* end print_eigen.hpp */