Skip to content

Commit

Permalink
Merge 719e115 into f4d1e00
Browse files Browse the repository at this point in the history
  • Loading branch information
FrancoisCarouge committed Feb 16, 2024
2 parents f4d1e00 + 719e115 commit 78d631e
Show file tree
Hide file tree
Showing 4 changed files with 64 additions and 31 deletions.
12 changes: 6 additions & 6 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -202,9 +202,9 @@ class kalman
| `gain` | x by z | Type of the gain matrix `k`. | *K*, *L* |
| `innovation_uncertainty` | z by z | Type of the innovation uncertainty matrix `s`. | *S* |
| `innovation` | z by 1 | Type of the innovation column vector `y`. | *Y* |
| `input_control` | x by u | Type of the control transition matrix `g`. | *G*, *B* |
| `input` | u by 1 | Type of the control column vector `u`. | *U* |
| `output_model` | z by x | Type of the observation transition matrix `h`. | *H*, *C* |
| `input_control` | x by u | Type of the control transition matrix `g`. This member type is defined only if the filter supports input control. | *G*, *B* |
| `input` | u by 1 | Type of the control column vector `u`. This member type is defined only if the filter supports input. | *U* |
| `output_model` | z by x | Type of the observation transition matrix `h`. This member type is defined only if the filter supports output model. | *H*, *C* |
| `output_uncertainty` | z by z | Type of the observation, measurement noise covariance matrix `r`. | *R* |
| `output` | z by 1 | Type of the observation column vector `z`. | *Z*, *Y*, *O* |
| `process_uncertainty` | x by x | Type of the process noise covariance matrix `q`. | *Q* |
Expand All @@ -224,14 +224,14 @@ class kalman
| Characteristic | Definition |
| --- | --- |
| `f` | Manages the state transition matrix *F*. Gets the value. Initializes and sets the value. Configures the callable object of expression `state_transition(const state &, const input &, const PredictionTypes &...)` to compute the value. The default value is the identity matrix. |
| `g` | Manages the control transition matrix *G*. Gets the value. Initializes and sets the value. Configures the callable object of expression `input_control(const PredictionTypes &...)` to compute the value. The default value is the identity matrix. |
| `h` | Manages the observation transition matrix *H*. Gets the value. Initializes and sets the value. Configures the callable object of expression `output_model(const state &, const UpdateTypes &...)` to compute the value. The default value is the identity matrix. |
| `g` | Manages the control transition matrix *G*. Gets the value. Initializes and sets the value. Configures the callable object of expression `input_control(const PredictionTypes &...)` to compute the value. The default value is the identity matrix. This member function is defined only if the filter supports input control. |
| `h` | Manages the observation transition matrix *H*. Gets the value. Initializes and sets the value. Configures the callable object of expression `output_model(const state &, const UpdateTypes &...)` to compute the value. The default value is the identity matrix. This member function is defined only if the filter supports output model. |
| `k` | Manages the gain matrix *K*. Gets the value last computed during the update. The default value is the identity matrix. |
| `p` | Manages the estimated covariance matrix *P*. Gets the value. Initializes and sets the value. The default value is the identity matrix. |
| `q` | Manages the process noise covariance matrix *Q* from the process noise *w* expected value *E[wwᵀ]* and its variance *σ²* found by measuring, tuning, educated guesses of the noise. Gets the value. Initializes and sets the value. Configures the callable object of expression `process_uncertainty(const state &, const PredictionTypes &...)` to compute the value. The default value is the null matrix. |
| `r` | Manages the observation, measurement noise covariance matrix *R* from the measurement noise *v* expected value *E[vvᵀ]* and its variance *σ²* found by measuring, tuning, educated guesses of the noise. Gets the value. Initializes and sets the value. Configures the callable object of expression `output_uncertainty(const state &, const output &, const UpdateTypes &...)` to compute the value. The default value is the null matrix. |
| `s` | Manages the innovation uncertainty matrix *S*. Gets the value last computed during the update. The default value is the identity matrix. |
| `u` | Manages the control column vector *U*. Gets the value last used in prediction. |
| `u` | Manages the control column vector *U*. Gets the value last used in prediction. This member function is defined only if the filter supports input. |
| `x` | Manages the state estimate column vector *X*. Gets the value. Initializes and sets the value. The default value is the null column vector. |
| `y` | Manages the innovation column vector *Y*. Gets the value last computed during the update. The default value is the null column vector. |
| `z` | Manages the observation column vector *Z*. Gets the value last used during the update. The default value is the null column vector. |
Expand Down
9 changes: 7 additions & 2 deletions include/fcarouge/internal/kalman.tpp
Original file line number Diff line number Diff line change
Expand Up @@ -233,8 +233,13 @@ kalman<State, Output, Input, UpdateTypes, PredictionTypes>::h(
const auto &value, const auto &...values)
requires(has_output_model<implementation>)
{
if constexpr (std::is_convertible_v<decltype(value), output_model>) {
filter.h = std::move(output_model{value, values...});
if constexpr (std::is_convertible_v<
decltype(value),
typename kalman<State, Output, Input, UpdateTypes,
PredictionTypes>::output_model>) {
filter.h = std::move(
typename kalman<State, Output, Input, UpdateTypes,
PredictionTypes>::output_model{value, values...});
} else {
using observation_state_function = decltype(filter.observation_state_h);
filter.observation_state_h =
Expand Down
38 changes: 38 additions & 0 deletions include/fcarouge/internal/utility.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,44 @@ template <typename Filter>
concept has_output_model =
has_output_model_member<Filter> || has_output_model_method<Filter>;

// There is only one known way to do conditional member types: partial
// specialization of class templates.
template <typename Filter> struct conditional_input {};

template <has_input Filter> struct conditional_input<Filter> {
//! @brief Type of the control column vector U.
//!
//! @details This member type is conditionally present. The presence of the
//! member depends on the filter capabilities.
using input = Filter::input;
};

template <typename Filter> struct conditional_input_control {};

template <has_input_control Filter> struct conditional_input_control<Filter> {
//! @brief Type of the control transition matrix G.
//!
//! @details Also known as B. This member type is conditionally present. The
//! presence of the member depends on the filter capabilities.
using input_control = Filter::input_control;
};

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.
using output_model = Filter::output_model;
};

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

struct empty {
inline constexpr explicit empty([[maybe_unused]] auto &&...any) noexcept {
// Constructs from anything for all initializations compatibility.
Expand Down
36 changes: 13 additions & 23 deletions include/fcarouge/kalman.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -157,14 +157,16 @@ namespace fcarouge {
template <typename State = double, typename Output = double,
typename Input = void, typename UpdateTypes = empty_pack,
typename PredictionTypes = empty_pack>
class kalman final {
class kalman final : public internal::conditional_member_types<internal::kalman<
State, Output, Input, internal::repack_t<UpdateTypes>,
internal::repack_t<PredictionTypes>>> {
private:
//! @name Private Member Types
//! @{
//! @brief Implementation details of the filter.
//!
//! @brief The internal implementation unpacks the parameter packs from
//! tuple-like types which allows for multiple parameter pack deductions.
//! @details The internal implementation, filtering strategies, and presence
//! of members vary based on the configured filter.
using implementation =
internal::kalman<State, Output, Input, internal::repack_t<UpdateTypes>,
internal::repack_t<PredictionTypes>>;
Expand All @@ -187,11 +189,6 @@ class kalman final {
//! @details Also known as Y or O.
using output = implementation::output;

//! @brief Type of the control column vector U.
//!
//! @todo Conditionally remove this member type when no input is present.
using input = implementation::input;

//! @brief Type of the estimated correlated variance matrix P.
//!
//! @details Also known as Σ.
Expand All @@ -208,18 +205,6 @@ class kalman final {
//! @details Also known as the fundamental matrix, propagation, Φ, or A.
using state_transition = implementation::state_transition;

//! @brief Type of the observation transition matrix H.
//!
//! @details Also known as the measurement transition matrix or C.
using output_model = implementation::output_model;

//! @brief Type of the control transition matrix G.
//!
//! @details Also known as B.
//!
//! @todo Conditionally remove this member type when no input is present.
using input_control = implementation::input_control;

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

Expand Down Expand Up @@ -447,6 +432,9 @@ class kalman final {

//! @brief Returns the observation transition matrix H.
//!
//! @details This member function is not present when the filter has no output
//! model.
//!
//! @return The observation, measurement transition matrix H.
//!
//! @complexity Constant.
Expand All @@ -462,7 +450,8 @@ class kalman final {
//! &...)`. For non-linear system, or extended filter, H is the Jacobian of
//! the state observation function: `H = ∂h/∂X = ∂hj/∂xi` that is each row i
//! contains the derivatives of the state observation function for every
//! element j in the state column vector X.
//! element j in the state column vector X. This member function is not
//! present when the filter has no output model.
//!
//! @param value The first copied initializer used to set the copied
//! observation transition matrix H or the copied target Callable object
Expand All @@ -479,7 +468,8 @@ class kalman final {

//! @brief Returns the control transition matrix G.
//!
//! @details This member function is not present when the filter has no input.
//! @details This member function is not present when the filter has no input
//! control.
//!
//! @return The control transition matrix G.
//!
Expand All @@ -493,7 +483,7 @@ class kalman final {
//!
//! @details The control transition matrix G is of type `input_control` and
//! the function is of the form `input_control(const PredictionTypes &...)`.
//! This member function is not present when the filter has no input.
//! This member function is not present when the filter has no input control.
//!
//! @param value The first copied initializer used to set the copied control
//! transition matrix G or the copied target Callable object (function object,
Expand Down

0 comments on commit 78d631e

Please sign in to comment.