diff --git a/include/fcarouge/internal/eigen.hpp b/include/fcarouge/internal/eigen.hpp index cd37c4b43d..3d8ac5efcd 100644 --- a/include/fcarouge/internal/eigen.hpp +++ b/include/fcarouge/internal/eigen.hpp @@ -60,6 +60,23 @@ namespace fcarouge::eigen::internal template concept arithmetic = std::integral || std::floating_point; +[[nodiscard]] inline constexpr auto matrix(const auto &value) +{ + return value; +} + +[[nodiscard]] inline constexpr auto matrix(const arithmetic auto &value) +{ + using type = std::decay_t; + return Eigen::Matrix{ value }; +} + +template +[[nodiscard]] inline constexpr auto matrix(const std::array &value) +{ + return Eigen::Matrix{ value.data() }; +} + //! @brief Function object for performing Eigen matrix transposition. //! //! @details Implemented with the Eigen linear algebra library matrices with @@ -70,23 +87,19 @@ struct transpose { //! @param value Value to compute the transpose of. //! //! @exception May throw implementation-defined exceptions. - [[nodiscard]] inline constexpr auto operator()(const auto &value) const + [[nodiscard]] inline constexpr auto operator()(const auto &value) const -> + typename Eigen::Matrix::Scalar, + std::decay_t::ColsAtCompileTime, + std::decay_t::RowsAtCompileTime> { - using type = std::decay_t; - using result_type = - typename Eigen::Matrix; - - return result_type{ value.transpose() }; + return value.transpose(); } //! @brief Returns the transpose of `value`. //! //! @param value Value to compute the transpose of. - //! - //! @todo Can this be optimized? [[nodiscard]] inline constexpr auto - operator()(const arithmetic auto &value) const + operator()(const arithmetic auto &value) const noexcept -> const auto & { return value; } @@ -102,22 +115,20 @@ struct symmetrize { //! @param value Value to compute the symmetry of. //! //! @exception May throw implementation-defined exceptions. + //! + //! @todo Should overflow be protected? [[nodiscard]] inline constexpr auto operator()(const auto &value) const { - using result_type = std::decay_t; - - return result_type{ (value + value.transpose()) / 2 }; + return (value + value.transpose()) / 2; } //! @brief Returns the symmetrized `value`. //! //! @param value Value to compute the symmetry of. - //! - //! @todo Can this be optimized? [[nodiscard]] inline constexpr auto - operator()(const arithmetic auto &value) const + operator()(arithmetic auto &&value) const noexcept -> auto && { - return value; + return std::forward(value); } }; @@ -134,83 +145,17 @@ struct divide { //! @return The quotient matrix. Q: m x o //! //! @exception May throw implementation-defined exceptions. - //! - //! @todo Why compilation fails if we specify the return type in the body of - //! the function? template [[nodiscard]] inline constexpr auto operator()(const Numerator &numerator, const Denominator &denominator) const - -> typename Eigen::Matrix::Scalar, - std::decay_t::RowsAtCompileTime, - std::decay_t::RowsAtCompileTime> - { - return denominator.transpose() - .fullPivHouseholderQr() - .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. - template - [[nodiscard]] inline constexpr auto - operator()(const Numerator &numerator, - const arithmetic auto &denominator) const -> - typename Eigen::Vector::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. - template - [[nodiscard]] inline constexpr auto - operator()(const arithmetic auto &numerator, - const Denominator &denominator) const -> - typename Eigen::RowVector::Scalar, - std::decay_t::RowsAtCompileTime> { - return denominator.transpose() - .fullPivHouseholderQr() - .solve(Eigen::Matrix::Scalar, - 1, 1>{ numerator }) - .transpose(); - } + auto result{ matrix(denominator) + .transpose() + .fullPivHouseholderQr() + .solve(matrix(numerator).transpose()) + .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; + return typename decltype(result)::PlainMatrix{ result }; } }; @@ -230,7 +175,7 @@ struct identity_matrix { //! //! @exception May throw implementation-defined exceptions. template - [[nodiscard]] inline constexpr auto operator()() const + [[nodiscard]] inline constexpr auto operator()() const -> Type { return Type::Identity(); } @@ -241,9 +186,9 @@ struct identity_matrix { //! //! @return The value `1`. template - [[nodiscard]] inline constexpr auto operator()() const noexcept + [[nodiscard]] inline constexpr auto operator()() const noexcept -> Type { - return Type{ 1 }; + return 1; } }; @@ -254,14 +199,9 @@ template -using kalman = fcarouge::kalman< - std::conditional_t>, - std::conditional_t>, - std::conditional_t< - Input == 0, void, - std::conditional_t>>, - transpose, symmetrize, divide, identity_matrix, UpdateTypes, - PredictionTypes>; +using kalman = + fcarouge::kalman; } // namespace fcarouge::eigen::internal diff --git a/include/fcarouge/internal/format.hpp b/include/fcarouge/internal/format.hpp index acfad1c0b2..2e4492d4f7 100644 --- a/include/fcarouge/internal/format.hpp +++ b/include/fcarouge/internal/format.hpp @@ -43,17 +43,19 @@ For more information, please refer to */ namespace fcarouge { -template +template class kalman; } // namespace fcarouge -template +template struct std::formatter< - fcarouge::kalman, Char> { //! @todo Support parsing arguments. @@ -64,7 +66,7 @@ struct std::formatter< // @todo How to support different nested types? template - auto format(const fcarouge::kalman &filter, std::basic_format_context &format_context) diff --git a/include/fcarouge/internal/kalman.hpp b/include/fcarouge/internal/kalman.hpp index 953dac76bb..852e2d43b1 100644 --- a/include/fcarouge/internal/kalman.hpp +++ b/include/fcarouge/internal/kalman.hpp @@ -39,6 +39,8 @@ For more information, please refer to */ #ifndef FCAROUGE_INTERNAL_KALMAN_HPP #define FCAROUGE_INTERNAL_KALMAN_HPP +#include +#include #include #include @@ -61,34 +63,39 @@ struct repack> { template using repack_t = typename repack::type; -template +template struct kalman { //! @todo Support some more specializations, all, or disable others? }; -template -struct kalman +struct kalman, pack> { struct empty { }; - using state = State; - using output = Output; + template + using matrix = std::decay_t>; + template + using array = std::conditional_t< + Size == 1, Type, + std::decay_t>, + Type>>>; + using value_type = Type; + using state = array; + using output = array; using input = empty; - using estimate_uncertainty = - std::decay_t>; - using process_uncertainty = - std::decay_t>; - using output_uncertainty = - std::decay_t>; - using state_transition = - std::decay_t>; - using output_model = - std::decay_t>; + using estimate_uncertainty = matrix; + using process_uncertainty = matrix; + using output_uncertainty = matrix; + using state_transition = matrix; + using output_model = matrix; using input_control = empty; - using gain = std::decay_t>; + using gain = matrix; using innovation = output; using innovation_uncertainty = output_uncertainty; using observation_state_function = @@ -106,23 +113,24 @@ struct kalman; //! @todo Is there a simpler way to initialize to the zero matrix? - state x{ 0 * Identity().template operator()() }; + state x{ value_type{ 0 } * Identity().template operator()() }; estimate_uncertainty p{ Identity().template operator()() }; process_uncertainty q{ - 0 * Identity().template operator()() + value_type{ 0 } * Identity().template operator()() }; - output_uncertainty r{ 0 * + output_uncertainty r{ value_type{ 0 } * Identity().template operator()() }; output_model h{ Identity().template operator()() }; state_transition f{ Identity().template operator()() }; gain k{ Identity().template operator()() }; - innovation y{ 0 * Identity().template operator()() }; + innovation y{ value_type{ 0 } * + Identity().template operator()() }; innovation_uncertainty s{ Identity().template operator()() }; - output z{ 0 * Identity().template operator()() }; + output z{ value_type{ 0 } * Identity().template operator()() }; //! @todo Should we pass through the reference to the state x or have the user //! access it through k.x() when needed? Where does the practical/performance @@ -218,27 +226,31 @@ struct kalman -struct kalman, pack> { - using state = State; - using output = Output; - using input = Input; - using estimate_uncertainty = - std::decay_t>; - using process_uncertainty = - std::decay_t>; - using output_uncertainty = - std::decay_t>; - using state_transition = - std::decay_t>; - using output_model = - std::decay_t>; - using input_control = - std::decay_t>; - using gain = std::decay_t>; +template +struct kalman, pack> { + template + using matrix = std::decay_t>; + template + using array = std::conditional_t< + Size == 1, Type, + std::decay_t>, + Type>>>; + using value_type = Type; + using state = array; + using output = array; + using input = array; + using estimate_uncertainty = matrix; + using process_uncertainty = matrix; + using output_uncertainty = matrix; + using state_transition = matrix; + using output_model = matrix; + using input_control = matrix; + using gain = matrix; using innovation = output; using innovation_uncertainty = output_uncertainty; using observation_state_function = @@ -264,18 +276,19 @@ struct kalman() }; - output_uncertainty r{ 0 * + output_uncertainty r{ value_type{ 0 } * Identity().template operator()() }; output_model h{ Identity().template operator()() }; state_transition f{ Identity().template operator()() }; input_control g{ Identity().template operator()() }; gain k{ Identity().template operator()() }; - innovation y{ 0 * Identity().template operator()() }; + innovation y{ value_type{ 0 } * + Identity().template operator()() }; innovation_uncertainty s{ Identity().template operator()() }; - output z{ 0 * Identity().template operator()() }; - input u{ 0 * Identity().template operator()() }; + output z{ value_type{ 0 } * Identity().template operator()() }; + input u{ value_type{ 0 } * Identity().template operator()() }; //! @todo Should we pass through the reference to the state x or have the user //! access it through k.x() when needed? Where does the practical/performance diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index dd87d28b70..5a7c81ad17 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -46,6 +46,7 @@ For more information, please refer to */ #include "internal/kalman.hpp" #include +#include #include #include #include @@ -83,15 +84,16 @@ struct identity_matrix { //! 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 State The type template parameter of the state column 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 column vector +//! @tparam Type The filter data value type used for computation. +//! @tparam State The non-type template size of the state column vector x. State +//! variables can be observed (measured), or hidden variables (inferred). This +//! is the the mean of the multivariate Gaussian. +//! @tparam Output The non-type template size of the measurement column vector //! z. -//! @tparam Input The type template parameter of the control u. A `void` input -//! type can be used for systems with no input control to disable all of the -//! input control features, the control transition matrix G support, and the -//! other related computations from the filter. +//! @tparam Input The non-type template size of the control column vector u. A +//! `void` input type can be used for systems with no input control to disable +//! all of the input control features, the control transition matrix G support, +//! and the other related computations from the filter. //! @tparam Transpose The customization point object template parameter of the //! matrix transpose functor. //! @tparam Symmetrize The customization point object template parameter of the @@ -153,12 +155,13 @@ struct identity_matrix { //! re-initializations but to what default? //! @todo Could the Input be void by default? Or empty? //! @todo Expand std::format support with standard arguments and Eigen3 types. -template < - typename State = double, typename Output = State, typename Input = void, - typename Transpose = std::identity, typename Symmetrize = std::identity, - typename Divide = std::divides, typename Identity = identity_matrix, - typename UpdateTypes = internal::empty_pack_t, - typename PredictionTypes = internal::empty_pack_t> +template , + typename Identity = identity_matrix, + typename UpdateTypes = internal::empty_pack_t, + typename PredictionTypes = internal::empty_pack_t> class kalman { private: @@ -170,8 +173,8 @@ class kalman //! @brief The internal implementation unpacks the parameter packs from //! tuple-like types which allows for multiple parameter pack deductions. using implementation = - internal::kalman, + internal::kalman, internal::repack_t>; //! @} @@ -180,6 +183,9 @@ class kalman //! @name Public Member Types //! @{ + //! @brief Type of the filter scalar data. + using value_type = typename implementation::value_type; + //! @brief Type of the state estimate column vector X. using state = typename implementation::state; @@ -385,7 +391,7 @@ class kalman //! @complexity Constant. [[nodiscard("The returned control column vector U is unexpectedly " "discarded.")]] inline constexpr auto - u() const -> input requires(!std::is_void_v) + u() const -> input requires(Input > 0) { return filter.u; } @@ -870,7 +876,7 @@ class kalman //! @complexity Constant. [[nodiscard("The returned control transition matrix G is unexpectedly " "discarded.")]] inline constexpr auto - g() const -> input_control requires(!std::is_void_v) + g() const -> input_control requires(Input > 0) { return filter.g; } @@ -880,8 +886,7 @@ class kalman //! @param value The copied control transition matrix G. //! //! @complexity Constant. - inline constexpr void - g(const input_control &value) requires(!std::is_void_v) + inline constexpr void g(const input_control &value) requires(Input > 0) { filter.g = value; } @@ -891,8 +896,7 @@ class kalman //! @param value The moved control transition matrix G. //! //! @complexity Constant. - inline constexpr void - g(input_control &&value) requires(!std::is_void_v) + inline constexpr void g(input_control &&value) requires(Input > 0) { filter.g = std::move(value); } @@ -906,10 +910,9 @@ class kalman //! //! @complexity Constant. inline constexpr void g(const auto &value, const auto &...values) requires( - !std::is_void_v && - !std::is_assignable_v< - typename implementation::transition_control_function, - std::decay_t>) + Input > 0 && !std::is_assignable_v< + typename implementation::transition_control_function, + std::decay_t>) { filter.g = std::move(input_control{ value, values... }); } @@ -923,10 +926,9 @@ class kalman //! //! @complexity Constant. inline constexpr void g(auto &&value, auto &&...values) requires( - !std::is_void_v && - !std::is_assignable_v< - typename implementation::transition_control_function, - std::decay_t>) + Input > 0 && !std::is_assignable_v< + typename implementation::transition_control_function, + std::decay_t>) { filter.g = std::move(input_control{ std::forward(value), @@ -943,7 +945,7 @@ class kalman //! //! @complexity Constant. inline constexpr void g(const auto &callable) requires( - !std::is_void_v && + Input > 0 && std::is_assignable_v>) { @@ -960,7 +962,7 @@ class kalman //! //! @complexity Constant. inline constexpr void g(auto &&callable) requires( - !std::is_void_v && + Input > 0 && std::is_assignable_v>) { diff --git a/sample/dog_position.cpp b/sample/dog_position.cpp index 4d6d3c253a..3cba450903 100644 --- a/sample/dog_position.cpp +++ b/sample/dog_position.cpp @@ -32,7 +32,7 @@ namespace //! //! @example dog_position.cpp [[maybe_unused]] auto dog_position{ [] { - using kalman = fcarouge::kalman; + using kalman = fcarouge::kalman; kalman k; // Initialization diff --git a/support/include/fcarouge/format b/support/include/fcarouge/format index c7b5cac2ef..ef2a14a5dc 100644 --- a/support/include/fcarouge/format +++ b/support/include/fcarouge/format @@ -54,11 +54,14 @@ For more information, please refer to */ #include #include +#include + namespace fcarouge { -template +template class kalman; } // namespace fcarouge @@ -102,17 +105,18 @@ template } } // namespace std -template +template struct fmt::formatter< - fcarouge::kalman, + fcarouge::kalman, Char> : public std::formatter< - fcarouge::kalman, + fcarouge::kalman, Char> { }; diff --git a/test/format.cpp b/test/format.cpp index 29cbeda554..825f3419c9 100644 --- a/test/format.cpp +++ b/test/format.cpp @@ -51,7 +51,7 @@ namespace //! @test Verifies formatting filters for single-dimension filters with input //! control. [[maybe_unused]] auto format111{ [] { - using kalman = fcarouge::kalman; + using kalman = fcarouge::kalman; kalman k; assert(std::format("{}", k) == diff --git a/test/initialization.cpp b/test/initialization.cpp index 055986cb61..9b3694948d 100644 --- a/test/initialization.cpp +++ b/test/initialization.cpp @@ -67,7 +67,7 @@ namespace //! @test Verifies default values are initialized for single-dimension filters //! with input control. [[maybe_unused]] auto defaults111{ [] { - using kalman = fcarouge::kalman; + using kalman = fcarouge::kalman; kalman k; assert(k.f() == 1);