diff --git a/include/fcarouge/internal/format.hpp b/include/fcarouge/internal/format.hpp index cd77062a9..71051d621 100644 --- a/include/fcarouge/internal/format.hpp +++ b/include/fcarouge/internal/format.hpp @@ -70,8 +70,14 @@ struct std::formatter< auto format(const kalman &filter, std::basic_format_context &format_context) const -> OutputIt { + format_context.advance_to( - format_to(format_context.out(), R"({{"f": {}, )", filter.f())); + format_to(format_context.out(), R"({{)", filter.f())); + + if constexpr (fcarouge::internal::has_state_transition_method) { + format_context.advance_to( + format_to(format_context.out(), R"("f": {}, )", filter.f())); + } if constexpr (fcarouge::internal::has_input_control_method) { format_context.advance_to( @@ -98,9 +104,18 @@ struct std::formatter< }); } - format_context.advance_to(format_to(format_context.out(), - R"("q": {}, "r": {}, "s": {}, )", - filter.q(), filter.r(), filter.s())); + if constexpr (fcarouge::internal::has_output_uncertainty_method) { + format_context.advance_to( + format_to(format_context.out(), R"("q": {}, )", filter.q())); + } + + if constexpr (fcarouge::internal::has_process_uncertainty_method) { + format_context.advance_to( + format_to(format_context.out(), R"("r": {}, )", filter.r())); + } + + format_context.advance_to( + format_to(format_context.out(), R"("s": {}, )", filter.s())); //! @todo Generalize out internal method concept when MSVC has better //! if-constexpr-requires support. diff --git a/include/fcarouge/internal/kalman.tpp b/include/fcarouge/internal/kalman.tpp index 2ee63a175..41b511430 100644 --- a/include/fcarouge/internal/kalman.tpp +++ b/include/fcarouge/internal/kalman.tpp @@ -113,18 +113,20 @@ kalman::p( template [[nodiscard("The returned process noise covariance matrix Q is unexpectedly " - "discarded.")]] inline constexpr auto + "discarded.")]] inline constexpr const auto & kalman::q() const - -> const process_uncertainty & { + requires(has_process_uncertainty) +{ return filter.q; } template [[nodiscard("The returned process noise covariance matrix Q is unexpectedly " - "discarded.")]] inline constexpr auto + "discarded.")]] inline constexpr auto & kalman::q() - -> process_uncertainty & { + requires(has_process_uncertainty) +{ return filter.q; } @@ -132,9 +134,16 @@ template inline constexpr void kalman::q( - const auto &value, const auto &...values) { - if constexpr (std::is_convertible_v) { - filter.q = std::move(process_uncertainty{value, values...}); + const auto &value, const auto &...values) + requires(has_process_uncertainty) +{ + if constexpr (std::is_convertible_v< + decltype(value), + typename kalman::process_uncertainty>) { + filter.q = std::move(typename kalman::process_uncertainty{ + value, values...}); } else { using noise_process_function = decltype(filter.noise_process_q); filter.noise_process_q = @@ -145,18 +154,20 @@ kalman::q( template [[nodiscard("The returned observation noise covariance matrix R is " - "unexpectedly discarded.")]] inline constexpr auto + "unexpectedly discarded.")]] inline constexpr const auto & kalman::r() const - -> const output_uncertainty & { + requires(has_output_uncertainty) +{ return filter.r; } template [[nodiscard("The returned observation noise covariance matrix R is " - "unexpectedly discarded.")]] inline constexpr auto + "unexpectedly discarded.")]] inline constexpr auto & kalman::r() - -> output_uncertainty & { + requires(has_output_uncertainty) +{ return filter.r; } @@ -164,9 +175,16 @@ template inline constexpr void kalman::r( - const auto &value, const auto &...values) { - if constexpr (std::is_convertible_v) { - filter.r = std::move(output_uncertainty{value, values...}); + const auto &value, const auto &...values) + requires(has_output_uncertainty) +{ + if constexpr (std::is_convertible_v< + decltype(value), + typename kalman::output_uncertainty>) { + filter.r = std::move( + typename kalman::output_uncertainty{value, values...}); } else { using noise_observation_function = decltype(filter.noise_observation_r); filter.noise_observation_r = @@ -177,18 +195,20 @@ kalman::r( template [[nodiscard("The returned state transition matrix F is unexpectedly " - "discarded.")]] inline constexpr auto + "discarded.")]] inline constexpr const auto & kalman::f() const - -> const state_transition & { + requires(has_state_transition) +{ return filter.f; } template [[nodiscard("The returned state transition matrix F is unexpectedly " - "discarded.")]] inline constexpr auto + "discarded.")]] inline constexpr auto & kalman::f() - -> state_transition & { + requires(has_state_transition) +{ return filter.f; } @@ -196,9 +216,16 @@ template inline constexpr void kalman::f( - const auto &value, const auto &...values) { - if constexpr (std::is_convertible_v) { - filter.f = std::move(state_transition{value, values...}); + const auto &value, const auto &...values) + requires(has_state_transition) +{ + if constexpr (std::is_convertible_v< + decltype(value), + typename kalman::state_transition>) { + filter.f = std::move( + typename kalman::state_transition{value, values...}); } else { using transition_state_function = decltype(filter.transition_state_f); filter.transition_state_f = diff --git a/include/fcarouge/internal/utility.hpp b/include/fcarouge/internal/utility.hpp index 142d7cd02..2fdf9a5e1 100644 --- a/include/fcarouge/internal/utility.hpp +++ b/include/fcarouge/internal/utility.hpp @@ -55,6 +55,9 @@ concept eigen = requires { typename Type::PlainMatrix; }; template concept has_input_member = requires(Filter filter) { filter.u; }; +//! @todo Is the _method concept extraneous or incorrect? Explain the +//! shortcoming? + template concept has_input_method = requires(Filter filter) { filter.u(); }; @@ -62,6 +65,29 @@ concept has_input_method = requires(Filter filter) { filter.u(); }; template concept has_input = has_input_member || has_input_method; +template +concept has_process_uncertainty_member = requires(Filter filter) { filter.q; }; + +template +concept has_process_uncertainty_method = + requires(Filter filter) { filter.q(); }; + +//! @todo Shorten when MSVC has better if-constexpr-requires support. +template +concept has_process_uncertainty = has_process_uncertainty_member || + has_process_uncertainty_method; + +template +concept has_output_uncertainty_member = requires(Filter filter) { filter.r; }; + +template +concept has_output_uncertainty_method = requires(Filter filter) { filter.r(); }; + +//! @todo Shorten when MSVC has better if-constexpr-requires support. +template +concept has_output_uncertainty = has_output_uncertainty_member || + has_output_uncertainty_method; + template concept has_input_control_member = requires(Filter filter) { filter.g; }; @@ -73,6 +99,17 @@ template concept has_input_control = has_input_control_member || has_input_control_method; +template +concept has_state_transition_member = requires(Filter filter) { filter.f; }; + +template +concept has_state_transition_method = requires(Filter filter) { filter.f(); }; + +//! @todo Shorten when MSVC has better if-constexpr-requires support. +template +concept has_state_transition = + has_state_transition_member || has_state_transition_method; + template concept has_output_model_member = requires(Filter filter) { filter.h; }; @@ -111,16 +148,46 @@ template struct conditional_output_model {}; template struct conditional_output_model { //! @brief Type of the observation transition matrix H. //! - //! @details Also known as the measurement transition matrix or C. + //! @details Also known as the measurement transition matrix or C. The + //! presence of the member depends on the filter capabilities. using output_model = Filter::output_model; }; +template struct conditional_process_uncertainty {}; + +template +struct conditional_process_uncertainty { + //! @brief Type of the process noise correlated variance matrix Q. + using process_uncertainty = Filter::process_uncertainty; +}; + +template struct conditional_output_uncertainty {}; + +template +struct conditional_output_uncertainty { + //! @brief Type of the observation noise correlated variance matrix R. + using output_uncertainty = Filter::output_uncertainty; +}; + +template struct conditional_state_transition {}; + +template +struct conditional_state_transition { + //! @brief Type of the state transition matrix F. + //! + //! @details Also known as the fundamental matrix, propagation, Φ, or A. + using state_transition = Filter::state_transition; +}; + // The only way to have a conditional member type is to inherit from a template // specialization on the member type. template struct conditional_member_types : public conditional_input, conditional_input_control, - conditional_output_model {}; + conditional_output_model, + conditional_process_uncertainty, + conditional_output_uncertainty, + conditional_state_transition {}; struct empty { inline constexpr explicit empty([[maybe_unused]] auto &&...any) noexcept { diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index 34de56e85..93207cb35 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -167,7 +167,8 @@ class kalman final : public internal::conditional_member_types, internal::repack_t>; @@ -177,6 +178,9 @@ class kalman final : public internal::conditional_member_types const process_uncertainty &; - inline constexpr auto q() -> process_uncertainty &; + inline constexpr const auto &q() const + requires(has_process_uncertainty); + inline constexpr auto &q() + requires(has_process_uncertainty); //! @brief Sets the process noise covariance matrix Q. //! @@ -377,7 +372,8 @@ class kalman final : public internal::conditional_member_types); //! @brief Returns the observation noise covariance //! matrix R. @@ -387,8 +383,10 @@ class kalman final : public internal::conditional_member_types const output_uncertainty &; - inline constexpr auto r() -> output_uncertainty &; + inline constexpr const auto &r() const + requires(has_output_uncertainty); + inline constexpr auto &r() + requires(has_output_uncertainty); //! @brief Sets the observation noise covariance matrix R. //! @@ -404,15 +402,18 @@ class kalman final : public internal::conditional_member_types); //! @brief Returns the state transition matrix F. //! //! @return The state transition matrix F. //! //! @complexity Constant. - inline constexpr auto f() const -> const state_transition &; - inline constexpr auto f() -> state_transition &; + inline constexpr const auto &f() const + requires(has_state_transition); + inline constexpr auto &f() + requires(has_state_transition); //! @brief Sets the state transition matrix F. //! @@ -433,7 +434,8 @@ class kalman final : public internal::conditional_member_types); //! @brief Returns the observation transition matrix H. //! diff --git a/include/fcarouge/utility.hpp b/include/fcarouge/utility.hpp index c8b156ef8..fb157f7a4 100644 --- a/include/fcarouge/utility.hpp +++ b/include/fcarouge/utility.hpp @@ -78,6 +78,20 @@ concept eigen = internal::eigen; template concept has_input = internal::has_input; +//! @brief Filter process uncertainty support concept. +//! +//! @details The filter supports the process uncertainty related functionality: +//! `process_uncertainty` type member and `q()` method. +template +concept has_process_uncertainty = internal::has_process_uncertainty; + +//! @brief Filter output uncertainty support concept. +//! +//! @details The filter supports the output uncertainty related functionality: +//! `output_uncertainty` type member and `r()` method. +template +concept has_output_uncertainty = internal::has_output_uncertainty; + //! @brief Filter input control support concept. //! //! @details The filter supports the input control related functionality: @@ -85,6 +99,13 @@ concept has_input = internal::has_input; template concept has_input_control = internal::has_input_control; +//! @brief Filter state transition support concept. +//! +//! @details The filter supports the state transition related functionality: +//! `state_transition` type member and `f()` method. +template +concept has_state_transition = internal::has_state_transition; + //! @brief Filter output model support concept. //! //! @details The filter supports the output model related functionality: