From 7e7bb77f387d959883abd7e151f599b33d973a67 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Fran=C3=A7ois=20Carouge?= Date: Thu, 30 Jun 2022 18:56:27 -0700 Subject: [PATCH] [operation] specialize operations support for 1-by-N filters --- include/fcarouge/internal/kalman.hpp | 21 ++++ ...an_eigen_operator.hpp => kalman_eigen.hpp} | 111 ++++++++++++++++-- include/fcarouge/internal/kalman_operator.hpp | 68 ----------- include/fcarouge/kalman.hpp | 34 ++++-- include/fcarouge/kalman_eigen.hpp | 45 ++++--- sample/rocket_altitude.cpp | 6 +- 6 files changed, 175 insertions(+), 110 deletions(-) rename include/fcarouge/internal/{kalman_eigen_operator.hpp => kalman_eigen.hpp} (55%) delete mode 100644 include/fcarouge/internal/kalman_operator.hpp diff --git a/include/fcarouge/internal/kalman.hpp b/include/fcarouge/internal/kalman.hpp index 5d01e88a4..b9401dc25 100644 --- a/include/fcarouge/internal/kalman.hpp +++ b/include/fcarouge/internal/kalman.hpp @@ -47,6 +47,27 @@ For more information, please refer to */ namespace fcarouge::internal { + +//! @brief Function object for providing an identity matrix. +//! +//! @todo Could we remove this for a standard facility? Perhaps a form of +//! std::integral_constant? +//! +//! @note Could this function object template be a variable template as proposed +//! in paper P2008R0 entitled "Enabling variable template template parameters"? +struct identity_matrix { + //! @brief Returns `1`, the 1-by-1 identity matrix equivalent. + //! + //! @tparam Type The type template parameter of the value. + //! + //! @return The value `1`. + template + [[nodiscard]] inline constexpr auto operator()() const noexcept + { + return Type{ 1 }; + } +}; + template diff --git a/include/fcarouge/internal/kalman_eigen_operator.hpp b/include/fcarouge/internal/kalman_eigen.hpp similarity index 55% rename from include/fcarouge/internal/kalman_eigen_operator.hpp rename to include/fcarouge/internal/kalman_eigen.hpp index eed9bf147..cf5e35c59 100644 --- a/include/fcarouge/internal/kalman_eigen_operator.hpp +++ b/include/fcarouge/internal/kalman_eigen.hpp @@ -36,22 +36,30 @@ OTHER DEALINGS IN THE SOFTWARE. For more information, please refer to */ -#ifndef FCAROUGE_INTERNAL_KALMAN_EIGEN_OPERATOR_HPP -#define FCAROUGE_INTERNAL_KALMAN_EIGEN_OPERATOR_HPP +#ifndef FCAROUGE_INTERNAL_KALMAN_EIGEN_HPP +#define FCAROUGE_INTERNAL_KALMAN_EIGEN_HPP //! @file //! @brief Kalman operation for Eigen 3 types. //! -//! @details Default customization point objects (CPO) for Eigen 3 types. +//! @details Default customization point objects (CPO). -#include "kalman.hpp" +#include "fcarouge/kalman.hpp" #include +#include +#include +#include #include namespace fcarouge::eigen::internal { + +//! @brief Arithmetic concept. +template +concept arithmetic = std::integral || std::floating_point; + //! @brief Function object for performing Eigen matrix transposition. //! //! @details Implemented with the Eigen linear algebra library matrices with @@ -78,7 +86,7 @@ struct transpose { //! @details Implemented with the Eigen linear algebra library matrices with //! sizes fixed at compile-time. struct symmetrize { - //! @brief Returns the symmetrised `value`. + //! @brief Returns the symmetrized `value`. //! //! @param value Value to compute the symmetry of. //! @@ -101,7 +109,7 @@ struct divide { //! @param numerator The dividend matrix of the division. N: m x n //! @param denominator The divisor matrix of the division. D: o x n //! - //! @return The quotien matrix. Q: m x o + //! @return The quotient matrix. Q: m x o //! //! @exception May throw implementation-defined exceptions. //! @@ -119,6 +127,66 @@ struct divide { .solve(numerator.transpose()) .transpose(); } + + //! @brief Returns the quotient of `numerator` and `denominator`. + //! + //! @param numerator The dividend matrix of the division. N: m x 1 + //! @param denominator The divisor value of the division. + //! + //! @return The quotient column vector. Q: m x 1 + //! + //! @exception May throw implementation-defined exceptions. + //! + //! @todo Simplify implementation. + [[nodiscard]] inline constexpr auto + operator()(const auto &numerator, const arithmetic auto &denominator) const -> + typename Eigen::Vector< + typename std::decay_t::Scalar, + std::decay_t::RowsAtCompileTime> + { + return Eigen::Matrix::Scalar, 1, + 1>{ denominator } + .transpose() + .fullPivHouseholderQr() + .solve(numerator.transpose()) + .transpose(); + } + + //! @brief Returns the quotient of `numerator` and `denominator`. + //! + //! @param numerator The dividend value of the division. + //! @param denominator The divisor matrix of the division. D: o x 1 + //! + //! @return The quotient row vector. Q: 1 x o + //! + //! @exception May throw implementation-defined exceptions. + //! + //! @todo Simplify implementation. + [[nodiscard]] inline constexpr auto + operator()(const arithmetic auto &numerator, const auto &denominator) const -> + typename Eigen::RowVector< + typename std::decay_t::Scalar, + std::decay_t::RowsAtCompileTime> + { + return denominator.transpose() + .fullPivHouseholderQr() + .solve(Eigen::Matrix::Scalar, + 1, 1>{ numerator }) + .transpose(); + } + + //! @brief Returns the quotient of `numerator` and `denominator`. + //! + //! @param numerator The dividend value of the division. + //! @param denominator The divisor value of the division. + //! + //! @return The quotient value. + [[nodiscard]] inline constexpr auto + operator()(const arithmetic auto &numerator, + const arithmetic auto &denominator) const + { + return numerator / denominator; + } }; //! @brief Function object for providing an Eigen identity matrix. @@ -126,11 +194,10 @@ struct divide { //! @details Implemented with the Eigen linear algebra library matrices with //! sizes fixed at compile-time. //! -//! @note Could this function object template be replaced by a variable -//! template? Proposed in paper P2008R0 entitled "Enabling variable template -//! template parameters". -struct identity { - //! @brief Returns the identity maxtrix. +//! @note Could this function object template be a variable template as proposed +//! in paper P2008R0 entitled "Enabling variable template template parameters"? +struct identity_matrix { + //! @brief Returns the identity matrix. //! //! @tparam Type The type template parameter of the matrix. //! @@ -142,8 +209,28 @@ struct identity { { return Type::Identity(); } + + //! @brief Returns `1`, the 1-by-1 identity matrix equivalent. + //! + //! @tparam Type The type template parameter of the value. + //! + //! @return The value `1`. + template + [[nodiscard]] inline constexpr auto operator()() const noexcept + { + return Type{ 1 }; + } }; +template , + typename PredictionArguments = std::tuple<>> +using kalman = fcarouge::kalman< + Type, std::conditional_t>, + std::conditional_t>, + std::conditional_t>, transpose, + symmetrize, divide, identity_matrix, UpdateArguments, PredictionArguments>; + } // namespace fcarouge::eigen::internal -#endif // FCAROUGE_INTERNAL_KALMAN_EIGEN_OPERATOR_HPP +#endif // FCAROUGE_INTERNAL_KALMAN_EIGEN_HPP diff --git a/include/fcarouge/internal/kalman_operator.hpp b/include/fcarouge/internal/kalman_operator.hpp deleted file mode 100644 index 74d8257de..000000000 --- a/include/fcarouge/internal/kalman_operator.hpp +++ /dev/null @@ -1,68 +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_OPERATOR_HPP -#define FCAROUGE_INTERNAL_KALMAN_OPERATOR_HPP - -//! @file -//! @brief Kalman operation for standard types. -//! -//! @details Default customization point objects (CPO) for standard types. - -namespace fcarouge::internal -{ -//! @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". -struct identity { - //! @brief Returns the identity maxtrix. - //! - //! @tparam Type The type template parameter of the matrix. - //! - //! @return The identity matrix `diag(1, 1, ..., 1)`. - template - [[nodiscard]] inline constexpr auto operator()() const noexcept - { - return Type{ 1 }; - } -}; - -} // namespace fcarouge::internal - -#endif // FCAROUGE_INTERNAL_KALMAN_OPERATOR_HPP diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index 77a0d1c89..fb9f52d30 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -43,7 +43,6 @@ For more information, please refer to */ //! @brief The main Kalman filter class. #include "internal/kalman.hpp" -#include "internal/kalman_operator.hpp" #include #include @@ -53,6 +52,9 @@ For more information, please refer to */ namespace fcarouge { +//! @brief Function object for providing an identity matrix. +using identity_matrix = internal::identity_matrix; + //! @brief Kalman filter. //! //! @details A Bayesian filter that uses multivariate Gaussians. @@ -60,16 +62,16 @@ namespace fcarouge //! 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. +//! 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 +//! @tparam State The type template parameter of the state vector X. State //! variables can be observed (measured), or hidden variables (inferred). This //! is the the mean of the multivariate Gaussian. -//! @tparam Output The type template parameter of the measurement vector z. -//! @tparam Input The type template parameter of the control u. +//! @tparam Output The type template parameter of the measurement vector Z. +//! @tparam Input The type template parameter of the control U. //! @tparam Transpose The customization point object template parameter of the //! matrix transpose functor. //! @tparam Symmetrize The customization point object template parameter of the @@ -78,11 +80,18 @@ namespace fcarouge //! matrix division functor. //! @tparam Identity The customization point object template parameter of the //! matrix identity functor. +//! @tparam UpdateArguments The variadic type template parameter for additional +//! update function parameters. Parameters such as delta times, variances, or +//! linearized values. The parameters are propagated to the function objects +//! used to compute the state observation H and the observation noise R +//! matrices. The parameters are also propagated to the state observation +//! function object h. //! @tparam PredictionArguments The variadic type template parameter for -//! additional prediction function parameters. Time, or a delta thereof, is -//! often a prediction parameter. The parameters are propagated to the function -//! objects used to compute the process noise Q, the state transition F, and the -//! control transition G matrices. +//! additional prediction function parameters. Parameters such as delta times, +//! variances, or linearized values. The parameters are propagated to the +//! function objects used to compute the process noise Q, the state transition +//! F, and the control transition G matrices. The parameters are also propagated +//! to the state transition function object f. //! //! @note This class could be usable in constant expressions if `std::function` //! could too. The polymorphic function wrapper was used in place of function @@ -118,11 +127,12 @@ namespace fcarouge //! characteristics? //! @todo Consider additional constructors? //! @todo Consider additional characteristics method overloads? +//! @todo Could we do away with std::tuple, replaced by a template template? 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 Identity = identity_matrix, typename UpdateArguments = std::tuple<>, typename PredictionArguments = std::tuple<>> class kalman diff --git a/include/fcarouge/kalman_eigen.hpp b/include/fcarouge/kalman_eigen.hpp index 5757db75f..0e5c62ff4 100644 --- a/include/fcarouge/kalman_eigen.hpp +++ b/include/fcarouge/kalman_eigen.hpp @@ -42,37 +42,52 @@ For more information, please refer to */ //! @file //! @brief Kalman operation for Eigen 3 types. -#include "kalman.hpp" -#include "internal/kalman_eigen_operator.hpp" - -#include +#include "internal/kalman_eigen.hpp" #include -#include #include namespace fcarouge::eigen { +//! @brief Function object for performing Eigen matrix transposition. +using transpose = internal::transpose; + +//! @brief Function object for performing Eigen matrix symmetrization. +using symmetrize = internal::symmetrize; + +//! @brief Function object for performing Eigen matrix division. +using divide = internal::divide; + +//! @brief Function object for providing an Eigen identity matrix. +using identity_matrix = internal::identity_matrix; + //! @brief Eigen-based Kalman filter. //! //! @details Implemented with the Eigen linear algebra library matrices with //! sizes fixed at compile-time. //! //! @tparam Type The type template parameter of the matrices data. -//! @tparam State The non-type template parameter size of the state vector x. +//! @tparam State The non-type template parameter size of the state vector X. //! @tparam Output The non-type template parameter size of the measurement -//! vector z. -//! @tparam Input The non-type template parameter size of the control u. +//! vector Z. +//! @tparam Input The non-type template parameter size of the control U. +//! @tparam UpdateArguments The variadic type template parameter for additional +//! update function parameters. Parameters such as delta times, variances, or +//! linearized values. The parameters are propagated to the function objects +//! used to compute the state observation H and the observation noise R +//! matrices. The parameters are also propagated to the state observation +//! function object h. //! @tparam PredictionArguments The variadic type template parameter for -//! additional prediction function parameters. Time, or a delta thereof, is -//! often a prediction parameter. +//! additional prediction function parameters. Parameters such as delta times, +//! variances, or linearized values. The parameters are propagated to the +//! function objects used to compute the process noise Q, the state transition +//! F, and the control transition G matrices. The parameters are also propagated +//! to the state transition function object f. template , + std::size_t Input = 1, typename UpdateArguments = std::tuple<>, typename PredictionArguments = std::tuple<>> -using kalman = fcarouge::kalman< - Type, Eigen::Vector, Eigen::Vector, - Eigen::Vector, internal::transpose, internal::symmetrize, - internal::divide, internal::identity, UpdateArguments, PredictionArguments>; +using kalman = internal::kalman; } // namespace fcarouge::eigen diff --git a/sample/rocket_altitude.cpp b/sample/rocket_altitude.cpp index f62e8d98d..bc3ecc9d0 100644 --- a/sample/rocket_altitude.cpp +++ b/sample/rocket_altitude.cpp @@ -109,7 +109,7 @@ namespace // For the sake of the example simplicity, we will assume a constant // measurement uncertainty: R1 = R2...Rn-1 = Rn = R. - k.r(400); + k.r(400.); k.update(-32.4); @@ -146,7 +146,7 @@ namespace 211.6 - 0.1 < k.p()(1, 0) && k.p()(1, 0) < 211.6 + 0.1 && 438.8 - 0.1 < k.p()(1, 1) && k.p()(1, 1) < 438.8 + 0.1); - step(delta_time, 39.97 + gravity, 18); + step(delta_time, 39.97 + gravity, 18.); step(delta_time, 39.81 + gravity, 22.9); step(delta_time, 39.75 + gravity, 19.5); step(delta_time, 39.6 + gravity, 28.5); @@ -157,7 +157,7 @@ namespace step(delta_time, 39.81 + gravity, 90.5); step(delta_time, 39.92 + gravity, 104.9); step(delta_time, 39.78 + gravity, 140.9); - step(delta_time, 39.98 + gravity, 148); + step(delta_time, 39.98 + gravity, 148.); step(delta_time, 39.76 + gravity, 187.6); step(delta_time, 39.86 + gravity, 209.2); step(delta_time, 39.61 + gravity, 244.6);