roland/xo-reader
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

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

25
src/kalmanfilter/KalmanFilterInputToConsole.cpp Normal file
View file

@ -0,0 +1,25 @@
/* @file KalmanFilterInputToConsole.cpp */
#include "KalmanFilterInputToConsole.hpp"
#include "xo/indentlog/print/tag.hpp"
namespace xo {
using xo::xtag;
namespace kalman {
ref::rp<KalmanFilterInputToConsole>
KalmanFilterInputToConsole::make() {
return new KalmanFilterInputToConsole();
} /*make*/
void
KalmanFilterInputToConsole::display(std::ostream & os) const
{
os << "<KalmanFilterInputToConsole"
<< xtag("this", (void*)this)
<< ">";
} /*display*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterInputToConsole.cpp */

71
src/kalmanfilter/KalmanFilterObservable.cpp Normal file
View file

@ -0,0 +1,71 @@
/* @file KalmanFilterObservable.cpp */
#include "KalmanFilterObservable.hpp"
#include "print_eigen.hpp"
#include "xo/indentlog/scope.hpp"
namespace xo {
using xo::scope;
using logutil::matrix;
using xo::xtag;
namespace kalman {
KalmanFilterObservable
KalmanFilterObservable::keep_all(MatrixXd H,
MatrixXd R)
{
VectorXi keep(H.rows());
for (uint32_t j=0; j<H.rows(); ++j)
keep[j] = j;
return KalmanFilterObservable(std::move(keep),
std::move(H),
std::move(R));
} /*keep_all*/
KalmanFilterObservable
KalmanFilterObservable::reindex(VectorXi keep,
MatrixXd H,
MatrixXd R)
{
scope log(XO_DEBUG(false /*debug_flag*/));
/* Hp:
* - keep rows in H with indices that appear in keep[]
* - keep all columns of H
*/
MatrixXd Hp = H(keep, Eigen::all);
MatrixXd Rp = R(keep, keep);
if (log.enabled()) {
log(xtag("keep", matrix(keep)));
log(xtag("H", matrix(H)));
log(xtag("R", matrix(R)));
}
return KalmanFilterObservable(keep, Hp, Rp);
} /*reindex*/
void
KalmanFilterObservable::display(std::ostream & os) const
{
os << "<KalmanFilterObservable"
<< xtag("H", matrix(H_))
<< xtag("R", matrix(R_))
<< ">";
} /*display*/
std::string
KalmanFilterObservable::display_string() const
{
std::stringstream ss;
this->display(ss);
return ss.str();
} /*display_string*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterObservable.cpp */

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

25
src/kalmanfilter/KalmanFilterStateToConsole.cpp Normal file
View file

@ -0,0 +1,25 @@
/* @file KalmanFilterStateToConsole.cpp */
#include "KalmanFilterStateToConsole.hpp"
#include "xo/indentlog/print/tag.hpp"
namespace xo {
using xo::xtag;
namespace kalman {
ref::rp<KalmanFilterStateToConsole>
KalmanFilterStateToConsole::make() {
return new KalmanFilterStateToConsole();
} /*make*/
void
KalmanFilterStateToConsole::display(std::ostream & os) const
{
os << "<KalmanFilterStateToConsole"
<< xtag("this", (void*)this)
<< ">";
} /*display*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterStateToConsole.cpp */

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

44
src/kalmanfilter/KalmanFilterSvc.cpp Normal file
View file

@ -0,0 +1,44 @@
/* @file KalmanFilterSvc.cpp */
#include "KalmanFilterSvc.hpp"
namespace xo {
using xo::ref::rp;
using xo::scope;
using xo::xtag;
namespace kalman {
rp<KalmanFilterSvc>
KalmanFilterSvc::make(KalmanFilterSpec spec)
{
return new KalmanFilterSvc(std::move(spec));
} /*make*/
KalmanFilterSvc::KalmanFilterSvc(KalmanFilterSpec spec)
: filter_{std::move(spec)}
{}
void
KalmanFilterSvc::notify_ev(ref::rp<KalmanFilterInput> const & input_kp1)
{
this->filter_.notify_input(input_kp1);
++(this->n_in_ev_);
this->notify_secondary_event(this->filter_.state_ext());
} /*notify_input*/
void
KalmanFilterSvc::display(std::ostream & os) const
{
os << "<KalmanFilterSvc"
<< xtag("name", this->name())
<< xtag("n_in_ev", this->n_in_ev())
<< xtag("n_queued_out_ev", this->n_queued_out_ev())
<< xtag("n_out_ev", this->n_out_ev())
//<< xtag("filter", this->filter_)
<< ">";
} /*display*/
} /*namespace kalman*/
} /*namespace xo*/
/* end KalmanFilterSvc.cpp */

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