roland/xo-expression2
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

initial implementation

Browse source
This commit is contained in:
Roland Conybeare 2023-10-23 11:11:56 -04:00
commit e1f39b7b0d
60 changed files with 3104 additions and 2261 deletions
Download patch file Download diff file Expand all files Collapse all files

47
CMakeLists.txt Normal file
View file

@ -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

157
EigenUtil.cpp
View file

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

78
KalmanFilter.cpp
View file

@ -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<KalmanFilterInput> const & input_kp1)
{
scope log(XO_ENTER0(info));
/* on entry:
* .state_ext refers to t(k)
* on exit:
* .step refers to t(k+1)
* .state_ext refers to t(k+1)
*/
log && log(xtag("step_dt",
input_kp1->tkp1() - this->state_ext_->tm()));
/* establish step inputs for this filter step:
* F(k+1) (system transition matrix)
* Q(k+1) (system noise covariance matrix)
* H(k+1) (observation coupling matrix)
* R(k+1) (observation noise covariance matrix)
* z(k+1) (observation vector)
*/
this->step_ = this->filter_spec_.make_step(this->state_ext_, input_kp1);
//if (lscope.enabled()) { lscope.log(xtag("step", this->step_)); }
/* extrapolate filter state to t(k+1),
* and correct based on z(k+1)
*/
this->state_ext_ = KalmanFilterEngine::step(this->step_);
//if (lscope.enabled()) { lscope.log(xtag("state_ext", this->state_ext_)); }
} /*notify_input*/
void
KalmanFilter::display(std::ostream & os) const
{
os << "<KalmanFilter";
os << xtag("filter_spec", filter_spec_);
//os << xtag("step", step_);
os << xtag("state_ext", state_ext_);
os << ">";
} /*display*/
std::string
KalmanFilter::display_string() const
{
return tostr(*this);
} /*display_string*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilter.cpp */

128
KalmanFilter.hpp
View file

@ -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<KalmanFilterStateExt> const & state_ext() const { return state_ext_; }
/* notify kalman filter with input for time t(k+1) = input_kp1.tkp1()
* Require: input.tkp1() >= .current_tm()
* Promise:
* - .tm() = input_kp1.tkp1()
* - .step_no() = old .step_no() + 1
* - .filter_spec_k, .step_k, .state_k updated
* for observations in input_kp1
*/
void notify_input(ref::rp<KalmanFilterInput> const & input_kp1);
void display(std::ostream & os) const;
std::string display_string() const;
private:
/* specification for kalman filter;
* produces process/observation matrices on demand
*/
KalmanFilterSpec filter_spec_;
/* filter step for most recent observation */
KalmanFilterStep step_;
/* filter state as of most recent observation;
* result of applying KalmanFilterEngine::step() to contents of .step
*/
ref::rp<KalmanFilterStateExt> state_ext_;
}; /*KalmanFilter*/
inline std::ostream &
operator<<(std::ostream & os, KalmanFilter const & x) {
x.display(os);
return os;
} /*operator<<*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilter.hpp */

360
KalmanFilterEngine.cpp
View file

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

185
KalmanFilterEngine.hpp
View file

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

118
KalmanFilterInput.cpp
View file

@ -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>
KalmanFilterInput::make(utc_nanos tkp1,
VectorXb const & presence,
VectorXd const & z,
VectorXd const & Rd)
{
return new KalmanFilterInput(tkp1, presence, z, Rd);
} /*make*/
ref::rp<KalmanFilterInput>
KalmanFilterInput::make_present(utc_nanos tkp1,
VectorXd const & z)
{
VectorXb presence = VectorXb::Ones(z.cols());
return new KalmanFilterInput(tkp1,
presence,
z,
VectorXd(0) /*Rd - not using*/);
} /*make*/
VectorXi
KalmanFilterInput::make_kept_index() const
{
scope log(XO_DEBUG(false /*!debug_flag*/));
log && log(xtag("presence", matrix(presence_)));
/* count truth values */
uint32_t mstar = 0;
for (uint32_t j = 0, m = this->presence_.rows(); j<m; ++j) {
if (this->presence_[j])
++mstar;
}
log && log(xtag("m*", mstar));
VectorXi keep(mstar);
/* 2nd pass, populate keep[] */
uint32_t jstar = 0;
for (uint32_t j = 0, m = this->presence_.rows(); j<m; ++j) {
if (this->presence_[j]) {
keep[jstar] = j;
++jstar;
}
}
log && log("keep", matrix(keep));
return keep;
} /*make_kept_index*/
void
KalmanFilterInput::reflect_self()
{
StructReflector<KalmanFilterInput> sr;
if (sr.is_incomplete()) {
REFLECT_MEMBER(sr, tkp1);
REFLECT_MEMBER(sr, presence);
REFLECT_MEMBER(sr, z);
REFLECT_MEMBER(sr, Rd);
}
} /*reflect_self*/
reflect::TaggedRcptr
KalmanFilterInput::self_tp()
{
return Reflect::make_rctp(this);
} /*self_tp*/
void
KalmanFilterInput::display(std::ostream & os) const
{
os << "<KalmanFilterInput"
<< xtag("tkp1", tkp1_)
<< xtag("z", matrix(z_))
<< xtag("presence", matrix(presence_))
<< xtag("Rd", matrix(Rd_))
<< ">";
} /*display*/
std::string
KalmanFilterInput::display_string() const
{
std::stringstream ss;
this->display(ss);
return ss.str();
} /*display_string*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterInput.cpp */

121
KalmanFilterInput.hpp
View file

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

14
KalmanFilterInputCallback.hpp
View file

