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 c91fcb9
Show file tree
Hide file tree
Showing 4 changed files with 150 additions and 39 deletions.
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
60 changes: 58 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,35 @@ 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;
};

// 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> {};

struct empty {
inline constexpr explicit empty([[maybe_unused]] auto &&...any) noexcept {
Expand Down
39 changes: 23 additions & 16 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,12 +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.
Expand Down Expand Up @@ -360,8 +358,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 +377,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 +388,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 +407,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 +439,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
21 changes: 21 additions & 0 deletions include/fcarouge/utility.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -78,13 +78,34 @@ concept eigen = internal::eigen<Type>;
template <typename Filter>
concept has_input = internal::has_input<Filter>;

//! @brief Filter process uncertainty support concept.
//!
//! @details The filter supports the process uncertainty related functionality:
//! `process_uncertainty` type member and `q()` method.
template <typename Filter>
concept has_process_uncertainty = internal::has_process_uncertainty<Filter>;

//! @brief Filter output uncertainty support concept.
//!
//! @details The filter supports the output uncertainty related functionality:
//! `output_uncertainty` type member and `r()` method.
template <typename Filter>
concept has_output_uncertainty = internal::has_output_uncertainty<Filter>;

//! @brief Filter input control support concept.
//!
//! @details The filter supports the input control related functionality:
//! `input_control` type member and `g()` method.
template <typename Filter>
concept has_input_control = internal::has_input_control<Filter>;

//! @brief Filter state transition support concept.
//!
//! @details The filter supports the state transition related functionality:
//! `state_transition` type member and `f()` method.
template <typename Filter>
concept has_state_transition = internal::has_state_transition<Filter>;

//! @brief Filter output model support concept.
//!
//! @details The filter supports the output model related functionality:
Expand Down

0 comments on commit c91fcb9

Please sign in to comment.