Skip to content

Commit

Permalink
[filter] more optional internals
Browse files Browse the repository at this point in the history
  • Loading branch information
FrancoisCarouge committed May 20, 2024
1 parent acd4e03 commit e781c56
Show file tree
Hide file tree
Showing 5 changed files with 180 additions and 48 deletions.
23 changes: 19 additions & 4 deletions include/fcarouge/internal/format.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,8 +70,14 @@ struct std::formatter<
auto format(const kalman &filter,
std::basic_format_context<OutputIt, Char> &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<kalman>) {
format_context.advance_to(
format_to(format_context.out(), R"("f": {}, )", filter.f()));
}

if constexpr (fcarouge::internal::has_input_control_method<kalman>) {
format_context.advance_to(
Expand All @@ -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<kalman>) {
format_context.advance_to(
format_to(format_context.out(), R"("q": {}, )", filter.q()));
}

if constexpr (fcarouge::internal::has_process_uncertainty_method<kalman>) {
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.
Expand Down
69 changes: 48 additions & 21 deletions include/fcarouge/internal/kalman.tpp
Original file line number Diff line number Diff line change
Expand Up @@ -113,28 +113,37 @@ kalman<State, Output, Input, UpdateTypes, PredictionTypes>::p(
template <typename State, typename Output, typename Input, typename UpdateTypes,
typename PredictionTypes>
[[nodiscard("The returned process noise covariance matrix Q is unexpectedly "
"discarded.")]] inline constexpr auto
"discarded.")]] inline constexpr const auto &
kalman<State, Output, Input, UpdateTypes, PredictionTypes>::q() const
-> const process_uncertainty & {
requires(has_process_uncertainty<implementation>)
{
return filter.q;
}

template <typename State, typename Output, typename Input, typename UpdateTypes,
typename PredictionTypes>
[[nodiscard("The returned process noise covariance matrix Q is unexpectedly "
"discarded.")]] inline constexpr auto
"discarded.")]] inline constexpr auto &
kalman<State, Output, Input, UpdateTypes, PredictionTypes>::q()
-> process_uncertainty & {
requires(has_process_uncertainty<implementation>)
{
return filter.q;
}

template <typename State, typename Output, typename Input, typename UpdateTypes,
typename PredictionTypes>
inline constexpr void
kalman<State, Output, Input, UpdateTypes, PredictionTypes>::q(
const auto &value, const auto &...values) {
if constexpr (std::is_convertible_v<decltype(value), process_uncertainty>) {
filter.q = std::move(process_uncertainty{value, values...});
const auto &value, const auto &...values)
requires(has_process_uncertainty<implementation>)
{
if constexpr (std::is_convertible_v<
decltype(value),
typename kalman<State, Output, Input, UpdateTypes,
PredictionTypes>::process_uncertainty>) {
filter.q = std::move(typename kalman<State, Output, Input, UpdateTypes,
PredictionTypes>::process_uncertainty{
value, values...});
} else {
using noise_process_function = decltype(filter.noise_process_q);
filter.noise_process_q =
Expand All @@ -145,28 +154,37 @@ kalman<State, Output, Input, UpdateTypes, PredictionTypes>::q(
template <typename State, typename Output, typename Input, typename UpdateTypes,
typename PredictionTypes>
[[nodiscard("The returned observation noise covariance matrix R is "
"unexpectedly discarded.")]] inline constexpr auto
"unexpectedly discarded.")]] inline constexpr const auto &
kalman<State, Output, Input, UpdateTypes, PredictionTypes>::r() const
-> const output_uncertainty & {
requires(has_output_uncertainty<implementation>)
{
return filter.r;
}

template <typename State, typename Output, typename Input, typename UpdateTypes,
typename PredictionTypes>
[[nodiscard("The returned observation noise covariance matrix R is "
"unexpectedly discarded.")]] inline constexpr auto
"unexpectedly discarded.")]] inline constexpr auto &
kalman<State, Output, Input, UpdateTypes, PredictionTypes>::r()
-> output_uncertainty & {
requires(has_output_uncertainty<implementation>)
{
return filter.r;
}

template <typename State, typename Output, typename Input, typename UpdateTypes,
typename PredictionTypes>
inline constexpr void
kalman<State, Output, Input, UpdateTypes, PredictionTypes>::r(
const auto &value, const auto &...values) {
if constexpr (std::is_convertible_v<decltype(value), output_uncertainty>) {
filter.r = std::move(output_uncertainty{value, values...});
const auto &value, const auto &...values)
requires(has_output_uncertainty<implementation>)
{
if constexpr (std::is_convertible_v<
decltype(value),
typename kalman<State, Output, Input, UpdateTypes,
PredictionTypes>::output_uncertainty>) {
filter.r = std::move(
typename kalman<State, Output, Input, UpdateTypes,
PredictionTypes>::output_uncertainty{value, values...});
} else {
using noise_observation_function = decltype(filter.noise_observation_r);
filter.noise_observation_r =
Expand All @@ -177,28 +195,37 @@ kalman<State, Output, Input, UpdateTypes, PredictionTypes>::r(
template <typename State, typename Output, typename Input, typename UpdateTypes,
typename PredictionTypes>
[[nodiscard("The returned state transition matrix F is unexpectedly "
"discarded.")]] inline constexpr auto
"discarded.")]] inline constexpr const auto &
kalman<State, Output, Input, UpdateTypes, PredictionTypes>::f() const
-> const state_transition & {
requires(has_state_transition<implementation>)
{
return filter.f;
}

template <typename State, typename Output, typename Input, typename UpdateTypes,
typename PredictionTypes>
[[nodiscard("The returned state transition matrix F is unexpectedly "
"discarded.")]] inline constexpr auto
"discarded.")]] inline constexpr auto &
kalman<State, Output, Input, UpdateTypes, PredictionTypes>::f()
-> state_transition & {
requires(has_state_transition<implementation>)
{
return filter.f;
}

template <typename State, typename Output, typename Input, typename UpdateTypes,
typename PredictionTypes>
inline constexpr void
kalman<State, Output, Input, UpdateTypes, PredictionTypes>::f(
const auto &value, const auto &...values) {
if constexpr (std::is_convertible_v<decltype(value), state_transition>) {
filter.f = std::move(state_transition{value, values...});
const auto &value, const auto &...values)
requires(has_state_transition<implementation>)
{
if constexpr (std::is_convertible_v<
decltype(value),
typename kalman<State, Output, Input, UpdateTypes,
PredictionTypes>::state_transition>) {
filter.f = std::move(
typename kalman<State, Output, Input, UpdateTypes,
PredictionTypes>::state_transition{value, values...});
} else {
using transition_state_function = decltype(filter.transition_state_f);
filter.transition_state_f =
Expand Down
71 changes: 69 additions & 2 deletions include/fcarouge/internal/utility.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,13 +55,39 @@ concept eigen = requires { typename Type::PlainMatrix; };
template <typename Filter>
concept has_input_member = requires(Filter filter) { filter.u; };

//! @todo Is the _method concept extraneous or incorrect? Explain the
//! shortcoming?

template <typename Filter>
concept has_input_method = requires(Filter filter) { filter.u(); };

//! @todo Shorten when MSVC has better if-constexpr-requires support.
template <typename Filter>
concept has_input = has_input_member<Filter> || has_input_method<Filter>;

template <typename Filter>
concept has_process_uncertainty_member = requires(Filter filter) { filter.q; };

template <typename Filter>
concept has_process_uncertainty_method =
requires(Filter filter) { filter.q(); };

//! @todo Shorten when MSVC has better if-constexpr-requires support.
template <typename Filter>
concept has_process_uncertainty = has_process_uncertainty_member<Filter> ||
has_process_uncertainty_method<Filter>;

template <typename Filter>
concept has_output_uncertainty_member = requires(Filter filter) { filter.r; };

template <typename Filter>
concept has_output_uncertainty_method = requires(Filter filter) { filter.r(); };

//! @todo Shorten when MSVC has better if-constexpr-requires support.
template <typename Filter>
concept has_output_uncertainty = has_output_uncertainty_member<Filter> ||
has_output_uncertainty_method<Filter>;

template <typename Filter>
concept has_input_control_member = requires(Filter filter) { filter.g; };

Expand All @@ -73,6 +99,17 @@ template <typename Filter>
concept has_input_control =
has_input_control_member<Filter> || has_input_control_method<Filter>;

template <typename Filter>
concept has_state_transition_member = requires(Filter filter) { filter.f; };

template <typename Filter>
concept has_state_transition_method = requires(Filter filter) { filter.f(); };

//! @todo Shorten when MSVC has better if-constexpr-requires support.
template <typename Filter>
concept has_state_transition =
has_state_transition_member<Filter> || has_state_transition_method<Filter>;

template <typename Filter>
concept has_output_model_member = requires(Filter filter) { filter.h; };

Expand Down Expand Up @@ -111,16 +148,46 @@ template <typename Filter> struct conditional_output_model {};
template <has_output_model Filter> struct conditional_output_model<Filter> {
//! @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 <typename Filter> struct conditional_process_uncertainty {};

template <has_process_uncertainty Filter>
struct conditional_process_uncertainty<Filter> {
//! @brief Type of the process noise correlated variance matrix Q.
using process_uncertainty = Filter::process_uncertainty;
};

template <typename Filter> struct conditional_output_uncertainty {};

template <has_output_uncertainty Filter>
struct conditional_output_uncertainty<Filter> {
//! @brief Type of the observation noise correlated variance matrix R.
using output_uncertainty = Filter::output_uncertainty;
};

template <typename Filter> struct conditional_state_transition {};

template <has_state_transition Filter>
struct conditional_state_transition<Filter> {
//! @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 <typename Filter>
struct conditional_member_types : public conditional_input<Filter>,
conditional_input_control<Filter>,
conditional_output_model<Filter> {};
conditional_output_model<Filter>,
conditional_process_uncertainty<Filter>,
conditional_output_uncertainty<Filter>,
conditional_state_transition<Filter> {};

struct empty {
inline constexpr explicit empty([[maybe_unused]] auto &&...any) noexcept {
Expand Down
44 changes: 23 additions & 21 deletions include/fcarouge/kalman.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -167,7 +167,8 @@ class kalman final : public internal::conditional_member_types<internal::kalman<
//! @brief Implementation details of the filter.
//!
//! @details The internal implementation, filtering strategies, and presence
//! of members vary based on the configured filter.
//! of members vary based on the constructed, configured, declared, deduced
//! filter.
using implementation =
internal::kalman<State, Output, Input, internal::repack_t<UpdateTypes>,
internal::repack_t<PredictionTypes>>;
Expand All @@ -177,6 +178,9 @@ class kalman final : public internal::conditional_member_types<internal::kalman<
//! @{

//! @brief Encapsulates the implementation details of the filter.
//!
//! @details Optionally exposes a variety of members and methods according to
//! the selected implementation.
implementation filter;
//! @}

Expand All @@ -197,17 +201,6 @@ class kalman final : public internal::conditional_member_types<internal::kalman<
//! @details Also known as Σ.
using estimate_uncertainty = implementation::estimate_uncertainty;

//! @brief Type of the process noise correlated variance matrix Q.
using process_uncertainty = implementation::process_uncertainty;

//! @brief Type of the observation noise correlated variance matrix R.
using output_uncertainty = implementation::output_uncertainty;

//! @brief Type of the state transition matrix F.
//!
//! @details Also known as the fundamental matrix, propagation, Φ, or A.
using state_transition = implementation::state_transition;

//! @brief Type of the gain matrix K.
using gain = implementation::gain;

Expand Down Expand Up @@ -360,8 +353,10 @@ class kalman final : public internal::conditional_member_types<internal::kalman<
//! @return The process noise correlated variance matrix Q.
//!
//! @complexity Constant.
inline constexpr auto q() const -> const process_uncertainty &;
inline constexpr auto q() -> process_uncertainty &;
inline constexpr const auto &q() const
requires(has_process_uncertainty<implementation>);
inline constexpr auto &q()
requires(has_process_uncertainty<implementation>);

//! @brief Sets the process noise covariance matrix Q.
//!
Expand All @@ -377,7 +372,8 @@ class kalman final : public internal::conditional_member_types<internal::kalman<
//! process noise covariance matrix Q.
//!
//! @complexity Constant.
inline constexpr void q(const auto &value, const auto &...values);
inline constexpr void q(const auto &value, const auto &...values)
requires(has_process_uncertainty<implementation>);

//! @brief Returns the observation noise covariance
//! matrix R.
Expand All @@ -387,8 +383,10 @@ class kalman final : public internal::conditional_member_types<internal::kalman<
//! @return The observation noise correlated variance matrix R.
//!
//! @complexity Constant.
inline constexpr auto r() const -> const output_uncertainty &;
inline constexpr auto r() -> output_uncertainty &;
inline constexpr const auto &r() const
requires(has_output_uncertainty<implementation>);
inline constexpr auto &r()
requires(has_output_uncertainty<implementation>);

//! @brief Sets the observation noise covariance matrix R.
//!
Expand All @@ -404,15 +402,18 @@ class kalman final : public internal::conditional_member_types<internal::kalman<
//! observation noise covariance matrix R.
//!
//! @complexity Constant.
inline constexpr void r(const auto &value, const auto &...values);
inline constexpr void r(const auto &value, const auto &...values)
requires(has_output_uncertainty<implementation>);

//! @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<implementation>);
inline constexpr auto &f()
requires(has_state_transition<implementation>);

//! @brief Sets the state transition matrix F.
//!
Expand All @@ -433,7 +434,8 @@ class kalman final : public internal::conditional_member_types<internal::kalman<
//! state transition matrix F.
//!
//! @complexity Constant.
inline constexpr void f(const auto &value, const auto &...values);
inline constexpr void f(const auto &value, const auto &...values)
requires(has_state_transition<implementation>);

//! @brief Returns the observation transition matrix H.
//!
Expand Down

0 comments on commit e781c56

Please sign in to comment.