@ -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<ref::rp<KalmanFilterInput>>;
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterInputCallback.hpp */

28
KalmanFilterInputSource.hpp
View file

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

30
KalmanFilterInputToConsole.hpp
View file

@ -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<ref::rp<KalmanFilterInput>>
{
public:
KalmanFilterInputToConsole() = default;
static ref::rp<KalmanFilterInputToConsole> make();
virtual void display(std::ostream & os) const;
//virtual std::string display_string() const;
}; /*KalmanFilterInputToConsole*/
inline std::ostream &
operator<<(std::ostream & os, KalmanFilterInputToConsole const & x) {
x.display(os);
return os;
} /*operator<<*/
} /*namespace option*/
} /*namespace xo*/
/* end KalmanFilterInputToConsole.hpp */

110
KalmanFilterObservable.hpp
View file

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

15
KalmanFilterOutputCallback.hpp
View file

@ -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<ref::rp<KalmanFilterStateExt>>;
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterOutputCallback.hpp */

27
KalmanFilterSpec.cpp
View file

@ -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 << "<KalmanFilterSpec"
<< xtag("start_ext", start_ext_)
<< ">";
} /*display*/
std::string
KalmanFilterSpec::display_string() const
{
return tostr(*this);
} /*display_string*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterSpec.cpp */

86
KalmanFilterSpec.hpp
View file

@ -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<KalmanFilterStep
(ref::rp<KalmanFilterState> const & sk,
KalmanFilterInputPtr const & zkp1)>;
public:
explicit KalmanFilterSpec(ref::rp<KalmanFilterStateExt> s0, MkStepFn mkstepfn)
: start_ext_{std::move(s0)},
mk_step_fn_{std::move(mkstepfn)} {}
ref::rp<KalmanFilterStateExt> const & start_ext() const { return start_ext_; }
/* get step parameters (i.e. matrices F, Q, H, R)
* for step t(k) -> t(k+1).
*
* We supply t(k) state s and t(k+1) observation z(k+1):
* - to allow time stepping to be observation-driven
* - to allow for selective outlier removal
*/
KalmanFilterStep make_step(ref::rp<KalmanFilterState> const & sk,
ref::rp<KalmanFilterInput> const & zkp1) {
return this->mk_step_fn_(sk, zkp1);
} /*make_step*/
void display(std::ostream & os) const;
std::string display_string() const;
private:
/* starting state */
ref::rp<KalmanFilterStateExt> start_ext_;
/* creates kalman filter step object on demand;
* step object specifies inputs to 1 step in discrete
* linear kalman filter
*/
MkStepFn mk_step_fn_;
}; /*KalmanFilterSpec*/
inline std::ostream &
operator<<(std::ostream & os, KalmanFilterSpec const & x) {
x.display(os);
return os;
} /*operator<<*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterSpec.hpp */

231
KalmanFilterState.cpp
View file

