diff --git a/README.md b/README.md index d432ba439..b2437ea35 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,6 @@ The library supports simple and extended filters. The update equation uses the J - [6x2 Constant Acceleration Dynamic Model](#6x2-constant-acceleration-dynamic-model) - [4x1 Non-Linear Dynamic Model](#4x1-non-linear-dynamic-model) - [Continuous Integration & Deployment Actions](#continuous-integration--deployment-actions) -- [Motivation](#motivation) -- [Installation](#installation) - [Class kalman](#class-kalman) - [Template Parameters](#template-parameters) - [Member Types](#member-types) @@ -19,6 +17,8 @@ The library supports simple and extended filters. The update equation uses the J - [Characteristics](#characteristics) - [Modifiers](#modifiers) - [Format](#format) +- [Installation](#installation) +- [Motivation](#motivation) - [Resources](#resources) - [License](#license) @@ -162,24 +162,12 @@ k(drift_x, drift_y, position_x, position_y, variometer);
[![Deploy Code Coverage: Coveralls](https://github.com/FrancoisCarouge/Kalman/actions/workflows/deploy_test_coverage_coveralls.yml/badge.svg)](https://github.com/FrancoisCarouge/Kalman/actions/workflows/deploy_test_coverage_coveralls.yml) - -# Motivation - -Kalman filters can be difficult to learn, use, and implement. Users often need fair algebra, domain, and software knowledge. Inadequacy leads to incorrectness, underperformance, and a big ball of mud. - -This package explores what could be a Kalman filter implementation a la standard library. The following concerns and tradeoffs are considered: -- Separation of the application domain. -- Separation of the algebra implementation. -- Generalization of the support. - -# Installation - -See installaton instructions in [INSTALL.txt](INSTALL.txt). - # Class kalman Defined in header [fcarouge/kalman.hpp](include/fcarouge/kalman.hpp) +A Bayesian filter that uses multivariate Gaussians. Applicable for unimodal and uncorrelated uncertainties. Kalman filters assume white noise, propagation and measurement functions are differentiable, and that the uncertainty stays centered on the state estimate. The filter updates estimates by multiplying Gaussians and predicts estimates by adding Gaussians. Design the state (X, P), the process (F, Q), the measurement (Z, R), the measurement function H, and if the system has control inputs (U, B). Designing a filter is as much art as science. Filters with `state x output x input` dimensions as 1x1x1 and 1x1x0 (no input) are supported through the Standard Templated Library (STL). Higher dimension filters require Eigen 3 support. + ```cpp template < typename Type, @@ -199,15 +187,16 @@ class kalman | Template Parameter | Definition | | --- | --- | -| `State` | The type template parameter of the state column vector x. State variables can be observed (measured), or hidden variables (inferred). This is the the mean of the multivariate Gaussian. | -| `Output` | The type template parameter of the measurement column vector z. | -| `Input` | The type template parameter of the control u. A `void` input type can be used for systems with no input control to disable all of the input control features, the control transition matrix G support, and the other related computations from the filter. | -| `Transpose` | The customization point object template parameter of the matrix transpose functor. | -| `Symmetrize` | The customization point object template parameter of the matrix symmetrization functor. | -| `Divide` | The customization point object template parameter of the matrix division functor. | -| `Identity` | The customization point object template parameter of the matrix identity functor. | -| `UpdateTypes` | The additional update function parameter types passed in through a tuple-like parameter type, composing zero or more types. Parameters such as delta times, variances, or linearized values. The parameters are propagated to the function objects used to compute the state observation H and the observation noise R matrices. The parameters are also propagated to the state observation function object h. | -| `PredictionTypes` | The additional prediction function parameter types passed in through a tuple-like parameter type, composing zero or more types. Parameters such as delta times, variances, or linearized values. The parameters are propagated to the function objects used to compute the process noise Q, the state transition F, and the control transition G matrices. The parameters are also propagated to the state transition function object f. | +| `Type` | The type template parameter of the filter data value type used for computation. Defaults to `double`. | +| `State` | The non-type template size of the state column vector x. State variables can be observed (measured), or hidden variables (inferred). This is the the mean of the multivariate Gaussian. Defaults to `1`. | +| `Output` | The non-type template size of the measurement column vector z. Defaults to `1`. | +| `Input` | The non-type template size of the control column vector u. A zero `0` input size value can be used for systems with no input control to disable all of the input control features, the control transition matrix G support, and the other related computations from the filter. Defaults to `0`. | +| `Transpose` | The customization point object template parameter of the matrix transpose functor. Defaults to the standard passthrough `std::identity` function object since the transposed value of an arithmetic type is itself. | +| `Symmetrize` | The customization point object template parameter of the matrix symmetrization functor. Defaults to the standard passthrough `std::identity` function object since the symmetric value of an arithmetic type is itself. | +| `Divide` | The customization point object template parameter of the matrix division functor. Default to the standard division `std::divides` function object. | +| `Identity` | The customization point object template parameter of the matrix identity functor. Defaults to an `identity_matrix` function object returning the arithmetic `1` value. | +| `UpdateTypes` | The additional update function parameter types passed in through a tuple-like parameter type, composing zero or more types. Parameters such as delta times, variances, or linearized values. The parameters are propagated to the function objects used to compute the state observation H and the observation noise R matrices. The parameters are also propagated to the state observation function object h. Defaults to no parameter types, the empty pack. | +| `PredictionTypes` | The additional prediction function parameter types passed in through a tuple-like parameter type, composing zero or more types. Parameters such as delta times, variances, or linearized values. The parameters are propagated to the function objects used to compute the process noise Q, the state transition F, and the control transition G matrices. The parameters are also propagated to the state transition function object f. Defaults to no parameter types, the empty pack. | ## Member Types @@ -272,6 +261,19 @@ std::string message{ std::format("{}", k) }; // {f:1,h:1,k:1,p:1,q:0,r:0,s:1,x:0,y:0,z:0} ``` +# Installation + +See installaton instructions in [INSTALL.txt](INSTALL.txt). + +# Motivation + +Kalman filters can be difficult to learn, use, and implement. Users often need fair algebra, domain, and software knowledge. Inadequacy leads to incorrectness, underperformance, and a big ball of mud. + +This package explores what could be a Kalman filter implementation a la standard library. The following concerns and tradeoffs are considered: +- Separation of the application domain. +- Separation of the algebra implementation. +- Generalization of the support. + # Resources Awesome resources to learn about Kalman filters: diff --git a/include/fcarouge/internal/eigen.hpp b/include/fcarouge/internal/eigen.hpp index cd37c4b43..7a603e517 100644 --- a/include/fcarouge/internal/eigen.hpp +++ b/include/fcarouge/internal/eigen.hpp @@ -39,11 +39,6 @@ For more information, please refer to */ #ifndef FCAROUGE_INTERNAL_EIGEN_HPP #define FCAROUGE_INTERNAL_EIGEN_HPP -//! @file -//! @brief Kalman operation for Eigen 3 types. -//! -//! @details Default customization point objects (CPO). - #include "fcarouge/kalman.hpp" #include @@ -56,213 +51,90 @@ For more information, please refer to */ namespace fcarouge::eigen::internal { -//! @brief Arithmetic concept. template concept arithmetic = std::integral || std::floating_point; -//! @brief Function object for performing Eigen matrix transposition. -//! -//! @details Implemented with the Eigen linear algebra library matrices with -//! sizes fixed at compile-time. -struct transpose { - //! @brief Returns the transpose of `value`. - //! - //! @param value Value to compute the transpose of. - //! - //! @exception May throw implementation-defined exceptions. - [[nodiscard]] inline constexpr auto operator()(const auto &value) const +using empty_pack = fcarouge::internal::empty_pack; + +struct matrix { + template + [[nodiscard]] inline constexpr auto operator()(const Type &value) -> + typename std::decay_t::PlainMatrix { - using type = std::decay_t; - using result_type = - typename Eigen::Matrix; + return value; + } - return result_type{ value.transpose() }; + [[nodiscard]] inline constexpr auto operator()(const arithmetic auto &value) + { + using type = std::decay_t; + return Eigen::Matrix{ value }; } - //! @brief Returns the transpose of `value`. - //! - //! @param value Value to compute the transpose of. - //! - //! @todo Can this be optimized? + template [[nodiscard]] inline constexpr auto - operator()(const arithmetic auto &value) const + operator()(const std::array &value) { - return value; + return Eigen::Matrix{ value.data() }; } }; -//! @brief Function object for performing Eigen matrix symmetrization. -//! -//! @details Implemented with the Eigen linear algebra library matrices with -//! sizes fixed at compile-time. -struct symmetrize { - //! @brief Returns the symmetrized `value`. - //! - //! @param value Value to compute the symmetry of. - //! - //! @exception May throw implementation-defined exceptions. - [[nodiscard]] inline constexpr auto operator()(const auto &value) const +struct transpose { + template + [[nodiscard]] inline constexpr auto operator()(const Type &value) const -> + typename Eigen::Transpose::PlainMatrix { - using result_type = std::decay_t; - - return result_type{ (value + value.transpose()) / 2 }; + return value.transpose(); } +}; - //! @brief Returns the symmetrized `value`. - //! - //! @param value Value to compute the symmetry of. - //! - //! @todo Can this be optimized? - [[nodiscard]] inline constexpr auto - operator()(const arithmetic auto &value) const +struct symmetrize { + //! @todo Protect overflow? Is there a better way? + [[nodiscard]] inline constexpr auto operator()(const auto &value) const { - return value; + return (value + value.transpose()) / 2; } }; -//! @brief Function object for performing Eigen matrix division. -//! -//! @details Implemented with the Eigen linear algebra library matrices with -//! sizes fixed at compile-time. struct divide { - //! @brief Returns the quotient of `numerator` and `denominator`. - //! - //! @param numerator The dividend matrix of the division. N: m x n - //! @param denominator The divisor matrix of the division. D: o x n - //! - //! @return The quotient matrix. Q: m x o - //! - //! @exception May throw implementation-defined exceptions. - //! - //! @todo Why compilation fails if we specify the return type in the body of - //! the function? template - [[nodiscard]] inline constexpr auto - operator()(const Numerator &numerator, const Denominator &denominator) const - -> typename Eigen::Matrix::Scalar, - std::decay_t::RowsAtCompileTime, - std::decay_t::RowsAtCompileTime> - { - return denominator.transpose() - .fullPivHouseholderQr() - .solve(numerator.transpose()) - .transpose(); - } + // Numerator [m x n] / Denominator [o x n] -> Quotient [m x o] + using result = typename Eigen::Matrix< + typename std::decay_t>::Scalar, + std::decay_t>::RowsAtCompileTime, + std::decay_t< + std::invoke_result_t>::RowsAtCompileTime>; - //! @brief Returns the quotient of `numerator` and `denominator`. - //! - //! @param numerator The dividend matrix of the division. N: m x 1 - //! @param denominator The divisor value of the division. - //! - //! @return The quotient column vector. Q: m x 1 - //! - //! @exception May throw implementation-defined exceptions. - //! - //! @todo Simplify implementation. - template + template [[nodiscard]] inline constexpr auto - operator()(const Numerator &numerator, - const arithmetic auto &denominator) const -> - typename Eigen::Vector::Scalar, - std::decay_t::RowsAtCompileTime> + operator()(const Numerator &numerator, const Denominator &denominator) const + -> result { - return Eigen::Matrix::Scalar, 1, 1>{ - denominator - } + matrix to_matrix; + return to_matrix(denominator) .transpose() .fullPivHouseholderQr() - .solve(numerator.transpose()) - .transpose(); - } - - //! @brief Returns the quotient of `numerator` and `denominator`. - //! - //! @param numerator The dividend value of the division. - //! @param denominator The divisor matrix of the division. D: o x 1 - //! - //! @return The quotient row vector. Q: 1 x o - //! - //! @exception May throw implementation-defined exceptions. - //! - //! @todo Simplify implementation. - template - [[nodiscard]] inline constexpr auto - operator()(const arithmetic auto &numerator, - const Denominator &denominator) const -> - typename Eigen::RowVector::Scalar, - std::decay_t::RowsAtCompileTime> - { - return denominator.transpose() - .fullPivHouseholderQr() - .solve(Eigen::Matrix::Scalar, - 1, 1>{ numerator }) - .transpose(); - } - - //! @brief Returns the quotient of `numerator` and `denominator`. - //! - //! @param numerator The dividend value of the division. - //! @param denominator The divisor value of the division. - //! - //! @return The quotient value. - [[nodiscard]] inline constexpr auto - operator()(const arithmetic auto &numerator, - const arithmetic auto &denominator) const - { - return numerator / denominator; + .solve(to_matrix(numerator).transpose()) + .transpose() + .eval(); } }; -//! @brief Function object for providing an Eigen identity matrix. -//! -//! @details Implemented with the Eigen linear algebra library matrices with -//! sizes fixed at compile-time. -//! -//! @note Could this function object template be a variable template as proposed +//! @todo Could this function object template be a variable template as proposed //! in paper P2008R0 entitled "Enabling variable template template parameters"? struct identity_matrix { - //! @brief Returns the identity matrix. - //! - //! @tparam Type The type template parameter of the matrix. - //! - //! @return The identity matrix `diag(1, 1, ..., 1)`. - //! - //! @exception May throw implementation-defined exceptions. template - [[nodiscard]] inline constexpr auto operator()() const + [[nodiscard]] inline constexpr auto operator()() const -> Type { return Type::Identity(); } - //! @brief Returns `1`, the 1-by-1 identity matrix equivalent. - //! - //! @tparam Type The type template parameter of the value. - //! - //! @return The value `1`. template - [[nodiscard]] inline constexpr auto operator()() const noexcept + [[nodiscard]] inline constexpr auto operator()() const noexcept -> Type { - return Type{ 1 }; + return 1; } }; -//! @todo Improve support and optimize for no input: neither type nor void but -//! an equivalent empty type? Void may be more intuitive, practical for the user -//! although less theoretically correct? -template -using kalman = fcarouge::kalman< - std::conditional_t>, - std::conditional_t>, - std::conditional_t< - Input == 0, void, - std::conditional_t>>, - transpose, symmetrize, divide, identity_matrix, UpdateTypes, - PredictionTypes>; - } // namespace fcarouge::eigen::internal #endif // FCAROUGE_INTERNAL_EIGEN_HPP diff --git a/include/fcarouge/internal/format.hpp b/include/fcarouge/internal/format.hpp index acfad1c0b..2e4492d4f 100644 --- a/include/fcarouge/internal/format.hpp +++ b/include/fcarouge/internal/format.hpp @@ -43,17 +43,19 @@ For more information, please refer to */ namespace fcarouge { -template +template class kalman; } // namespace fcarouge -template +template struct std::formatter< - fcarouge::kalman, Char> { //! @todo Support parsing arguments. @@ -64,7 +66,7 @@ struct std::formatter< // @todo How to support different nested types? template - auto format(const fcarouge::kalman &filter, std::basic_format_context &format_context) diff --git a/include/fcarouge/internal/kalman.hpp b/include/fcarouge/internal/kalman.hpp index 953dac76b..777defb6d 100644 --- a/include/fcarouge/internal/kalman.hpp +++ b/include/fcarouge/internal/kalman.hpp @@ -39,6 +39,8 @@ For more information, please refer to */ #ifndef FCAROUGE_INTERNAL_KALMAN_HPP #define FCAROUGE_INTERNAL_KALMAN_HPP +#include +#include #include #include @@ -48,7 +50,7 @@ namespace fcarouge::internal template struct pack { }; -using empty_pack_t = pack<>; +using empty_pack = pack<>; template struct repack { using type = Type; @@ -61,34 +63,41 @@ struct repack> { template using repack_t = typename repack::type; -template +template struct kalman { //! @todo Support some more specializations, all, or disable others? }; -template -struct kalman +struct kalman, pack> { struct empty { }; - using state = State; - using output = Output; + template + using matrix = std::decay_t>; + template + //! @todo Should we remove the dependency on `std::array`? It can be done with + //! C-style `Type[Size]` array but may be not recommended. + using array = std::conditional_t< + Size == 1, Type, + std::decay_t>, + Type>>>; + using value_type = Type; + using state = array; + using output = array; using input = empty; - using estimate_uncertainty = - std::decay_t>; - using process_uncertainty = - std::decay_t>; - using output_uncertainty = - std::decay_t>; - using state_transition = - std::decay_t>; - using output_model = - std::decay_t>; + using estimate_uncertainty = matrix; + using process_uncertainty = matrix; + using output_uncertainty = matrix; + using state_transition = matrix; + using output_model = matrix; using input_control = empty; - using gain = std::decay_t>; + using gain = matrix; using innovation = output; using innovation_uncertainty = output_uncertainty; using observation_state_function = @@ -106,23 +115,24 @@ struct kalman; //! @todo Is there a simpler way to initialize to the zero matrix? - state x{ 0 * Identity().template operator()() }; + state x{ value_type{ 0 } * Identity().template operator()() }; estimate_uncertainty p{ Identity().template operator()() }; process_uncertainty q{ - 0 * Identity().template operator()() + value_type{ 0 } * Identity().template operator()() }; - output_uncertainty r{ 0 * + output_uncertainty r{ value_type{ 0 } * Identity().template operator()() }; output_model h{ Identity().template operator()() }; state_transition f{ Identity().template operator()() }; gain k{ Identity().template operator()() }; - innovation y{ 0 * Identity().template operator()() }; + innovation y{ value_type{ 0 } * + Identity().template operator()() }; innovation_uncertainty s{ Identity().template operator()() }; - output z{ 0 * Identity().template operator()() }; + output z{ value_type{ 0 } * Identity().template operator()() }; //! @todo Should we pass through the reference to the state x or have the user //! access it through k.x() when needed? Where does the practical/performance @@ -218,27 +228,31 @@ struct kalman -struct kalman, pack> { - using state = State; - using output = Output; - using input = Input; - using estimate_uncertainty = - std::decay_t>; - using process_uncertainty = - std::decay_t>; - using output_uncertainty = - std::decay_t>; - using state_transition = - std::decay_t>; - using output_model = - std::decay_t>; - using input_control = - std::decay_t>; - using gain = std::decay_t>; +template +struct kalman, pack> { + template + using matrix = std::decay_t>; + template + using array = std::conditional_t< + Size == 1, Type, + std::decay_t>, + Type>>>; + using value_type = Type; + using state = array; + using output = array; + using input = array; + using estimate_uncertainty = matrix; + using process_uncertainty = matrix; + using output_uncertainty = matrix; + using state_transition = matrix; + using output_model = matrix; + using input_control = matrix; + using gain = matrix; using innovation = output; using innovation_uncertainty = output_uncertainty; using observation_state_function = @@ -264,18 +278,19 @@ struct kalman() }; - output_uncertainty r{ 0 * + output_uncertainty r{ value_type{ 0 } * Identity().template operator()() }; output_model h{ Identity().template operator()() }; state_transition f{ Identity().template operator()() }; input_control g{ Identity().template operator()() }; gain k{ Identity().template operator()() }; - innovation y{ 0 * Identity().template operator()() }; + innovation y{ value_type{ 0 } * + Identity().template operator()() }; innovation_uncertainty s{ Identity().template operator()() }; - output z{ 0 * Identity().template operator()() }; - input u{ 0 * Identity().template operator()() }; + output z{ value_type{ 0 } * Identity().template operator()() }; + input u{ value_type{ 0 } * Identity().template operator()() }; //! @todo Should we pass through the reference to the state x or have the user //! access it through k.x() when needed? Where does the practical/performance diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index dd87d28b7..cdfa43bd7 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -46,6 +46,7 @@ For more information, please refer to */ #include "internal/kalman.hpp" #include +#include #include #include #include @@ -72,6 +73,12 @@ struct identity_matrix { } }; +//! @brief Convenience tuple-like empty pack type. +using empty_pack = internal::empty_pack; + +//! @brief Convenience tuple-like pack type. +template using pack = internal::pack; + //! @brief Kalman filter. //! //! @details A Bayesian filter that uses multivariate Gaussians. @@ -82,37 +89,50 @@ struct identity_matrix { //! estimates by adding Gaussians. Design the state (X, P), the process (F, Q), //! the measurement (Z, R), the measurement function H, and if the system has //! control inputs (U, B). Designing a filter is as much art as science. +//! Filters with `state x output x input` dimensions as 1x1x1 and 1x1x0 (no +//! input) are supported through the Standard Templated Library (STL). Higher +//! dimension filters require Eigen 3 support. //! -//! @tparam State The type template parameter of the state column vector x. -//! State variables can be observed (measured), or hidden variables (inferred). -//! This is the the mean of the multivariate Gaussian. -//! @tparam Output The type template parameter of the measurement column vector -//! z. -//! @tparam Input The type template parameter of the control u. A `void` input -//! type can be used for systems with no input control to disable all of the -//! input control features, the control transition matrix G support, and the -//! other related computations from the filter. +//! @tparam Type The type template parameter of the filter data value type used +//! for computation. Defaults to `double`. +//! @tparam State The non-type template size of the state column vector x. State +//! variables can be observed (measured), or hidden variables (inferred). This +//! is the the mean of the multivariate Gaussian. Defaults to one `1` state. +//! @tparam Output The non-type template size of the measurement column vector +//! z. Defaults to one `1` output. +//! @tparam Input The non-type template size of the control column vector u. A +//! zero `0` input size value can be used for systems with no input control to +//! disable all of the input control features, the control transition matrix G +//! support, and the other related computations from the filter. Defaults to +//! no `0` input. //! @tparam Transpose The customization point object template parameter of the -//! matrix transpose functor. +//! matrix transpose functor. Defaults to the standard passthrough +//! `std::identity` function object since the transposed value of an arithmetic +//! type is itself. //! @tparam Symmetrize The customization point object template parameter of the -//! matrix symmetrization functor. +//! matrix symmetrization functor. Defaults to the standard passthrough +//! `std::identity` function object since the symmetric value of an arithmetic +//! type is itself. //! @tparam Divide The customization point object template parameter of the -//! matrix division functor. +//! matrix division functor. Default to the standard division +//! `std::divides` function object. //! @tparam Identity The customization point object template parameter of the -//! matrix identity functor. +//! matrix identity functor. Defaults to an `identity_matrix` function object +//! returning the arithmetic `1` value. //! @tparam UpdateTypes The additional update function parameter types passed in //! through a tuple-like parameter type, composing zero or more types. //! Parameters such as delta times, variances, or linearized values. The //! parameters are propagated to the function objects used to compute the state //! observation H and the observation noise R matrices. The parameters are also -//! propagated to the state observation function object h. +//! propagated to the state observation function object h. Defaults to no +//! parameter types, the empty pack. //! @tparam PredictionTypes The additional prediction function parameter types //! passed in through a tuple-like parameter type, composing zero or more types. //! Parameters such as delta times, variances, or linearized values. The //! parameters are propagated to the function objects used to compute the //! process noise Q, the state transition F, and the control transition G //! matrices. The parameters are also propagated to the state transition -//! function object f. +//! function object f. Defaults to no parameter types, the empty pack. //! //! @note This class could be usable in constant expressions if `std::function` //! could too. The polymorphic function wrapper was used in place of function @@ -153,12 +173,13 @@ struct identity_matrix { //! re-initializations but to what default? //! @todo Could the Input be void by default? Or empty? //! @todo Expand std::format support with standard arguments and Eigen3 types. +//! @todo Support complex number filters? template < - typename State = double, typename Output = State, typename Input = void, - typename Transpose = std::identity, typename Symmetrize = std::identity, - typename Divide = std::divides, typename Identity = identity_matrix, - typename UpdateTypes = internal::empty_pack_t, - typename PredictionTypes = internal::empty_pack_t> + typename Type = double, std::size_t State = 1, std::size_t Output = 1, + std::size_t Input = 0, typename Transpose = std::identity, + typename Symmetrize = std::identity, typename Divide = std::divides, + typename Identity = identity_matrix, typename UpdateTypes = empty_pack, + typename PredictionTypes = empty_pack> class kalman { private: @@ -170,8 +191,8 @@ class kalman //! @brief The internal implementation unpacks the parameter packs from //! tuple-like types which allows for multiple parameter pack deductions. using implementation = - internal::kalman, + internal::kalman, internal::repack_t>; //! @} @@ -180,6 +201,9 @@ class kalman //! @name Public Member Types //! @{ + //! @brief Type of the filter scalar data. + using value_type = typename implementation::value_type; + //! @brief Type of the state estimate column vector X. using state = typename implementation::state; @@ -385,7 +409,7 @@ class kalman //! @complexity Constant. [[nodiscard("The returned control column vector U is unexpectedly " "discarded.")]] inline constexpr auto - u() const -> input requires(!std::is_void_v) + u() const -> input requires(Input > 0) { return filter.u; } @@ -870,7 +894,7 @@ class kalman //! @complexity Constant. [[nodiscard("The returned control transition matrix G is unexpectedly " "discarded.")]] inline constexpr auto - g() const -> input_control requires(!std::is_void_v) + g() const -> input_control requires(Input > 0) { return filter.g; } @@ -880,8 +904,7 @@ class kalman //! @param value The copied control transition matrix G. //! //! @complexity Constant. - inline constexpr void - g(const input_control &value) requires(!std::is_void_v) + inline constexpr void g(const input_control &value) requires(Input > 0) { filter.g = value; } @@ -891,8 +914,7 @@ class kalman //! @param value The moved control transition matrix G. //! //! @complexity Constant. - inline constexpr void - g(input_control &&value) requires(!std::is_void_v) + inline constexpr void g(input_control &&value) requires(Input > 0) { filter.g = std::move(value); } @@ -906,10 +928,9 @@ class kalman //! //! @complexity Constant. inline constexpr void g(const auto &value, const auto &...values) requires( - !std::is_void_v && - !std::is_assignable_v< - typename implementation::transition_control_function, - std::decay_t>) + Input > 0 && !std::is_assignable_v< + typename implementation::transition_control_function, + std::decay_t>) { filter.g = std::move(input_control{ value, values... }); } @@ -923,10 +944,9 @@ class kalman //! //! @complexity Constant. inline constexpr void g(auto &&value, auto &&...values) requires( - !std::is_void_v && - !std::is_assignable_v< - typename implementation::transition_control_function, - std::decay_t>) + Input > 0 && !std::is_assignable_v< + typename implementation::transition_control_function, + std::decay_t>) { filter.g = std::move(input_control{ std::forward(value), @@ -943,7 +963,7 @@ class kalman //! //! @complexity Constant. inline constexpr void g(const auto &callable) requires( - !std::is_void_v && + Input > 0 && std::is_assignable_v>) { @@ -960,7 +980,7 @@ class kalman //! //! @complexity Constant. inline constexpr void g(auto &&callable) requires( - !std::is_void_v && + Input > 0 && std::is_assignable_v>) { diff --git a/include/fcarouge/kalman_eigen.hpp b/include/fcarouge/kalman_eigen.hpp index 7c4cd9b7b..925717464 100644 --- a/include/fcarouge/kalman_eigen.hpp +++ b/include/fcarouge/kalman_eigen.hpp @@ -43,7 +43,6 @@ For more information, please refer to */ //! @brief Kalman operation for Eigen 3 types. #include "internal/eigen.hpp" -#include "internal/kalman.hpp" #include @@ -61,35 +60,20 @@ using divide = internal::divide; //! @brief Function object for providing an Eigen identity matrix. using identity_matrix = internal::identity_matrix; +using empty_pack = internal::empty_pack; + //! @brief Eigen-based Kalman filter. //! //! @details Implemented with the Eigen linear algebra library matrices with //! sizes fixed at compile-time. //! -//! @tparam Type The type template parameter of the matrices data. -//! @tparam State The non-type template parameter size of the state vector X. -//! @tparam Output The non-type template parameter size of the measurement -//! vector Z. -//! @tparam Input The non-type template parameter size of the control U. -//! @tparam UpdateTypes The additional update function parameter types passed in -//! through a tuple-like parameter type, composing zero or more types. -//! Parameters such as delta times, variances, or linearized values. The -//! parameters are propagated to the function objects used to compute the state -//! observation H and the observation noise R matrices. The parameters are also -//! propagated to the state observation function object h. -//! @tparam PredictionTypes The additional prediction function parameter types -//! passed in through a tuple-like parameter type, composing zero or more types. -//! Parameters such as delta times, variances, or linearized values. The -//! parameters are propagated to the function objects used to compute the -//! process noise Q, the state transition F, and the control transition G -//! matrices. The parameters are also propagated to the state transition -//! function object f. +//! @see fcarouge::kalman template + std::size_t Input = 0, typename UpdateTypes = empty_pack, + typename PredictionTypes = empty_pack> using kalman = - internal::kalman; + fcarouge::kalman; } // namespace fcarouge::eigen diff --git a/sample/dog_position.cpp b/sample/dog_position.cpp index 4d6d3c253..3cba45090 100644 --- a/sample/dog_position.cpp +++ b/sample/dog_position.cpp @@ -32,7 +32,7 @@ namespace //! //! @example dog_position.cpp [[maybe_unused]] auto dog_position{ [] { - using kalman = fcarouge::kalman; + using kalman = fcarouge::kalman; kalman k; // Initialization diff --git a/support/include/fcarouge/format b/support/include/fcarouge/format index c7b5cac2e..ef2a14a5d 100644 --- a/support/include/fcarouge/format +++ b/support/include/fcarouge/format @@ -54,11 +54,14 @@ For more information, please refer to */ #include #include +#include + namespace fcarouge { -template +template class kalman; } // namespace fcarouge @@ -102,17 +105,18 @@ template } } // namespace std -template +template struct fmt::formatter< - fcarouge::kalman, + fcarouge::kalman, Char> : public std::formatter< - fcarouge::kalman, + fcarouge::kalman, Char> { }; diff --git a/test/format.cpp b/test/format.cpp index 29cbeda55..825f3419c 100644 --- a/test/format.cpp +++ b/test/format.cpp @@ -51,7 +51,7 @@ namespace //! @test Verifies formatting filters for single-dimension filters with input //! control. [[maybe_unused]] auto format111{ [] { - using kalman = fcarouge::kalman; + using kalman = fcarouge::kalman; kalman k; assert(std::format("{}", k) == diff --git a/test/initialization.cpp b/test/initialization.cpp index 055986cb6..9b3694948 100644 --- a/test/initialization.cpp +++ b/test/initialization.cpp @@ -67,7 +67,7 @@ namespace //! @test Verifies default values are initialized for single-dimension filters //! with input control. [[maybe_unused]] auto defaults111{ [] { - using kalman = fcarouge::kalman; + using kalman = fcarouge::kalman; kalman k; assert(k.f() == 1);