diff --git a/cmake/xo-bootstrap-macros.cmake b/cmake/xo-bootstrap-macros.cmake index 16644435..96592216 100644 --- a/cmake/xo-bootstrap-macros.cmake +++ b/cmake/xo-bootstrap-macros.cmake @@ -3,8 +3,10 @@ if (("${CMAKE_MODULE_PATH}" STREQUAL "") OR ("${CMAKE_MODULE_PATH}" STREQUAL "pr set(CMAKE_MODULE_PATH ${CMAKE_INSTALL_PREFIX}/share/cmake) endif() -message("-- CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}") -message("-- CMAKE_MODULE_PATH=${CMAKE_MODULE_PATH}") +if (NOT XO_SUBMODULE_BUILD) + message("-- CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}") + message("-- CMAKE_MODULE_PATH=${CMAKE_MODULE_PATH}") +endif() # needs to have been installed somewhere on CMAKE_MODULE_PATH, # (e.g. from xo-cmake with the same value for CMAKE_INSTALL_PREFIX) diff --git a/include/xo/kalmanfilter/KalmanFilterStep.hpp b/include/xo/kalmanfilter/KalmanFilterStep.hpp index 89813cc2..432ac570 100644 --- a/include/xo/kalmanfilter/KalmanFilterStep.hpp +++ b/include/xo/kalmanfilter/KalmanFilterStep.hpp @@ -8,121 +8,121 @@ #include "KalmanFilterObservable.hpp" namespace xo { - namespace kalman { - /* encapsulate {state + observation} models for a single time step t(k). - * Emitted by KalmanFilterSpec, q.v. - */ - class KalmanFilterStepBase { - public: - KalmanFilterStepBase() = default; - KalmanFilterStepBase(KalmanFilterTransition model, - KalmanFilterObservable obs) - : model_{std::move(model)}, - obs_{std::move(obs)} {} + namespace kalman { + /* encapsulate {state + observation} models for a single time step t(k). + * Emitted by KalmanFilterSpec, q.v. + */ + class KalmanFilterStepBase { + public: + KalmanFilterStepBase() = default; + KalmanFilterStepBase(KalmanFilterTransition model, + KalmanFilterObservable obs) + : model_{std::move(model)}, + obs_{std::move(obs)} {} - /* aka system_model() */ - KalmanFilterTransition const & model() const { return model_; } - KalmanFilterObservable const & obs() const { return obs_; } + /* aka system_model() */ + KalmanFilterTransition const & model() const { return model_; } + KalmanFilterObservable const & obs() const { return obs_; } - private: - /* model for process being observed (state transition + noise) */ - KalmanFilterTransition model_; - /* what can be observed (observables + noise) */ - KalmanFilterObservable obs_; - }; /*KalmanFilterStepBase*/ + private: + /* model for process being observed (state transition + noise) */ + KalmanFilterTransition model_; + /* what can be observed (observables + noise) */ + KalmanFilterObservable obs_; + }; /*KalmanFilterStepBase*/ - /* encapsulate {state + observation} models for a single time step t(k). - * Emitted by KalmanFilterSpec, q.v. - * - * holds: - * x(k) - * P(k) - * F(k) - * H(k+1) - * z(k+1) - * - * contains all the inputs needed to compute: - * x(k+1) - * P(k+1) - * - * does not provide that result - */ - class KalmanFilterStep : public KalmanFilterStepBase { - public: - using utc_nanos = xo::time::utc_nanos; - using MatrixXd = Eigen::MatrixXd; - using VectorXd = Eigen::VectorXd; + /* encapsulate {state + observation} models for a single time step t(k). + * Emitted by KalmanFilterSpec, q.v. + * + * holds: + * x(k) + * P(k) + * F(k) + * H(k+1) + * z(k+1) + * + * contains all the inputs needed to compute: + * x(k+1) + * P(k+1) + * + * does not provide that result + */ + class KalmanFilterStep : public KalmanFilterStepBase { + public: + using utc_nanos = xo::time::utc_nanos; + using MatrixXd = Eigen::MatrixXd; + using VectorXd = Eigen::VectorXd; - public: - KalmanFilterStep() = default; - KalmanFilterStep(ref::rp state, - KalmanFilterTransition model, - KalmanFilterObservable obs, - ref::rp zkp1) - : KalmanFilterStepBase(model, obs), - state_{std::move(state)}, - input_{std::move(zkp1)} {} - - ref::rp const & state() const { return state_; } - ref::rp const & input() const { return input_; } + public: + KalmanFilterStep() = default; + KalmanFilterStep(ref::rp state, + KalmanFilterTransition model, + KalmanFilterObservable obs, + ref::rp zkp1) + : KalmanFilterStepBase(model, obs), + state_{std::move(state)}, + input_{std::move(zkp1)} {} - utc_nanos tkp1() const { return input_->tkp1(); } + ref::rp const & state() const { return state_; } + ref::rp const & input() const { return input_; } - /* extrapolate kalman filter state forward to time - * .tkp1() (i.e. to t(k+1)); computes - * x(k+1|k) - * P(k+1|k) - * does not use the t(k+1) observations .input.z - */ - ref::rp extrapolate() const; + utc_nanos tkp1() const { return input_->tkp1(); } - /* compute kalman gain matrix K(k+1) - * given extrapolated t(k+1) state skp1_ext = {x(k+1|k), P(k+1|k)} - * - * note that .state() != skp1_ext; .state() reports {x(k), P(k)} - */ - MatrixXd gain(ref::rp const & skp1_ext) const; + /* extrapolate kalman filter state forward to time + * .tkp1() (i.e. to t(k+1)); computes + * x(k+1|k) + * P(k+1|k) + * does not use the t(k+1) observations .input.z + */ + ref::rp extrapolate() const; - /* compute kalman gain vector K(k+1) - * given extrapolated t(k+1) state skp1_ext = {x(k+1|k), P(k+1|k)}, - * on behalf of a single observation z[j]. - * actual observation z[j] is not given here, - * just computing the gain vector. i'th member of gain vector - * gives effect of innovation on i'th member of kalman filter state. - */ - VectorXd gain1(ref::rp const & skp1_ext, - uint32_t j) const; + /* compute kalman gain matrix K(k+1) + * given extrapolated t(k+1) state skp1_ext = {x(k+1|k), P(k+1|k)} + * + * note that .state() != skp1_ext; .state() reports {x(k), P(k)} + */ + MatrixXd gain(ref::rp const & skp1_ext) const; - /* compute correction to extrapolated filter state {x(k+1|k), P(k+1|k)}, - * for observation z(k+1) = .input.z() - */ - ref::rp correct(ref::rp const & skp1_ext); + /* compute kalman gain vector K(k+1) + * given extrapolated t(k+1) state skp1_ext = {x(k+1|k), P(k+1|k)}, + * on behalf of a single observation z[j]. + * actual observation z[j] is not given here, + * just computing the gain vector. i'th member of gain vector + * gives effect of innovation on i'th member of kalman filter state. + */ + VectorXd gain1(ref::rp const & skp1_ext, + uint32_t j) const; - /* compute correction to extrapolated filter state skp1_ext = {x(k+1|k), P(k+1|k)}, - * for a single observation z(k+1, j) = .input.z()[j] - */ - ref::rp correct1(ref::rp const & skp1_ext, - uint32_t j); + /* compute correction to extrapolated filter state {x(k+1|k), P(k+1|k)}, + * for observation z(k+1) = .input.z() + */ + ref::rp correct(ref::rp const & skp1_ext); - void display(std::ostream & os) const; - std::string display_string() const; + /* compute correction to extrapolated filter state skp1_ext = {x(k+1|k), P(k+1|k)}, + * for a single observation z(k+1, j) = .input.z()[j] + */ + ref::rp correct1(ref::rp const & skp1_ext, + uint32_t j); - private: - /* system state: timestamp, estimated process state, process covariance - * asof beginning of this step - */ - ref::rp state_; - /* input: observations at time t(k+1) */ - KalmanFilterInputPtr input_; - }; /*KalmanFilterStep*/ + void display(std::ostream & os) const; + std::string display_string() const; - inline std::ostream & - operator<<(std::ostream & os, KalmanFilterStep const & x) { - x.display(os); - return os; - } /*operator<<*/ + private: + /* system state: timestamp, estimated process state, process covariance + * asof beginning of this step + */ + ref::rp state_; + /* input: observations at time t(k+1) */ + KalmanFilterInputPtr input_; + }; /*KalmanFilterStep*/ - } /*namespace kalman*/ + inline std::ostream & + operator<<(std::ostream & os, KalmanFilterStep const & x) { + x.display(os); + return os; + } /*operator<<*/ + + } /*namespace kalman*/ } /*namespace xo*/ /* end KalmanFilterStep.hpp */ diff --git a/src/kalmanfilter/CMakeLists.txt b/src/kalmanfilter/CMakeLists.txt index 2dbdcff0..882db194 100644 --- a/src/kalmanfilter/CMakeLists.txt +++ b/src/kalmanfilter/CMakeLists.txt @@ -23,8 +23,8 @@ xo_add_shared_library4(${SELF_LIB} ${PROJECT_NAME}Targets ${PROJECT_VERSION} 1 $ # ---------------------------------------------------------------- # Dependencies # -# REMINDER: these must coodinate with find_dependency() calls in -# [xo-kalmanfilter/cmake/xo_kalmanfilterConfig.cmake.in] -# +# REMINDER: must coordinate with find_dependency() calls in +# [xo-kalmanfilter/cmake/xo_kalmanfilterConfig.cmake.in] + xo_dependency(${SELF_LIB} reactor) xo_external_target_dependency(${SELF_LIB} Eigen3 Eigen3::Eigen)