@ -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 <ostream>
#include <string>
namespace xo {
using xo::reflect::Reflect;
using xo::reflect::TaggedRcptr;
using xo::reflect::StructReflector;
using xo::time::utc_nanos;
using xo::ref::rp;
using logutil::matrix;
using logutil::vector;
//using xo::scope;
using xo::xtag;
using xo::tostr;
//using Eigen::LDLT;
using Eigen::MatrixXd;
using Eigen::VectorXd;
namespace kalman {
// ----- KalmanFilterState -----
rp<KalmanFilterState>
KalmanFilterState::make()
{
return new KalmanFilterState();
} /*make*/
rp<KalmanFilterState>
KalmanFilterState::make(uint32_t k,
utc_nanos tk,
VectorXd x,
MatrixXd P,
KalmanFilterTransition transition)
{
return new KalmanFilterState(k, tk,
std::move(x),
std::move(P),
std::move(transition));
} /*make*/
void
KalmanFilterState::reflect_self()
{
StructReflector<KalmanFilterState> sr;
if (sr.is_incomplete()) {
REFLECT_MEMBER(sr, k);
REFLECT_MEMBER(sr, tk);
REFLECT_MEMBER(sr, x);
REFLECT_MEMBER(sr, P);
}
} /*reflect_self*/
KalmanFilterState::KalmanFilterState() = default;
KalmanFilterState::KalmanFilterState(uint32_t k,
utc_nanos tk,
VectorXd x,
MatrixXd P,
KalmanFilterTransition transition)
: k_{k}, tk_{tk},
x_{std::move(x)}, P_{std::move(P)},
transition_{std::move(transition)}
{}
TaggedRcptr
KalmanFilterState::self_tp()
{
return Reflect::make_rctp(this);
} /*self_tp*/
// ----- KalmanFilterExt -----
rp<KalmanFilterStateExt>
KalmanFilterStateExt::make()
{
return new KalmanFilterStateExt();
} /*make*/
rp<KalmanFilterStateExt>
KalmanFilterStateExt::make(uint32_t k,
utc_nanos tk,
VectorXd x,
MatrixXd P,
KalmanFilterTransition transition,
MatrixXd K,
int32_t j,
rp<KalmanFilterInput> zk)
{
return new KalmanFilterStateExt(k,
tk,
std::move(x),
std::move(P),
std::move(transition),
std::move(K),
j,
std::move(zk));
} /*make*/
void
KalmanFilterStateExt::reflect_self()
{
StructReflector<KalmanFilterStateExt> sr;
if (sr.is_incomplete()) {
/* TODO: use sr.adopt_ancestors<KalmanFilterState>() */
REFLECT_EXPLICIT_MEMBER(sr, "k", &KalmanFilterState::k_);
REFLECT_EXPLICIT_MEMBER(sr, "tk", &KalmanFilterState::tk_);
REFLECT_EXPLICIT_MEMBER(sr, "x", &KalmanFilterState::x_);
REFLECT_EXPLICIT_MEMBER(sr, "P", &KalmanFilterState::P_);
REFLECT_EXPLICIT_MEMBER(sr, "transition", &KalmanFilterState::transition_);
REFLECT_MEMBER(sr, j);
REFLECT_MEMBER(sr, K);
REFLECT_MEMBER(sr, zk);
}
} /*reflect_self*/
KalmanFilterStateExt::KalmanFilterStateExt(uint32_t k,
utc_nanos tk,
VectorXd x,
MatrixXd P,
KalmanFilterTransition transition,
MatrixXd K,
int32_t j,
rp<KalmanFilterInput> zk)
: KalmanFilterState(k, tk,
std::move(x),
std::move(P),
std::move(transition)),
j_{j},
K_{std::move(K)},
zk_{std::move(zk)}
{
uint32_t n = x.size();
if (n != P.rows() || n != P.cols()) {
std::string err_msg
= tostr("with n=x.size expect [n x n] covar matrix P",
xtag("n", x.size()),
xtag("P.rows", P.rows()),
xtag("P.cols", P.cols()));
throw std::runtime_error(err_msg);
}
if ((K.rows() > 0) && (K.rows() > 0)) {
if (n != K.rows()) {
std::string err_msg
= tostr("with n=x.size expect [m x n] gain matrix K",
xtag("n", x.size()),
xtag("K.rows", K.rows()),
xtag("K.cols", K.cols()));
throw std::runtime_error(err_msg);
}
} else {
/* bypass test with [0 x 0] matrix K;
* normal for initial filter state
*/
}
} /*ctor*/
void
KalmanFilterState::display(std::ostream & os) const
{
os << "<KalmanFilterState"
<< xtag("k", k_)
<< xtag("tk", tk_)
<< xtag("x", matrix(x_))
<< xtag("P", matrix(P_))
<< ">";
} /*display*/
std::string
KalmanFilterState::display_string() const
{
std::stringstream ss;
ss << *this;
return ss.str();
} /*display_string*/
// ----- KalmanFilterStateExt -----
ref::rp<KalmanFilterStateExt>
KalmanFilterStateExt::initial(utc_nanos t0,
VectorXd x0,
MatrixXd P0)
{
return KalmanFilterStateExt::make
(0 /*k*/,
t0,
std::move(x0),
std::move(P0),
KalmanFilterTransition(MatrixXd() /*F - not used for initial step*/,
MatrixXd() /*Q - not used for initial step*/),
MatrixXd() /*K - not used for initial step*/,
-1 /*j - not used for initial step*/,
nullptr /*zk - not defined for initial step*/);
} /*initial*/
void
KalmanFilterStateExt::display(std::ostream & os) const
{
os << "<KalmanFilterStateExt"
<< xtag("k", this->step_no())
<< xtag("tk", this->tm())
<< xtag("x", matrix(this->state_v()))
<< xtag("P", matrix(this->state_cov()))
<< xtag("K", matrix(K_))
<< xtag("j", j_)
<< ">";
} /*display*/
TaggedRcptr
KalmanFilterStateExt::self_tp()
{
return Reflect::make_rctp(this);
} /*self_tp*/
} /*namespace filter*/
} /*namespace xo*/
/* end KalmanFilterState.cpp */

163
KalmanFilterState.hpp
View file

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

30
KalmanFilterStateToConsole.hpp
View file

@ -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<KalmanFilterStateExt>
{
public:
KalmanFilterStateToConsole() = default;
static ref::rp<KalmanFilterStateToConsole> make();
virtual void display(std::ostream & os) const;
//virtual std::string display_string() const;
}; /*KalmanFilterStateToConsole*/
inline std::ostream &
operator<<(std::ostream & os, KalmanFilterStateToConsole const & x) {
x.display(os);
return os;
} /*operator<<*/
} /*namespace option*/
} /*namespace xo*/
/* end KalmanFilterStateToConsole.hpp */

84
KalmanFilterStep.cpp
View file

@ -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<KalmanFilterState>
KalmanFilterStep::extrapolate() const
{
return KalmanFilterEngine::extrapolate(this->tkp1(),
this->state(),
this->model() /*transition*/);
} /*extrapolate*/
MatrixXd
KalmanFilterStep::gain(ref::rp<KalmanFilterState> const & skp1_ext) const
{
return KalmanFilterEngine::kalman_gain(skp1_ext,
this->obs());
} /*gain*/
VectorXd
KalmanFilterStep::gain1(ref::rp<KalmanFilterState> const & skp1_ext,
uint32_t j) const
{
return KalmanFilterEngine::kalman_gain1(skp1_ext,
this->obs(),
j);
} /*gain1*/
ref::rp<KalmanFilterStateExt>
KalmanFilterStep::correct(ref::rp<KalmanFilterState> const & skp1_ext)
{
return KalmanFilterEngine::correct(skp1_ext,
this->obs(),
this->input());
} /*correct*/
ref::rp<KalmanFilterStateExt>
KalmanFilterStep::correct1(ref::rp<KalmanFilterState> const & skp1_ext,
uint32_t j)
{
return KalmanFilterEngine::correct1(skp1_ext,
this->obs(),
this->input(),
j);
} /*correct1*/
void
KalmanFilterStep::display(std::ostream & os) const
{
//scope lscope("KalmanFilterStep::display");
os << "<KalmanFilterStep";
//lscope.log("state:");
os << xtag("state", state_);
//lscope.log("model:");
os << xtag("model", this->model());
//lscope.log("obs:");
os << xtag("obs", this->obs());
//lscope.log("input:");
os << xtag("input", this->input());
os << ">";
} /*display*/
std::string
KalmanFilterStep::display_string() const
{
return tostr(*this);
} /*display_string*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterStep.cpp */

