diff --git a/.clang-tidy b/.clang-tidy index e408511ca..62d3ec04e 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,7 +1,11 @@ Checks: '*, - -fuchsia-overloaded-operator, - -misc-non-private-member-variables-in-classes, - -llvmlibc-*, - -modernize-use-trailing-return-type, -altera-struct-pack-align, - -fuchsia-trailing-return' + -fuchsia-overloaded-operator, + -fuchsia-trailing-return, + -llvmlibc-implementation-in-namespace, + -llvmlibc-restrict-system-libc-headers, + -portability-simd-intrinsics' + +CheckOptions: + - key: misc-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic + value: 1 diff --git a/.github/workflows/verify_code_static_analysis_tidy.yml b/.github/workflows/verify_code_static_analysis_tidy.yml index 27fe9743a..a61f0851c 100644 --- a/.github/workflows/verify_code_static_analysis_tidy.yml +++ b/.github/workflows/verify_code_static_analysis_tidy.yml @@ -16,7 +16,7 @@ jobs: run: sudo apt update - name: Install run: | - sudo apt install clang-tidy-12 + sudo apt install clang-tidy-13 ( cd /tmp ; mkdir eigen ; git clone --depth 1 https://gitlab.com/libeigen/eigen.git ; ( cd eigen ; @@ -28,6 +28,6 @@ jobs: - name: Tidy run: | FILES=`find . -iname *.hpp` - clang-tidy-12 ${FILES} \ + clang-tidy-13 ${FILES} \ --warnings-as-errors=* \ -- -x c++ -Wall -Wextra -pedantic -std=c++2b -Iinclude -I/usr/local/include/eigen3 diff --git a/include/fcarouge/internal/kalman.hpp b/include/fcarouge/internal/kalman.hpp index afa07477b..161703e07 100644 --- a/include/fcarouge/internal/kalman.hpp +++ b/include/fcarouge/internal/kalman.hpp @@ -42,9 +42,6 @@ For more information, please refer to */ //! @file //! @brief The main Kalman filter class. -#include "kalman_equation.hpp" -#include "kalman_operator.hpp" - #include #include @@ -52,7 +49,7 @@ namespace fcarouge::internal { template + typename Multiply, typename... PredictionArguments> struct kalman { //! @name Public Member Types //! @{ @@ -94,12 +91,18 @@ struct kalman { //! @details Also known as B. using input_control = std::invoke_result_t; + using innovation = output; + using innovation_uncertainty = output_uncertainty; + using gain = std::invoke_result_t; + //! @} //! @name Public Member Variables //! @{ //! @brief The state estimate vector x. + //! + //! @todo Is there a simpler, more portable way to get a zero initialization? state x{ 0 * Identity().template operator()() }; //! @brief The estimate uncertainty, covariance matrix P. @@ -167,38 +170,64 @@ struct kalman { //! @details // Add prediction arguments? std::function predict_state = - [](const state &x, const state_transition &f) { return state{ f * x }; }; + [this](const state &x, const state_transition &f) { + return state{ multiply(f, x) }; + }; + + Transpose transpose; + Divide divide; + Symmetrize symmetrize; + Identity identity; + Multiply multiply; //! @} //! @name Public Member Functions //! @{ + //! @todo Do we want to allow the client to view the gain k? And the residual + //! y? + //! @todo Do we want to store i - k * h in a temporary result for reuse? inline constexpr void update(const auto &...output_z) { + const auto z{ output{ output_z... } }; + h = transition_observation_h(); r = noise_observation_r(); - const auto z{ output{ output_z... } }; - internal::update(x, p, h, r, z); + + const innovation_uncertainty s{ h * p * transpose(h) + r }; + const gain k{ divide(p * transpose(h), s) }; + const innovation y{ z - h * x }; + const auto i{ identity.template operator()() }; + + x = state{ x + k * y }; + p = symmetrize(estimate_uncertainty{ + (i - k * h) * p * transpose(i - k * h) + k * r * transpose(k) }); } inline constexpr void predict(const PredictionArguments &...arguments, const auto &...input_u) { const auto ff{ predict_state }; + const auto u{ input{ input_u... } }; + 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); + + x = state{ ff(x, f) + g * u }; + p = symmetrize(estimate_uncertainty{ f * p * transpose(f) + q }); } inline constexpr void predict(const PredictionArguments &...arguments) { const auto ff{ predict_state }; + f = transition_state_f(arguments...); q = noise_process_q(arguments...); - internal::predict(x, p, ff, f, q); + + x = state{ ff(x, f) }; + p = symmetrize(estimate_uncertainty{ f * p * transpose(f) + q }); } //! @} diff --git a/include/fcarouge/internal/kalman_equation.hpp b/include/fcarouge/internal/kalman_equation.hpp deleted file mode 100644 index b35ef3b24..000000000 --- a/include/fcarouge/internal/kalman_equation.hpp +++ /dev/null @@ -1,165 +0,0 @@ -/*_ __ _ __ __ _ _ - | |/ / /\ | | | \/ | /\ | \ | | - | ' / / \ | | | \ / | / \ | \| | - | < / /\ \ | | | |\/| | / /\ \ | . ` | - | . \ / ____ \| |____| | | |/ ____ \| |\ | - |_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| - -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_EQUATION_HPP -#define FCAROUGE_INTERNAL_KALMAN_EQUATION_HPP - -//! @file -//! @brief Kalman filter main project header. - -#include -#include - -namespace fcarouge::internal -{ -[[nodiscard]] inline constexpr auto -extrapolate_state(const auto &x, const auto &ff, const auto &f, const auto &g, - const auto &u) -{ - using state = std::decay_t; - - return state{ ff(x, f) + g * u }; -} - -[[nodiscard]] inline constexpr auto -extrapolate_state(const auto &x, const auto &ff, const auto &f) -{ - using state = std::decay_t; - - return state{ ff(x, f) }; -} - -template -[[nodiscard]] inline constexpr auto -extrapolate_covariance(const auto &p, const auto &f, const auto &q) -{ - using estimate_uncertainty = std::decay_t; - - Transpose transpose; - - return estimate_uncertainty{ f * p * transpose(f) + q }; -} - -template -inline constexpr void predict(auto &x, auto &p, const auto &ff, const auto &f, - const auto &q) -{ - x = extrapolate_state(x, ff, f); - - Symmetrize symmetrize; - - p = symmetrize(extrapolate_covariance(p, f, q)); -} - -template -inline constexpr void predict(auto &x, auto &p, const auto &ff, const auto &f, - const auto &q, const auto &g, const auto &u) -{ - x = extrapolate_state(x, ff, f, g, u); - - Symmetrize symmetrize; - - p = symmetrize(extrapolate_covariance(p, f, q)); -} - -[[nodiscard]] inline constexpr auto update_state(const auto &x, const auto &k, - const auto &y) -{ - using state = std::decay_t; - - return state{ x + k * y }; -} - -template -[[nodiscard]] inline constexpr auto -update_covariance(const auto &p, const auto &k, const auto &h, const auto &r) -{ - using estimate_uncertainty = std::decay_t; - - Transpose transpose; - Identity identity; - const auto i{ identity.template operator()() }; - - return estimate_uncertainty{ (i - k * h) * p * transpose(i - k * h) + - k * r * transpose(k) }; -} - -template -[[nodiscard]] inline constexpr auto weight_gain(const auto &p, const auto &h, - const auto &r) -{ - using observation = std::decay_t; - using gain = std::invoke_result_t; - using innovation_uncertainty = std::decay_t; - - Transpose transpose; - Divide divides; - - const innovation_uncertainty s{ h * p * transpose(h) + r }; - - return gain{ divides(p * transpose(h), s) }; -} - -[[nodiscard]] inline constexpr auto innovate(const auto &x, const auto &z, - const auto &h) -{ - using innovation = std::decay_t; - - return innovation{ z - h * x }; -} - -//! @todo Do we want to allow the client to view the gain k? And the residual y? -template -inline constexpr void update(auto &x, auto &p, const auto &h, const auto &r, - const auto &z) -{ - const auto k{ weight_gain(p, h, r) }; - - const auto y{ innovate(x, z, h) }; - - x = update_state(x, k, y); - - Symmetrize symmetrize; - - p = symmetrize(update_covariance(p, k, h, r)); -} - -} // namespace fcarouge::internal - -#endif // FCAROUGE_INTERNAL_KALMAN_EQUATION_HPP diff --git a/include/fcarouge/internal/kalman_operator.hpp b/include/fcarouge/internal/kalman_operator.hpp index 4832293c8..4da9141b2 100644 --- a/include/fcarouge/internal/kalman_operator.hpp +++ b/include/fcarouge/internal/kalman_operator.hpp @@ -44,7 +44,7 @@ For more information, please refer to */ namespace fcarouge::internal { -//! @brief Function object for providing an identy matrix. +//! @brief Function object for providing an identity matrix. //! //! @note This function object template should be a variable template. Proposed //! in paper P2008R0 entitled "Enabling variable template template parameters". @@ -55,9 +55,9 @@ struct identity { //! //! @return The identity matrix `diag(1, 1, ..., 1)`. template - [[nodiscard]] inline constexpr Type operator()() const noexcept + [[nodiscard]] inline constexpr auto operator()() const noexcept { - return 1; + return Type{ 1 }; } }; diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index 86f7caff3..19e89ead2 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -43,6 +43,7 @@ For more information, please refer to */ //! @brief The main Kalman filter class. #include "internal/kalman.hpp" +#include "internal/kalman_operator.hpp" #include #include @@ -51,12 +52,14 @@ namespace fcarouge { //! @brief Kalman filter. //! -//! @details A Bayesian filter that uses multivariate Gaussians. Kalman filters -//! update estimates by multiplying Gaussians and predict estimates by adding -//! Gaussians. Design the state (x, P), the process (F, Q), the measurement (z, -//! R), the measurement function H, and if the system has control inputs (u, B). -//! Designing a filter is as much art as science. Kalman filters assume white -//! noise. +//! @details A Bayesian filter that uses multivariate Gaussians. +//! Applicable for unimodal and uncorrelated uncertainties. Kalman filters +//! assume white noise, propagation and measurement functions are +//! differentiable, and that the uncertainty stays centered on the state +//! estimate. The filter updates estimates by multiplying Gaussians and predicts +//! estimates by adding Gaussians. Design the state (x, P), the process (F, Q), +//! the measurement (z, R), the measurement function H, and if the system has +//! control inputs (u, B). Designing a filter is as much art as science. //! //! @tparam Type The type template parameter of the value type of the filter. //! @tparam State The type template parameter of the state vector x. State @@ -95,11 +98,14 @@ namespace fcarouge //! @todo Would we want to support smoothers? //! @todo How to add or associate constraints on the types and operation to //! support compilation and semantics? +//! @todo Which constructors to support? +//! @todo Is the Kalman filter a recursive state estimation? template < typename Type = double, typename State = Type, typename Output = State, typename Input = State, typename Transpose = std::identity, typename Symmetrize = std::identity, typename Divide = std::divides, - typename Identity = internal::identity, typename... PredictionArguments> + typename Identity = internal::identity, + typename Multiply = std::multiplies, typename... PredictionArguments> class kalman { private: @@ -109,7 +115,7 @@ class kalman //! @brief Implementation details of the filter. using implementation = internal::kalman; + Identity, Multiply, PredictionArguments...>; //! @} @@ -118,7 +124,7 @@ class kalman //! @{ //! @brief The type of the filtered data elements. - using value_type = State; + using value_type = Type; //! @brief Type of the state estimate vector X. using state = typename implementation::state; @@ -144,7 +150,7 @@ class kalman //! @brief Type of the state transition matrix F. //! - //! @details Also known as the fundamental matrix, Φ, or A. + //! @details Also known as the fundamental matrix, propagation, Φ, or A. using state_transition = typename implementation::state_transition; //! @brief Type of the observation transition matrix H. @@ -202,7 +208,7 @@ class kalman //! i.e. `*this`. //! //! @complexity Constant. - inline constexpr kalman &operator=(const kalman &other) = default; + inline constexpr auto operator=(const kalman &other) -> kalman & = default; //! @brief Move assignment operator. //! @@ -218,7 +224,8 @@ class kalman //! i.e. `*this`. //! //! @complexity Constant. - inline constexpr kalman &operator=(kalman &&other) noexcept = default; + inline constexpr auto operator=(kalman &&other) noexcept + -> kalman & = default; //! @brief Destructs the kalman filter. //! @@ -236,8 +243,8 @@ class kalman //! //! @complexity Constant. [[nodiscard("The returned state estimate vector X is unexpectedly " - "discarded.")]] inline constexpr state - x() const; + "discarded.")]] inline constexpr auto + x() const -> state; //! @brief Sets the state estimate vector X. //! @@ -253,8 +260,8 @@ class kalman //! //! @complexity Constant. [[nodiscard("The returned observation vector Z is unexpectedly " - "discarded.")]] inline constexpr output - z() const; + "discarded.")]] inline constexpr auto + z() const -> output; //! @brief Returns the control vector U. //! @@ -262,8 +269,8 @@ class kalman //! //! @complexity Constant. [[nodiscard("The returned control vector U is unexpectedly " - "discarded.")]] inline constexpr input - u() const; + "discarded.")]] inline constexpr auto + u() const -> input; //! @brief Returns the estimated covariance matrix P. //! @@ -271,8 +278,8 @@ class kalman //! //! @complexity Constant. [[nodiscard("The returned estimated covariance matrix P is unexpectedly " - "discarded.")]] inline constexpr estimate_uncertainty - p() const; + "discarded.")]] inline constexpr auto + p() const -> estimate_uncertainty; //! @brief Sets the estimated covariance matrix P. //! @@ -285,8 +292,8 @@ class kalman //! //! @complexity Constant. [[nodiscard("The returned process noise covariance matrix Q is unexpectedly " - "discarded.")]] inline constexpr process_uncertainty - q() const; + "discarded.")]] inline constexpr auto + q() const -> process_uncertainty; //! @brief Sets the process noise covariance matrix Q. //! @@ -296,11 +303,12 @@ class kalman //! @brief Sets the process noise covariance matrix Q function. //! //! @complexity Constant. - inline constexpr void q(const auto &value) requires std::constructible_from < + inline constexpr void + q(const auto &callable) requires std::constructible_from < std::function, - decltype(value) > + decltype(callable) > { - filter.noise_process_q = value; + filter.noise_process_q = callable; } //! @brief Returns the observation noise covariance @@ -312,8 +320,8 @@ class kalman //! //! @complexity Constant. [[nodiscard("The returned observation noise covariance matrix R is " - "unexpectedly discarded.")]] inline constexpr output_uncertainty - r() const; + "unexpectedly discarded.")]] inline constexpr auto + r() const -> output_uncertainty; //! @brief Sets the observation noise covariance matrix R. //! @@ -323,11 +331,11 @@ class kalman //! @brief Sets the observation noise covariance matrix R function. //! //! @complexity Constant. - inline constexpr void r(const auto &value) requires std::constructible_from < - std::function, - decltype(value) > + inline constexpr void r(const auto &callable) requires + std::constructible_from < std::function, + decltype(callable) > { - filter.noise_observation_r = value; + filter.noise_observation_r = callable; } //! @brief Returns the state transition matrix F. @@ -336,8 +344,8 @@ class kalman //! //! @complexity Constant. [[nodiscard("The returned state transition matrix F is unexpectedly " - "discarded.")]] inline constexpr state_transition - f() const; + "discarded.")]] inline constexpr auto + f() const -> state_transition; //! @brief Sets the state transition matrix F. //! @@ -347,11 +355,12 @@ class kalman //! @brief Sets the state transition matrix F function. //! //! @complexity Constant. - inline constexpr void f(const auto &value) requires std::constructible_from < + inline constexpr void + f(const auto &callable) requires std::constructible_from < std::function, - decltype(value) > + decltype(callable) > { - filter.transition_state_f = value; + filter.transition_state_f = callable; } //! @brief Returns the observation transition matrix H. @@ -360,8 +369,8 @@ class kalman //! //! @complexity Constant. [[nodiscard("The returned observation transition matrix H is unexpectedly " - "discarded.")]] inline constexpr output_model - h() const; + "discarded.")]] inline constexpr auto + h() const -> output_model; //! @brief Sets the observation, measurement transition matrix H. //! @@ -371,11 +380,12 @@ class kalman //! @brief Sets the observation, measurement transition matrix H function. //! //! @complexity Constant. - inline constexpr void h(const auto &value) requires std::constructible_from < + inline constexpr void + h(const auto &callable) requires std::constructible_from < std::function, - decltype(value) > + decltype(callable) > { - filter.transition_observation_h = value; + filter.transition_observation_h = callable; } //! @brief Returns the control transition matrix G. @@ -384,8 +394,8 @@ class kalman //! //! @complexity Constant. [[nodiscard("The returned control transition matrix G is unexpectedly " - "discarded.")]] inline constexpr input_control - g() const; + "discarded.")]] inline constexpr auto + g() const -> input_control; //! @brief Sets the control transition matrix G. //! @@ -395,11 +405,12 @@ class kalman //! @brief Sets the control transition matrix G function. //! //! @complexity Constant. - inline constexpr void g(const auto &value) requires std::constructible_from < + inline constexpr void + g(const auto &callable) requires std::constructible_from < std::function, - decltype(value) > + decltype(callable) > { - filter.transition_control_g = value; + filter.transition_control_g = callable; } //! @} @@ -489,56 +500,52 @@ class kalman template -inline constexpr - typename kalman::state - kalman::x() const + typename Identity, typename Multiply, typename... PredictionArguments> +inline constexpr auto +kalman::x() const -> state { return filter.x; } template + typename Identity, typename Multiply, typename... PredictionArguments> inline constexpr void kalman::x(const auto &value, const auto &...values) + Multiply, PredictionArguments...>::x(const auto &value, + const auto &...values) { filter.x = state{ value, values... }; } template -inline constexpr - typename kalman::estimate_uncertainty - kalman::p() const + typename Identity, typename Multiply, typename... PredictionArguments> +inline constexpr auto +kalman::p() const -> estimate_uncertainty { return filter.p; } template + typename Identity, typename Multiply, typename... PredictionArguments> inline constexpr void kalman::p(const auto &value, const auto &...values) + Multiply, PredictionArguments...>::p(const auto &value, + const auto &...values) { filter.p = estimate_uncertainty{ value, values... }; } template -inline constexpr - typename kalman::process_uncertainty - kalman::q() const + typename Identity, typename Multiply, typename... PredictionArguments> +inline constexpr auto +kalman::q() const -> process_uncertainty { return filter.q; } @@ -546,119 +553,117 @@ inline constexpr //! @todo Don't we need to reset functions or values when the other is set? template + typename Identity, typename Multiply, typename... PredictionArguments> inline constexpr void kalman::q(const auto &value, const auto &...values) + Multiply, PredictionArguments...>::q(const auto &value, + const auto &...values) { filter.q = process_uncertainty{ value, values... }; } template -inline constexpr - typename kalman::output_uncertainty - kalman::r() const + typename Identity, typename Multiply, typename... PredictionArguments> +inline constexpr auto +kalman::r() const -> output_uncertainty { return filter.r; } template + typename Identity, typename Multiply, typename... PredictionArguments> inline constexpr void kalman::r(const auto &value, const auto &...values) + Multiply, PredictionArguments...>::r(const auto &value, + const auto &...values) { filter.r = output_uncertainty{ value, values... }; } template -inline constexpr - typename kalman::state_transition - kalman::f() const + typename Identity, typename Multiply, typename... PredictionArguments> +inline constexpr auto +kalman::f() const -> state_transition { return filter.f; } template + typename Identity, typename Multiply, typename... PredictionArguments> inline constexpr void kalman::f(const auto &value, const auto &...values) + Multiply, PredictionArguments...>::f(const auto &value, + const auto &...values) { filter.f = state_transition{ value, values... }; } template -inline constexpr - typename kalman::output_model - kalman::h() const + typename Identity, typename Multiply, typename... PredictionArguments> +inline constexpr auto +kalman::h() const -> output_model { return filter.h; } template + typename Identity, typename Multiply, typename... PredictionArguments> inline constexpr void kalman::h(const auto &value, const auto &...values) + Multiply, PredictionArguments...>::h(const auto &value, + const auto &...values) { filter.h = output_model{ value, values... }; } template -inline constexpr - typename kalman::input_control - kalman::g() const + typename Identity, typename Multiply, typename... PredictionArguments> +inline constexpr auto +kalman::g() const -> input_control { return filter.g; } template + typename Identity, typename Multiply, typename... PredictionArguments> inline constexpr void kalman::g(const auto &value, const auto &...values) + Multiply, PredictionArguments...>::g(const auto &value, + const auto &...values) { filter.g = input_control{ value, values... }; } template + typename Identity, typename Multiply, typename... PredictionArguments> inline constexpr void kalman::update(const auto &...output_z) + Multiply, PredictionArguments...>::update(const auto &...output_z) { filter.update(output_z...); } template + typename Identity, typename Multiply, typename... PredictionArguments> inline constexpr void kalman::predict(const PredictionArguments &...arguments, - const auto &...input_u) + Multiply, PredictionArguments...>::predict(const PredictionArguments + &...arguments, + const auto &...input_u) { filter.predict(arguments..., input_u...); } diff --git a/include/fcarouge/kalman_eigen.hpp b/include/fcarouge/kalman_eigen.hpp index eed048dc2..502e2bb7e 100644 --- a/include/fcarouge/kalman_eigen.hpp +++ b/include/fcarouge/kalman_eigen.hpp @@ -47,6 +47,9 @@ For more information, please refer to */ #include +#include +#include + namespace fcarouge::eigen { //! @brief Eigen-based Kalman filter. @@ -62,12 +65,14 @@ namespace fcarouge::eigen //! @tparam PredictionArguments The variadic type template parameter for //! additional prediction function parameters. Time, or a delta thereof, is //! often a prediction parameter. -template -using kalman = fcarouge::kalman< - Type, Eigen::Vector, Eigen::Vector, - Eigen::Vector, internal::transpose, internal::symmetrize, - internal::divide, internal::identity, PredictionArguments...>; +template +using kalman = + fcarouge::kalman, + Eigen::Vector, Eigen::Vector, + internal::transpose, internal::symmetrize, + internal::divide, internal::identity, + std::multiplies, PredictionArguments...>; } // namespace fcarouge::eigen diff --git a/source/main.cpp b/source/main.cpp index 0bba5a052..3aa749653 100644 --- a/source/main.cpp +++ b/source/main.cpp @@ -40,7 +40,7 @@ For more information, please refer to */ //! //! @details A valid and empty point of entry is provided to support the tests //! and samples. Not further code necessary. -int main(int, char **) +auto main() -> int { return 0; }