diff --git a/README.md b/README.md index f014a8966..68d7e059f 100644 --- a/README.md +++ b/README.md @@ -244,6 +244,8 @@ The member types are optionally present according to the filter configuration. | `x` | Manages the state estimate column vector *X*. Gets or 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. | +| `transition` | Manages the state transition function object *f*. Configures the callable object of expression `state(const state &, const input &, const PredictionTypes &...)` to compute the transition state value. The default value is the equivalent to *f(x) = F * X*. The default function is suitable for linear systems. For extended filters `transition` is a linearization of the state transition while *F* is the Jacobian of the transition function: *F = ∂f/∂X = ∂fj/∂xi* that is each row *i* contains the derivatives of the state transition function for every element *j* in the state column vector *X*. | +| `observation` | Manages the state observation function object *h*. Configures the callable object of expression `output(const state &, const UpdateTypes &...arguments)` to compute the observation state value. The default value is the equivalent to *h(x) = H * X*. The default function is suitable for linear systems. For extended filters `observation` is a linearization of the state observation while *H* is the Jacobian of the 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 vector *X*. | The characteristics are optionally present according to the filter configuration. diff --git a/include/fcarouge/internal/kalman.tpp b/include/fcarouge/internal/kalman.tpp index 14ee6f664..e99e4c865 100644 --- a/include/fcarouge/internal/kalman.tpp +++ b/include/fcarouge/internal/kalman.tpp @@ -224,6 +224,16 @@ kalman::s() const -> const innovation_uncertainty & { return filter.s; } +template +inline constexpr void kalman::transition(const auto &callable) { + filter.transition = callable; +} + +template +inline constexpr void kalman::observation(const auto &callable) { + filter.observation = callable; +} + template inline constexpr void kalman::update(const auto &...arguments) { filter.update(arguments...); diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index 458553556..e6c4d8ed9 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -493,6 +493,34 @@ class kalman final : public internal::conditional_member_types { inline constexpr auto s() const -> const innovation_uncertainty &; //! @} + //! @brief Sets the extended state transition function f(x). + //! + //! @param callable The copied target Callable object (function object, + //! pointer to function, reference to function, pointer to member function, or + //! pointer to data member) that will be called to compute the next state X on + //! prediction steps of expression `state(const state &, const input &, const + //! PredictionTypes &...)`. The default function `f(x) = F * X` is suitable + //! for linear systems. For non-linear system, or extended filter, implement a + //! linearization of the transition function f and the state transition F + //! matrix is the Jacobian of the state transition function. + //! + //! @complexity Constant. + inline constexpr void transition(const auto &callable); + + //! @brief Sets the extended state observation function h(x). + //! + //! @param callable The copied target Callable object (function object, + //! pointer to function, reference to function, pointer to member function, or + //! pointer to data member) that will be called to compute the observation Z + //! on update steps of expression `output(const state &, const UpdateTypes + //! &...arguments)`. The default function `h(x) = H * X` is suitable for + //! linear systems. For non-linear system, or extended filter, the client + //! implements a linearization of the observation function hand the state + //! observation H matrix is the Jacobian of the state observation function. + //! + //! @complexity Constant. + inline constexpr void observation(const auto &callable); + //! @name Public Filtering Member Functions //! @{