64
KalmanFilterSvc.hpp
View file

@ -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<ref::rp<KalmanFilterInput>>,
public xo::reactor::DirectSourcePtr<ref::rp<KalmanFilterStateExt>> {
public:
using AbstractSource = xo::reactor::AbstractSource;
public:
/* named ctor idiom */
static ref::rp<KalmanFilterSvc> make(KalmanFilterSpec spec);
KalmanFilter const & filter() const { return filter_; }
/* notify incoming observations; will trigger kalman filter step */
void notify_ev(ref::rp<KalmanFilterInput> const & input_kp1) override;
// ----- inherited from reactor::AbstractSink -----
/* filter captures KF input pointer */
virtual bool allow_volatile_source() const override { return false; }
virtual uint32_t n_in_ev() const override { return n_in_ev_; }
virtual void display(std::ostream & os) const override;
// ----- inherited from reactor::AbstractSource -----
/* note: correct since KalmanFilterEngine.extrapolate()
* always creates new state object
*/
virtual bool is_volatile() const override { return false; }
// ----- Inherited from AbstractEventProcessor -----
private:
KalmanFilterSvc(KalmanFilterSpec spec);
private:
/* passive kalman filter */
KalmanFilter filter_;
/* receive filter input from this source; see .attach_input() */
ref::rp<KalmanFilterInputSource> input_src_;
/* counts lifetime #of input events (see .notify_ev()) */
uint32_t n_in_ev_ = 0;
/* publish filter state updates to these callbacks */
fn::RpCallbackSet<KalmanFilterOutputCallback> pub_;
}; /*KalmanFilterSvc*/
} /*namespace kalman*/
} /*namespace xo*/
/* KalmanFilterSvc.hpp */

55
KalmanFilterTransition.cpp
View file

@ -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<KalmanFilterTransition> sr;
if (sr.is_incomplete()) {
REFLECT_MEMBER(sr, F);
REFLECT_MEMBER(sr, Q);
}
} /*reflect_self*/
uint32_t
KalmanFilterTransition::n_state() const
{
/* we know F.rows() == F.cols() = Q.cols() == Q.rows(),
* see .check_ok()
*/
return F_.rows();
} /*n_state*/
void
KalmanFilterTransition::display(std::ostream & os) const
{
os << "<KalmanFilterTransition"
<< xtag("F", matrix(F_))
<< xtag("Q", matrix(Q_))
<< ">";
} /*display*/
std::string
KalmanFilterTransition::display_string() const
{
std::stringstream ss;
this->display(ss);
return ss.str();
} /*display_string*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterTransition.cpp */

62
KalmanFilterTransition.hpp
View file

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

17
cmake/xo_kalmanfilterConfig.cmake.in Normal file
View file

@ -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@")

0
EigenUtil.hpp → include/xo/kalmanfilter/EigenUtil.hpp
View file

127
include/xo/kalmanfilter/KalmanFilter.hpp Normal file
View file

@ -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<KalmanFilterStateExt> const & state_ext() const { return state_ext_; }
/* notify kalman filter with input for time t(k+1) = input_kp1.tkp1()
* Require: input.tkp1() >= .current_tm()
* Promise:
* - .tm() = input_kp1.tkp1()
* - .step_no() = old .step_no() + 1
* - .filter_spec_k, .step_k, .state_k updated
* for observations in input_kp1
*/
void notify_input(ref::rp<KalmanFilterInput> const & input_kp1);
void display(std::ostream & os) const;
std::string display_string() const;
private:
/* specification for kalman filter;
* produces process/observation matrices on demand
*/
KalmanFilterSpec filter_spec_;
/* filter step for most recent observation */
KalmanFilterStep step_;
/* filter state as of most recent observation;
* result of applying KalmanFilterEngine::step() to contents of .step
*/
ref::rp<KalmanFilterStateExt> state_ext_;
}; /*KalmanFilter*/
inline std::ostream &
operator<<(std::ostream & os, KalmanFilter const & x) {
x.display(os);
return os;
} /*operator<<*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilter.hpp */

185
include/xo/kalmanfilter/KalmanFilterEngine.hpp Normal file
View file

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

121
include/xo/kalmanfilter/KalmanFilterInput.hpp Normal file
View file

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

14
include/xo/kalmanfilter/KalmanFilterInputCallback.hpp Normal file
View file

@ -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<ref::rp<KalmanFilterInput>>;
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterInputCallback.hpp */

27
include/xo/kalmanfilter/KalmanFilterInputSource.hpp Normal file
View file

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

30
include/xo/kalmanfilter/KalmanFilterInputToConsole.hpp Normal file
View file

@ -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<ref::rp<KalmanFilterInput>>
{
public:
KalmanFilterInputToConsole() = default;
static ref::rp<KalmanFilterInputToConsole> make();
virtual void display(std::ostream & os) const;
//virtual std::string display_string() const;
}; /*KalmanFilterInputToConsole*/
inline std::ostream &
operator<<(std::ostream & os, KalmanFilterInputToConsole const & x) {
x.display(os);
return os;
} /*operator<<*/
} /*namespace option*/
} /*namespace xo*/
/* end KalmanFilterInputToConsole.hpp */

109
include/xo/kalmanfilter/KalmanFilterObservable.hpp Normal file
View file

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

15
include/xo/kalmanfilter/KalmanFilterOutputCallback.hpp Normal file
View file

@ -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<ref::rp<KalmanFilterStateExt>>;
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterOutputCallback.hpp */

