From 3af6aeb27bb92ad23c8a5163a025f3c1b515d562 Mon Sep 17 00:00:00 2001 From: Francois Carouge Date: Sun, 1 May 2022 16:57:20 -0700 Subject: [PATCH] [facade] encapsulate implementation details behind facade --- .../verify_code_static_analysis_cppcheck.yml | 14 +- README.md | 113 +++- include/fcarouge/internal/kalman.hpp | 236 +++++++ .../{ => internal}/kalman_eigen_operator.hpp | 10 +- .../{ => internal}/kalman_equation.hpp | 77 +-- .../{ => internal}/kalman_operator.hpp | 10 +- include/fcarouge/kalman.hpp | 586 ++++++++++++++---- include/fcarouge/kalman_eigen.hpp | 7 +- sample/building_height.cpp | 39 +- sample/liquid_temperature.cpp | 46 +- sample/rocket_altitude.cpp | 34 +- sample/vehicule_location.cpp | 63 +- 12 files changed, 952 insertions(+), 283 deletions(-) create mode 100644 include/fcarouge/internal/kalman.hpp rename include/fcarouge/{ => internal}/kalman_eigen_operator.hpp (95%) rename include/fcarouge/{ => internal}/kalman_equation.hpp (71%) rename include/fcarouge/{ => internal}/kalman_operator.hpp (94%) diff --git a/.github/workflows/verify_code_static_analysis_cppcheck.yml b/.github/workflows/verify_code_static_analysis_cppcheck.yml index 869a56f84..d8017902b 100644 --- a/.github/workflows/verify_code_static_analysis_cppcheck.yml +++ b/.github/workflows/verify_code_static_analysis_cppcheck.yml @@ -12,9 +12,17 @@ jobs: steps: - name: Checkout uses: actions/checkout@v3 - - name: Update - run: sudo apt update - name: Install - run: sudo apt install cppcheck + run: | + ( cd /tmp ; mkdir cppcheck ; + git clone --depth 1 https://github.com/danmar/cppcheck.git ; + ( cd cppcheck ; + mkdir build ) ; + ( cd cppcheck/build ; + cmake .. ; + cmake --build . ; + sudo make install ) ) + - name: Version + run: cppcheck --version - name: Check run: cppcheck --verbose --suppressions-list=.cppcheck --error-exitcode=1 --enable=all -I include . diff --git a/README.md b/README.md index 87c05ddbc..1178e9385 100644 --- a/README.md +++ b/README.md @@ -54,12 +54,9 @@ using kalman = fcarouge::kalman; kalman k; -k.state_x = 60; -k.estimate_uncertainty_p = 225; -k.transition_observation_h = [] { return kalman::observation{ 1 }; }; -k.noise_observation_r = [] { - return kalman::observation_noise_uncertainty{ 25 }; -}; +k.x(60.); +k.p(225.); +k.r(25.); k.observe(48.54); ``` @@ -76,28 +73,25 @@ const double gravitational_acceleration{ -9.8 }; // m.s^-2 const std::chrono::milliseconds delta_time{ 250 }; kalman k; -k.state_x = { 0, 0 }; -k.estimate_uncertainty_p = - kalman::estimate_uncertainty{ { 500, 0 }, { 0, 500 } }; -k.transition_state_f = [](const std::chrono::milliseconds &delta_time) { +k.x(0, 0); +k.p(kalman::estimate_uncertainty{ { 500, 0 }, { 0, 500 } }); +k.f([](const std::chrono::milliseconds &delta_time) { const auto dt{ std::chrono::duration(delta_time).count() }; return kalman::state_transition{ { 1, dt }, { 0, 1 } }; -}; -k.noise_process_q = [](const std::chrono::milliseconds &delta_time) { +}); +k.q([](const std::chrono::milliseconds &delta_time) { const auto dt{ std::chrono::duration(delta_time).count() }; - return kalman::process_noise_uncertainty{ + return kalman::process_uncertainty{ { 0.1 * 0.1 * dt * dt * dt * dt / 4, 0.1 * 0.1 * dt * dt * dt / 2 }, { 0.1 * 0.1 * dt * dt * dt / 2, 0.1 * 0.1 * dt * dt } }; -}; -k.transition_control_g = [](const std::chrono::milliseconds &delta_time) { +}); +k.g([](const std::chrono::milliseconds &delta_time) { const auto dt{ std::chrono::duration(delta_time).count() }; - return kalman::control{ 0.0313, dt }; -}; -k.transition_observation_h = [] { return kalman::observation{ { 1, 0 } }; }; -k.noise_observation_r = [] { - return kalman::observation_noise_uncertainty{ 400 }; -}; + return kalman::input_control{ 0.0313, dt }; +}); +k.h(1, 0); +k.r(400); k.predict(delta_time, gravitational_acceleration); k.observe(-32.40 ); @@ -105,6 +99,24 @@ k.predict(delta_time, 39.72); k.observe(-11.1); ``` +# Library + +- [Kalman Filter for C++](#kalman-filter-for-c) +- [Continuous Integration & Deployment Actions](#continuous-integration--deployment-actions) +- [Examples](#examples) + - [One-Dimensional](#one-dimensional) + - [Multi-Dimensional](#multi-dimensional) +- [Library](#library) +- [Motivation](#motivation) +- [Resources](#resources) +- [Class fcarouge::kalman](#class-fcarougekalman) + - [Template Parameters](#template-parameters) + - [Member Types](#member-types) + - [Member Functions](#member-functions) + - [Characteristics](#characteristics) + - [Modifiers](#modifiers) +- [License](#license) + # Motivation Kalman filters can be difficult to learn, use, and implement. Users often need fair algebra, domain, and software knowledge. Inadequacy leads to incorrectness, underperformance, and a big ball of mud. @@ -121,6 +133,65 @@ Awesome resources to learn about Kalman filters: - [KalmanFilter.NET](https://www.kalmanfilter.net) by Alex Becker. - [Kalman and Bayesian Filters in Python](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python) by Roger Labbe. +# Class fcarouge::kalman + +Defined in header [fcarouge/kalman.hpp](include/fcarouge/kalman.hpp) + +```cpp +template typename Transpose = internal::transpose, + template typename Symmetrize = internal::symmetrize, + template typename Divide = internal::divide, + template typename Identity = internal::identity, + typename... PredictionArguments> +class kalman +``` + +## Template Parameters + +## Member Types + +| Member Type | Definition | +| --- | --- | +| `state` | Type of the state estimate vector X. | +| `output` | Type of the observation vector Z, also known as Y. | +| `input` | Type of the control vector U. | +| `estimate_uncertainty` | Type of the estimated covariance matrix P, also known as Σ. | +| `process_uncertainty` | Type of the process noise covariance matrix Q. | +| `output_uncertainty` | Type of the observation, measurement noise covariance matrix R. | +| `state_transition` | Type of the state transition matrix F, also known as Φ or A. | +| `output_model` | Type of the observation transition matrix H, also known as C. | +| `input_control` | Type of the control transition matrix G, also known as B. | + +## Member Functions + +| Member Function | Definition | +| --- | --- | +| `(constructor)` | Constructs the filter. | +| `(destructor)` | Destructs the filter. | +| `operator=` | Assigns values to the filter. | + +### Characteristics + +| Modifier | Definition | +| --- | --- | +| `x` | Manages the state estimate vector. | +| `z` | Manages the observation vector. | +| `u` | Manages the control vector. | +| `p` | Manages the estimated covariance matrix. | +| `q` | Manages the process noise covariance matrix. | +| `r` | Manages the observation, measurement noise covariance matrix. | +| `f` | Manages the state transition matrix. | +| `h` | Manages the observation transition matrix. | +| `g` | Manages the control transition matrix. | + +### Modifiers + +| Modifier | Definition | +| --- | --- | +| `observe` | Updates the estimates with the outcome of a measurement. | +| `predict` | Produces estimates of the state variables and uncertainties. | + # License diff --git a/include/fcarouge/internal/kalman.hpp b/include/fcarouge/internal/kalman.hpp new file mode 100644 index 000000000..a640de54d --- /dev/null +++ b/include/fcarouge/internal/kalman.hpp @@ -0,0 +1,236 @@ +/*_ __ _ __ __ _ _ + | |/ / /\ | | | \/ | /\ | \ | | + | ' / / \ | | | \ / | / \ | \| | + | < / /\ \ | | | |\/| | / /\ \ | . ` | + | . \ / ____ \| |____| | | |/ ____ \| |\ | + |_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| + +Kalman Filter for C++ +Version 0.1.0 +https://github.com/FrancoisCarouge/Kalman + +SPDX-License-Identifier: Unlicense + +This is free and unencumbered software released into the public domain. + +Anyone is free to copy, modify, publish, use, compile, sell, or +distribute this software, either in source code form or as a compiled +binary, for any purpose, commercial or non-commercial, and by any +means. + +In jurisdictions that recognize copyright laws, the author or authors +of this software dedicate any and all copyright interest in the +software to the public domain. We make this dedication for the benefit +of the public at large and to the detriment of our heirs and +successors. We intend this dedication to be an overt act of +relinquishment in perpetuity of all present and future rights to this +software under copyright law. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR +OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +OTHER DEALINGS IN THE SOFTWARE. + +For more information, please refer to */ + +#ifndef FCAROUGE_INTERNAL_KALMAN_HPP +#define FCAROUGE_INTERNAL_KALMAN_HPP + +//! @file +//! @brief The main Kalman filter class. + +#include "kalman_equation.hpp" +#include "kalman_operator.hpp" + +#include +#include + +namespace fcarouge::internal +{ +template typename Transpose = transpose, + template typename Symmetrize = symmetrize, + template typename Divide = divide, + template typename Identity = identity, + typename... PredictionArguments> +struct kalman { + //! @name Public Member Types + //! @{ + + //! @brief Type of the state estimate vector X. + using state = State; + + //! @brief Type of the observation vector Z. + //! + //! @details Also known as Y. + using output = Output; + + //! @brief Type of the control vector U. + using input = Input; + + //! @brief Type of the estimated covariance matrix P. + //! + //! @details Also known as Σ. + using estimate_uncertainty = + std::invoke_result_t, State, State>; + + //! @brief Type of the process noise covariance matrix Q. + using process_uncertainty = + std::invoke_result_t, State, State>; + + //! @brief Type of the observation, measurement noise covariance matrix R. + using output_uncertainty = + std::invoke_result_t, Output, Output>; + + //! @brief Type of the state transition matrix F. + //! + //! @details Also known as Φ or A. + using state_transition = + std::invoke_result_t, State, State>; + + //! @brief Type of the observation transition matrix H. + //! + //! @details Also known as C. + using output_model = + std::invoke_result_t, Output, State>; + + //! @brief Type of the control transition matrix G. + //! + //! @details Also known as B. + using input_control = + std::invoke_result_t, State, Input>; + + //! @} + + //! @name Public Member Variables + //! @{ + + //! @brief The state estimate vector x. + state x{}; + + //! @brief The estimate uncertainty, covariance matrix P. + //! + //! @details The estimate uncertainty, covariance is also known as Σ. + estimate_uncertainty p{ Identity()() }; + + output_model h{ Identity()() }; + + output_uncertainty r{ Identity()() }; + + state_transition f{ Identity()() }; + + process_uncertainty q{}; + + input_control g{}; + + //! @} + + //! @name Public Member Function Objects + //! @{ + + //! @brief Compute observation transition H matrix. + //! + //! @details The observation transition H is also known as C. + std::function transition_observation_h{ [this] { + return h; + } }; + + //! @brief Compute observation noise R matrix. + std::function noise_observation_r{ [this] { + return r; + } }; + + //! @brief Compute state transition F matrix. + //! + //! @details The state transition F matrix is also known as Φ or A. + //! For non-linear system, or extended filter, F is the Jacobian of the state + //! transition function. F = ∂fj/∂xi that is each row i contains the the + //! derivatives of the state transition function for every element j in the + //! state vector x. + std::function + transition_state_f{ [this](const PredictionArguments &...arguments) { + static_cast((arguments, ...)); + return f; + } }; + + //! @brief Compute process noise Q matrix. + std::function + noise_process_q{ [this](const PredictionArguments &...arguments) { + static_cast((arguments, ...)); + return q; + } }; + + //! @brief Compute control transition G matrix. + std::function + transition_control_g{ [this](const PredictionArguments &...arguments) { + static_cast((arguments, ...)); + return g; + } }; + + //! @brief State transition function. + //! + //! @details + // Add prediction arguments? + std::function predict_state = + [](const state &x, const state_transition &f) { return state{ f * x }; }; + + //! @} + + //! @name Public Member Functions + //! @{ + + inline constexpr void observe(const auto &...output_z) + { + h = transition_observation_h(); + r = noise_observation_r(); + const auto z{ output{ output_z... } }; + internal::observe(x, p, h, r, z); + } + + inline constexpr void + predict(const PredictionArguments &...arguments, + const auto &...input_u) requires(sizeof...(PredictionArguments) > 0) + { + // use member variables + const auto ff{ predict_state }; + f = transition_state_f(arguments...); + q = noise_process_q(arguments...); + g = transition_control_g(arguments...); + const auto u{ input{ input_u... } }; + internal::predict(x, p, ff, f, q, g, u); + } + + // Support for MSVC C2064 + inline constexpr void + predict(const auto &...input_u) requires(sizeof...(PredictionArguments) == 0) + { + // use member variables + const auto ff{ predict_state }; + f = transition_state_f(); + q = noise_process_q(); + g = transition_control_g(); + const auto u{ input{ input_u... } }; + internal::predict(x, p, ff, f, q, g, u); + } + + // Support for MSVC C2064 + inline constexpr void predict() requires(sizeof...(PredictionArguments) == 0) + { + // use member variables + const auto ff{ predict_state }; + f = transition_state_f(); + q = noise_process_q(); + g = transition_control_g(); + const input u{}; + internal::predict(x, p, ff, f, q, g, u); + } + + //! @} +}; + +} // namespace fcarouge::internal + +#endif // FCAROUGE_INTERNAL_KALMAN_HPP diff --git a/include/fcarouge/kalman_eigen_operator.hpp b/include/fcarouge/internal/kalman_eigen_operator.hpp similarity index 95% rename from include/fcarouge/kalman_eigen_operator.hpp rename to include/fcarouge/internal/kalman_eigen_operator.hpp index 568246f83..8f704b5d7 100644 --- a/include/fcarouge/kalman_eigen_operator.hpp +++ b/include/fcarouge/internal/kalman_eigen_operator.hpp @@ -36,8 +36,8 @@ OTHER DEALINGS IN THE SOFTWARE. For more information, please refer to */ -#ifndef FCAROUGE_KALMAN_EIGEN_OPERATOR_HPP -#define FCAROUGE_KALMAN_EIGEN_OPERATOR_HPP +#ifndef FCAROUGE_INTERNAL_KALMAN_EIGEN_OPERATOR_HPP +#define FCAROUGE_INTERNAL_KALMAN_EIGEN_OPERATOR_HPP //! @file //! @brief Kalman operation for Eigen 3 types. @@ -46,7 +46,7 @@ For more information, please refer to */ #include -namespace fcarouge::eigen +namespace fcarouge::eigen::internal { //! @brief Function object for performing Eigen matrix transposition. //! @@ -138,6 +138,6 @@ template struct identity { } }; -} // namespace fcarouge::eigen +} // namespace fcarouge::eigen::internal -#endif // FCAROUGE_KALMAN_EIGEN_OPERATOR_HPP +#endif // FCAROUGE_INTERNAL_KALMAN_EIGEN_OPERATOR_HPP diff --git a/include/fcarouge/kalman_equation.hpp b/include/fcarouge/internal/kalman_equation.hpp similarity index 71% rename from include/fcarouge/kalman_equation.hpp rename to include/fcarouge/internal/kalman_equation.hpp index 9fe102aa4..481cf758f 100644 --- a/include/fcarouge/kalman_equation.hpp +++ b/include/fcarouge/internal/kalman_equation.hpp @@ -36,67 +36,48 @@ OTHER DEALINGS IN THE SOFTWARE. For more information, please refer to */ -#ifndef FCAROUGE_KALMAN_EQUATION_HPP -#define FCAROUGE_KALMAN_EQUATION_HPP +#ifndef FCAROUGE_INTERNAL_KALMAN_EQUATION_HPP +#define FCAROUGE_INTERNAL_KALMAN_EQUATION_HPP //! @file //! @brief Kalman filter main project header. #include -namespace fcarouge +namespace fcarouge::internal { -[[nodiscard]] inline constexpr auto extrapolate_state(const auto &x, - const auto &f) -{ - using state = std::remove_reference_t>; - - return state{ f * x }; -} - [[nodiscard]] inline constexpr auto -extrapolate_state(const auto &x, const auto &f, const auto &g, const auto &u) +extrapolate_state(const auto &x, const auto &ff, const auto &f, const auto &g, + const auto &u) { using state = std::remove_reference_t>; - return state{ f * x + g * u }; + return state{ ff(x, f) + g * u }; } template