86
include/xo/kalmanfilter/KalmanFilterSpec.hpp Normal file
View file

@ -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<KalmanFilterStep
(ref::rp<KalmanFilterState> const & sk,
KalmanFilterInputPtr const & zkp1)>;
public:
explicit KalmanFilterSpec(ref::rp<KalmanFilterStateExt> s0, MkStepFn mkstepfn)
: start_ext_{std::move(s0)},
mk_step_fn_{std::move(mkstepfn)} {}
ref::rp<KalmanFilterStateExt> const & start_ext() const { return start_ext_; }
/* get step parameters (i.e. matrices F, Q, H, R)
* for step t(k) -> t(k+1).
*
* We supply t(k) state s and t(k+1) observation z(k+1):
* - to allow time stepping to be observation-driven
* - to allow for selective outlier removal
*/
KalmanFilterStep make_step(ref::rp<KalmanFilterState> const & sk,
ref::rp<KalmanFilterInput> const & zkp1) {
return this->mk_step_fn_(sk, zkp1);
} /*make_step*/
void display(std::ostream & os) const;
std::string display_string() const;
private:
/* starting state */
ref::rp<KalmanFilterStateExt> start_ext_;
/* creates kalman filter step object on demand;
* step object specifies inputs to 1 step in discrete
* linear kalman filter
*/
MkStepFn mk_step_fn_;
}; /*KalmanFilterSpec*/
inline std::ostream &
operator<<(std::ostream & os, KalmanFilterSpec const & x) {
x.display(os);
return os;
} /*operator<<*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterSpec.hpp */

163
include/xo/kalmanfilter/KalmanFilterState.hpp Normal file
View file

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

30
include/xo/kalmanfilter/KalmanFilterStateToConsole.hpp Normal file
View file

@ -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<KalmanFilterStateExt>
{
public:
KalmanFilterStateToConsole() = default;
static ref::rp<KalmanFilterStateToConsole> make();
virtual void display(std::ostream & os) const;
//virtual std::string display_string() const;
}; /*KalmanFilterStateToConsole*/
inline std::ostream &
operator<<(std::ostream & os, KalmanFilterStateToConsole const & x) {
x.display(os);
return os;
} /*operator<<*/
} /*namespace option*/
} /*namespace xo*/
/* end KalmanFilterStateToConsole.hpp */

0
KalmanFilterStep.hpp → include/xo/kalmanfilter/KalmanFilterStep.hpp
View file

64
include/xo/kalmanfilter/KalmanFilterSvc.hpp Normal file
View file

@ -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<ref::rp<KalmanFilterInput>>,
public xo::reactor::DirectSourcePtr<ref::rp<KalmanFilterStateExt>> {
public:
using AbstractSource = xo::reactor::AbstractSource;
public:
/* named ctor idiom */
static ref::rp<KalmanFilterSvc> make(KalmanFilterSpec spec);
KalmanFilter const & filter() const { return filter_; }
/* notify incoming observations; will trigger kalman filter step */
void notify_ev(ref::rp<KalmanFilterInput> const & input_kp1) override;
// ----- inherited from reactor::AbstractSink -----
/* filter captures KF input pointer */
virtual bool allow_volatile_source() const override { return false; }
virtual uint32_t n_in_ev() const override { return n_in_ev_; }
virtual void display(std::ostream & os) const override;
// ----- inherited from reactor::AbstractSource -----
/* note: correct since KalmanFilterEngine.extrapolate()
* always creates new state object
*/
virtual bool is_volatile() const override { return false; }
// ----- Inherited from AbstractEventProcessor -----
private:
KalmanFilterSvc(KalmanFilterSpec spec);
private:
/* passive kalman filter */
KalmanFilter filter_;
/* receive filter input from this source; see .attach_input() */
ref::rp<KalmanFilterInputSource> input_src_;
/* counts lifetime #of input events (see .notify_ev()) */
uint32_t n_in_ev_ = 0;
/* publish filter state updates to these callbacks */
fn::RpCallbackSet<KalmanFilterOutputCallback> pub_;
}; /*KalmanFilterSvc*/
} /*namespace kalman*/
} /*namespace xo*/
/* KalmanFilterSvc.hpp */

62
include/xo/kalmanfilter/KalmanFilterTransition.hpp Normal file
View file

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

20
include/xo/kalmanfilter/init_filter.hpp Normal file
View file

@ -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<S_kalmanfilter_tag> {
static void init();
static InitEvidence require();
};
} /*namespace xo*/
/* end init_kalmanfilter.hpp */

41
include/xo/kalmanfilter/print_eigen.hpp Normal file
View file

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

51
init_filter.cpp
View file

@ -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<S_filter_tag>::init()
{
PrintJson * pjson = PrintJsonSingleton::instance().get();
EigenUtil::reflect_eigen();
EigenUtil::provide_json_printers(pjson);
KalmanFilterInput::reflect_self();
KalmanFilterTransition::reflect_self();
KalmanFilterState::reflect_self();
KalmanFilterStateExt::reflect_self();
} /*init*/
InitEvidence
InitSubsys<S_filter_tag>::require()
{
InitEvidence retval;
/* subsystem dependencies for filter/ */
retval ^= InitSubsys<S_reactor_tag>::require();
/* filter/'s own initialization code */
retval ^= Subsystem::provide<S_filter_tag>("filter", &init);
return retval;
} /*require*/
} /*namespace xo*/
/* end init_filter.cpp */

20
init_filter.hpp
View file

@ -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<S_filter_tag> {
static void init();
static InitEvidence require();
};
} /*namespace xo*/
/* end init_filter.hpp */

41
print_eigen.hpp
View file

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

31
src/kalmanfilter/CMakeLists.txt Normal file
View file

@ -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)

157
src/kalmanfilter/EigenUtil.cpp Normal file
View file

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

78
src/kalmanfilter/KalmanFilter.cpp Normal file
View file

@ -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<KalmanFilterInput> const & input_kp1)
{
scope log(XO_ENTER0(info));
/* on entry:
* .state_ext refers to t(k)
* on exit:
* .step refers to t(k+1)
* .state_ext refers to t(k+1)
*/
log && log(xtag("step_dt",
input_kp1->tkp1() - this->state_ext_->tm()));
/* establish step inputs for this filter step:
* F(k+1) (system transition matrix)
* Q(k+1) (system noise covariance matrix)
* H(k+1) (observation coupling matrix)
* R(k+1) (observation noise covariance matrix)
* z(k+1) (observation vector)
*/
this->step_ = this->filter_spec_.make_step(this->state_ext_, input_kp1);
//if (lscope.enabled()) { lscope.log(xtag("step", this->step_)); }
/* extrapolate filter state to t(k+1),
* and correct based on z(k+1)
*/
this->state_ext_ = KalmanFilterEngine::step(this->step_);
//if (lscope.enabled()) { lscope.log(xtag("state_ext", this->state_ext_)); }
} /*notify_input*/
void
KalmanFilter::display(std::ostream & os) const
{
os << "<KalmanFilter";
os << xtag("filter_spec", filter_spec_);
//os << xtag("step", step_);
os << xtag("state_ext", state_ext_);
os << ">";
} /*display*/
std::string
KalmanFilter::display_string() const
{
return tostr(*this);
} /*display_string*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilter.cpp */

360
src/kalmanfilter/KalmanFilterEngine.cpp Normal file
View file

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

118
src/kalmanfilter/KalmanFilterInput.cpp Normal file
View file

@ -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>
KalmanFilterInput::make(utc_nanos tkp1,
VectorXb const & presence,
VectorXd const & z,
VectorXd const & Rd)
{
return new KalmanFilterInput(tkp1, presence, z, Rd);
} /*make*/
ref::rp<KalmanFilterInput>
KalmanFilterInput::make_present(utc_nanos tkp1,
VectorXd const & z)
{
VectorXb presence = VectorXb::Ones(z.cols());
return new KalmanFilterInput(tkp1,
presence,
z,
VectorXd(0) /*Rd - not using*/);
} /*make*/
VectorXi
KalmanFilterInput::make_kept_index() const
{
scope log(XO_DEBUG(false /*!debug_flag*/));
log && log(xtag("presence", matrix(presence_)));
/* count truth values */
uint32_t mstar = 0;
for (uint32_t j = 0, m = this->presence_.rows(); j<m; ++j) {
if (this->presence_[j])
++mstar;
}
log && log(xtag("m*", mstar));
VectorXi keep(mstar);
/* 2nd pass, populate keep[] */
uint32_t jstar = 0;
for (uint32_t j = 0, m = this->presence_.rows(); j<m; ++j) {
if (this->presence_[j]) {
keep[jstar] = j;
++jstar;
}
}
log && log("keep", matrix(keep));
return keep;
} /*make_kept_index*/
void
KalmanFilterInput::reflect_self()
{
StructReflector<KalmanFilterInput> sr;
if (sr.is_incomplete()) {
REFLECT_MEMBER(sr, tkp1);
REFLECT_MEMBER(sr, presence);
REFLECT_MEMBER(sr, z);
REFLECT_MEMBER(sr, Rd);
}
} /*reflect_self*/
reflect::TaggedRcptr
KalmanFilterInput::self_tp()
{
return Reflect::make_rctp(this);
} /*self_tp*/
void
KalmanFilterInput::display(std::ostream & os) const
{
os << "<KalmanFilterInput"
<< xtag("tkp1", tkp1_)
<< xtag("z", matrix(z_))
<< xtag("presence", matrix(presence_))
<< xtag("Rd", matrix(Rd_))
<< ">";
} /*display*/
std::string
KalmanFilterInput::display_string() const
{
std::stringstream ss;
this->display(ss);
return ss.str();
} /*display_string*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterInput.cpp */

2
KalmanFilterInputToConsole.cpp → src/kalmanfilter/KalmanFilterInputToConsole.cpp
View file

@ -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;

2
KalmanFilterObservable.cpp → src/kalmanfilter/KalmanFilterObservable.cpp
View file

@ -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;

27
src/kalmanfilter/KalmanFilterSpec.cpp Normal file
View file

@ -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 << "<KalmanFilterSpec"
<< xtag("start_ext", start_ext_)
<< ">";
} /*display*/
std::string
KalmanFilterSpec::display_string() const
{
return tostr(*this);
} /*display_string*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterSpec.cpp */

231
src/kalmanfilter/KalmanFilterState.cpp Normal file
View file

@ -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 <ostream>
#include <string>
namespace xo {
using xo::reflect::Reflect;
using xo::reflect::TaggedRcptr;
using xo::reflect::StructReflector;
using xo::time::utc_nanos;
using xo::ref::rp;
using logutil::matrix;
using logutil::vector;
//using xo::scope;
using xo::xtag;
using xo::tostr;
//using Eigen::LDLT;
using Eigen::MatrixXd;
using Eigen::VectorXd;
namespace kalman {
// ----- KalmanFilterState -----
rp<KalmanFilterState>
KalmanFilterState::make()
{
return new KalmanFilterState();
} /*make*/
rp<KalmanFilterState>
KalmanFilterState::make(uint32_t k,
utc_nanos tk,
VectorXd x,
MatrixXd P,
KalmanFilterTransition transition)
{
return new KalmanFilterState(k, tk,
std::move(x),
std::move(P),
std::move(transition));
} /*make*/
void
KalmanFilterState::reflect_self()
{
StructReflector<KalmanFilterState> sr;
if (sr.is_incomplete()) {
REFLECT_MEMBER(sr, k);
REFLECT_MEMBER(sr, tk);
REFLECT_MEMBER(sr, x);
REFLECT_MEMBER(sr, P);
}
} /*reflect_self*/
KalmanFilterState::KalmanFilterState() = default;
KalmanFilterState::KalmanFilterState(uint32_t k,
utc_nanos tk,
VectorXd x,
MatrixXd P,
KalmanFilterTransition transition)
: k_{k}, tk_{tk},
x_{std::move(x)}, P_{std::move(P)},
transition_{std::move(transition)}
{}
TaggedRcptr
KalmanFilterState::self_tp()
{
return Reflect::make_rctp(this);
} /*self_tp*/
// ----- KalmanFilterExt -----
rp<KalmanFilterStateExt>
KalmanFilterStateExt::make()
{
return new KalmanFilterStateExt();
} /*make*/
rp<KalmanFilterStateExt>
KalmanFilterStateExt::make(uint32_t k,
utc_nanos tk,
VectorXd x,
MatrixXd P,
KalmanFilterTransition transition,
MatrixXd K,
int32_t j,
rp<KalmanFilterInput> zk)
{
return new KalmanFilterStateExt(k,
tk,
std::move(x),
std::move(P),
std::move(transition),
std::move(K),
j,
std::move(zk));
} /*make*/
void
KalmanFilterStateExt::reflect_self()
{
StructReflector<KalmanFilterStateExt> sr;
if (sr.is_incomplete()) {
/* TODO: use sr.adopt_ancestors<KalmanFilterState>() */
REFLECT_EXPLICIT_MEMBER(sr, "k", &KalmanFilterState::k_);
REFLECT_EXPLICIT_MEMBER(sr, "tk", &KalmanFilterState::tk_);
REFLECT_EXPLICIT_MEMBER(sr, "x", &KalmanFilterState::x_);
REFLECT_EXPLICIT_MEMBER(sr, "P", &KalmanFilterState::P_);
REFLECT_EXPLICIT_MEMBER(sr, "transition", &KalmanFilterState::transition_);
REFLECT_MEMBER(sr, j);
REFLECT_MEMBER(sr, K);
REFLECT_MEMBER(sr, zk);
}
} /*reflect_self*/
KalmanFilterStateExt::KalmanFilterStateExt(uint32_t k,
utc_nanos tk,
VectorXd x,
MatrixXd P,
KalmanFilterTransition transition,
MatrixXd K,
int32_t j,
rp<KalmanFilterInput> zk)
: KalmanFilterState(k, tk,
std::move(x),
std::move(P),
std::move(transition)),
j_{j},
K_{std::move(K)},
zk_{std::move(zk)}
{
uint32_t n = x.size();
if (n != P.rows() || n != P.cols()) {
std::string err_msg
= tostr("with n=x.size expect [n x n] covar matrix P",
xtag("n", x.size()),
xtag("P.rows", P.rows()),
xtag("P.cols", P.cols()));
throw std::runtime_error(err_msg);
}
if ((K.rows() > 0) && (K.rows() > 0)) {
if (n != K.rows()) {
std::string err_msg
= tostr("with n=x.size expect [m x n] gain matrix K",
xtag("n", x.size()),
xtag("K.rows", K.rows()),
xtag("K.cols", K.cols()));
throw std::runtime_error(err_msg);
}
} else {
/* bypass test with [0 x 0] matrix K;
* normal for initial filter state
*/
}
} /*ctor*/
void
KalmanFilterState::display(std::ostream & os) const
{
os << "<KalmanFilterState"
<< xtag("k", k_)
<< xtag("tk", tk_)
<< xtag("x", matrix(x_))
<< xtag("P", matrix(P_))
<< ">";
} /*display*/
std::string
KalmanFilterState::display_string() const
{
std::stringstream ss;
ss << *this;
return ss.str();
} /*display_string*/
// ----- KalmanFilterStateExt -----
ref::rp<KalmanFilterStateExt>
KalmanFilterStateExt::initial(utc_nanos t0,
VectorXd x0,
MatrixXd P0)
{
return KalmanFilterStateExt::make
(0 /*k*/,
t0,
std::move(x0),
std::move(P0),
KalmanFilterTransition(MatrixXd() /*F - not used for initial step*/,
MatrixXd() /*Q - not used for initial step*/),
MatrixXd() /*K - not used for initial step*/,
-1 /*j - not used for initial step*/,
nullptr /*zk - not defined for initial step*/);
} /*initial*/
void
KalmanFilterStateExt::display(std::ostream & os) const
{
os << "<KalmanFilterStateExt"
<< xtag("k", this->step_no())
<< xtag("tk", this->tm())
<< xtag("x", matrix(this->state_v()))
<< xtag("P", matrix(this->state_cov()))
<< xtag("K", matrix(K_))
<< xtag("j", j_)
<< ">";
} /*display*/
TaggedRcptr
KalmanFilterStateExt::self_tp()
{
return Reflect::make_rctp(this);
} /*self_tp*/
} /*namespace filter*/
} /*namespace xo*/
/* end KalmanFilterState.cpp */

2
KalmanFilterStateToConsole.cpp → src/kalmanfilter/KalmanFilterStateToConsole.cpp
View file

@ -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;

84
src/kalmanfilter/KalmanFilterStep.cpp Normal file
View file

@ -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<KalmanFilterState>
KalmanFilterStep::extrapolate() const
{
return KalmanFilterEngine::extrapolate(this->tkp1(),
this->state(),
this->model() /*transition*/);
} /*extrapolate*/
MatrixXd
KalmanFilterStep::gain(ref::rp<KalmanFilterState> const & skp1_ext) const
{
return KalmanFilterEngine::kalman_gain(skp1_ext,
this->obs());
} /*gain*/
VectorXd
KalmanFilterStep::gain1(ref::rp<KalmanFilterState> const & skp1_ext,
uint32_t j) const
{
return KalmanFilterEngine::kalman_gain1(skp1_ext,
this->obs(),
j);
} /*gain1*/
ref::rp<KalmanFilterStateExt>
KalmanFilterStep::correct(ref::rp<KalmanFilterState> const & skp1_ext)
{
return KalmanFilterEngine::correct(skp1_ext,
this->obs(),
this->input());
} /*correct*/
ref::rp<KalmanFilterStateExt>
KalmanFilterStep::correct1(ref::rp<KalmanFilterState> const & skp1_ext,
uint32_t j)
{
return KalmanFilterEngine::correct1(skp1_ext,
this->obs(),
this->input(),
j);
} /*correct1*/
void
KalmanFilterStep::display(std::ostream & os) const
{
//scope lscope("KalmanFilterStep::display");
os << "<KalmanFilterStep";
//lscope.log("state:");
os << xtag("state", state_);
//lscope.log("model:");
os << xtag("model", this->model());
//lscope.log("obs:");
os << xtag("obs", this->obs());
//lscope.log("input:");
os << xtag("input", this->input());
os << ">";
} /*display*/
std::string
KalmanFilterStep::display_string() const
{
return tostr(*this);
} /*display_string*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterStep.cpp */

0
KalmanFilterSvc.cpp → src/kalmanfilter/KalmanFilterSvc.cpp
View file

55
src/kalmanfilter/KalmanFilterTransition.cpp Normal file
View file

@ -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<KalmanFilterTransition> sr;
if (sr.is_incomplete()) {
REFLECT_MEMBER(sr, F);
REFLECT_MEMBER(sr, Q);
}
} /*reflect_self*/
uint32_t
KalmanFilterTransition::n_state() const
{
/* we know F.rows() == F.cols() = Q.cols() == Q.rows(),
* see .check_ok()
*/
return F_.rows();
} /*n_state*/
void
KalmanFilterTransition::display(std::ostream & os) const
{
os << "<KalmanFilterTransition"
<< xtag("F", matrix(F_))
<< xtag("Q", matrix(Q_))
<< ">";
} /*display*/
std::string
KalmanFilterTransition::display_string() const
{
std::stringstream ss;
this->display(ss);
return ss.str();
} /*display_string*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterTransition.cpp */

52
src/kalmanfilter/init_filter.cpp Normal file
View file

@ -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<S_kalmanfilter_tag>::init()
{
PrintJson * pjson = PrintJsonSingleton::instance().get();
EigenUtil::reflect_eigen();
EigenUtil::provide_json_printers(pjson);
KalmanFilterInput::reflect_self();
KalmanFilterTransition::reflect_self();
KalmanFilterState::reflect_self();
KalmanFilterStateExt::reflect_self();
} /*init*/
InitEvidence
InitSubsys<S_kalmanfilter_tag>::require()
{
InitEvidence retval;
/* subsystem dependencies for filter/ */
retval ^= InitSubsys<S_reactor_tag>::require();
/* filter/'s own initialization code */
retval ^= Subsystem::provide<S_kalmanfilter_tag>("kalmanfilter", &init);
return retval;
} /*require*/
} /*namespace xo*/
/* end init_filter.cpp */

54
utest/CMakeLists.txt Normal file
View file

@ -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

690
utest/KalmanFilter.test.cpp Normal file
View file

@ -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 <catch2/catch.hpp>
#include <fstream>
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<KalmanFilterState> const & sk,
rp<KalmanFilterInput> 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<xoshiro256ss>::make
(seed,
std::normal_distribution<double>(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<KalmanFilterStateExt> 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<KalmanFilterStateExt> 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<KalmanFilterInput> inputk
= KalmanFilterInput::make_present(tkp1, z);
KalmanFilterStep step_spec = spec.make_step(sk, inputk);
rp<KalmanFilterStateExt> 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<xoshiro256ss>::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<KalmanFilterStateExt> 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<KalmanFilterStateExt> 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<KalmanFilterInput> inputk
= KalmanFilterInput::make_present(tkp1, z);
KalmanFilterStep step_spec
= spec.make_step(sk, inputk);
rp<KalmanFilterStateExt> 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<KalmanFilterState> const & sk,
rp<KalmanFilterInput> 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<xoshiro256ss>::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<KalmanFilterStateExt> 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<KalmanFilterStateExt> 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<KalmanFilterInput> inputk
= KalmanFilterInput::make_present(tkp1, z);
KalmanFilterStep step_spec = spec.make_step(sk, inputk);
rp<KalmanFilterStateExt> 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<KalmanFilterState> const & sk,
rp<KalmanFilterInput> 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<xoshiro256ss>::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<KalmanFilterStateExt> 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<KalmanFilterStateExt> 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<KalmanFilterInput> inputk
= KalmanFilterInput::make_present(tkp1, z);
KalmanFilterStep step_spec = spec.make_step(sk, inputk);
rp<KalmanFilterStateExt> 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 */

6
utest/filter_utest_main.cpp Normal file
View file

@ -0,0 +1,6 @@
/* @file filter_utest_main.cpp */
#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"
/* end filter_utest_main.cpp */