[operation] specialize operations support for 1-by-N filters (#54)

Author: FrancoisCarouge

.github/workflows/verify_code_style_format.yml

@@ -15,6 +15,6 @@ jobs:
     - name: Update
       run: sudo apt update
     - name: Install
-      run: sudo apt install clang-format-12
+      run: sudo apt install clang-format-14
     - name: Format
-      run: find . -iname *.hpp -o -iname *.cpp | xargs clang-format-12 --Werror --dry-run --verbose -style=file
+      run: find . -iname *.hpp -o -iname *.cpp | xargs clang-format-14 --Werror --dry-run --verbose -style=file

include/fcarouge/internal/kalman.hpp

@@ -47,6 +47,27 @@ For more information, please refer to <https://unlicense.org> */
 
 namespace fcarouge::internal
 {
+
+//! @brief Function object for providing an identity matrix.
+//!
+//! @todo Could we remove this for a standard facility? Perhaps a form of
+//! std::integral_constant?
+//!
+//! @note 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 `1`, the 1-by-1 identity matrix equivalent.
+  //!
+  //! @tparam Type The type template parameter of the value.
+  //!
+  //! @return The value `1`.
+  template <typename Type>
+  [[nodiscard]] inline constexpr auto operator()() const noexcept
+  {
+    return Type{ 1 };
+  }
+};
+
 template <typename State, typename Output, typename Input, typename Transpose,
           typename Symmetrize, typename Divide, typename Identity,
           typename UpdateArguments, typename PredictionArguments>

include/fcarouge/internal/kalman_eigen_operator.hpp include/fcarouge/internal/kalman_eigen.hpp

@@ -36,22 +36,30 @@ OTHER DEALINGS IN THE SOFTWARE.
 
 For more information, please refer to <https://unlicense.org> */
 
-#ifndef FCAROUGE_INTERNAL_KALMAN_EIGEN_OPERATOR_HPP
-#define FCAROUGE_INTERNAL_KALMAN_EIGEN_OPERATOR_HPP
+#ifndef FCAROUGE_INTERNAL_KALMAN_EIGEN_HPP
+#define FCAROUGE_INTERNAL_KALMAN_EIGEN_HPP
 
 //! @file
 //! @brief Kalman operation for Eigen 3 types.
 //!
-//! @details Default customization point objects (CPO) for Eigen 3 types.
+//! @details Default customization point objects (CPO).
 
-#include "kalman.hpp"
+#include "fcarouge/kalman.hpp"
 
 #include <Eigen/Eigen>
 
+#include <concepts>
+#include <cstddef>
+#include <functional>
 #include <type_traits>
 
 namespace fcarouge::eigen::internal
 {
+
+//! @brief Arithmetic concept.
+template <typename Type>
+concept arithmetic = std::integral<Type> || std::floating_point<Type>;
+
 //! @brief Function object for performing Eigen matrix transposition.
 //!
 //! @details Implemented with the Eigen linear algebra library matrices with
@@ -78,7 +86,7 @@ struct transpose {
 //! @details Implemented with the Eigen linear algebra library matrices with
 //! sizes fixed at compile-time.
 struct symmetrize {
-  //! @brief Returns the symmetrised `value`.
+  //! @brief Returns the symmetrized `value`.
   //!
   //! @param value Value to compute the symmetry of.
   //!
@@ -101,7 +109,7 @@ struct divide {
   //! @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 quotien matrix. Q: m x o
+  //! @return The quotient matrix. Q: m x o
   //!
   //! @exception May throw implementation-defined exceptions.
   //!
@@ -119,18 +127,77 @@ struct divide {
         .solve(numerator.transpose())
         .transpose();
   }
+
+  //! @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.
+  [[nodiscard]] inline constexpr auto
+  operator()(const auto &numerator, const arithmetic auto &denominator) const ->
+      typename Eigen::Vector<
+          typename std::decay_t<decltype(numerator)>::Scalar,
+          std::decay_t<decltype(numerator)>::RowsAtCompileTime>
+  {
+    return Eigen::Matrix<typename std::decay_t<decltype(numerator)>::Scalar, 1,
+                         1>{ 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.
+  [[nodiscard]] inline constexpr auto
+  operator()(const arithmetic auto &numerator, const auto &denominator) const ->
+      typename Eigen::RowVector<
+          typename std::decay_t<decltype(denominator)>::Scalar,
+          std::decay_t<decltype(denominator)>::RowsAtCompileTime>
+  {
+    return denominator.transpose()
+        .fullPivHouseholderQr()
+        .solve(Eigen::Matrix<typename std::decay_t<decltype(numerator)>::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;
+  }
 };
 
 //! @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 replaced by a variable
-//! template? Proposed in paper P2008R0 entitled "Enabling variable template
-//! template parameters".
-struct identity {
-  //! @brief Returns the identity maxtrix.
+//! @note 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.
   //!
@@ -142,8 +209,28 @@ struct identity {
   {
     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 <arithmetic Type>
+  [[nodiscard]] inline constexpr auto operator()() const noexcept
+  {
+    return Type{ 1 };
+  }
 };
 
+template <typename Type = double, std::size_t State = 1, std::size_t Output = 1,
+          std::size_t Input = 1, typename UpdateArguments = std::tuple<>,
+          typename PredictionArguments = std::tuple<>>
+using kalman = fcarouge::kalman<
+    Type, std::conditional_t<State == 1, Type, Eigen::Vector<Type, State>>,
+    std::conditional_t<Output == 1, Type, Eigen::Vector<Type, Output>>,
+    std::conditional_t<Input == 1, Type, Eigen::Vector<Type, Input>>, transpose,
+    symmetrize, divide, identity_matrix, UpdateArguments, PredictionArguments>;
+
 } // namespace fcarouge::eigen::internal
 
-#endif // FCAROUGE_INTERNAL_KALMAN_EIGEN_OPERATOR_HPP
+#endif // FCAROUGE_INTERNAL_KALMAN_EIGEN_HPP

include/fcarouge/internal/kalman_operator.hpp

@@ -43,7 +43,6 @@ For more information, please refer to <https://unlicense.org> */
 //! @brief The main Kalman filter class.
 
 #include "internal/kalman.hpp"
-#include "internal/kalman_operator.hpp"
 
 #include <concepts>
 #include <functional>
@@ -53,23 +52,26 @@ For more information, please refer to <https://unlicense.org> */
 
 namespace fcarouge
 {
+//! @brief Function object for providing an identity matrix.
+using identity_matrix = internal::identity_matrix;
+
 //! @brief Kalman filter.
 //!
 //! @details 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.
+//! 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.
 //!
 //! @tparam Type The type template parameter of the value type of the filter.
-//! @tparam State The type template parameter of the state vector x. State
+//! @tparam State The type template parameter of the state 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 vector z.
-//! @tparam Input The type template parameter of the control u.
+//! @tparam Output The type template parameter of the measurement vector Z.
+//! @tparam Input The type template parameter of the control U.
 //! @tparam Transpose The customization point object template parameter of the
 //! matrix transpose functor.
 //! @tparam Symmetrize The customization point object template parameter of the
@@ -78,11 +80,18 @@ namespace fcarouge
 //! matrix division functor.
 //! @tparam Identity The customization point object template parameter of the
 //! matrix identity functor.
+//! @tparam UpdateArguments The variadic type template parameter for additional
+//! update function parameters. 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 PredictionArguments The variadic type template parameter for
-//! additional prediction function parameters. Time, or a delta thereof, is
-//! often a prediction parameter. 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.
+//! additional prediction function parameters. 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.
 //!
 //! @note This class could be usable in constant expressions if `std::function`
 //! could too. The polymorphic function wrapper was used in place of function
@@ -118,11 +127,12 @@ namespace fcarouge
 //! characteristics?
 //! @todo Consider additional constructors?
 //! @todo Consider additional characteristics method overloads?
+//! @todo Could we do away with std::tuple, replaced by a template template?
 template <
     typename Type = double, typename State = Type, typename Output = State,
     typename Input = State, typename Transpose = std::identity,
     typename Symmetrize = std::identity, typename Divide = std::divides<void>,
-    typename Identity = internal::identity,
+    typename Identity = identity_matrix,
     typename UpdateArguments = std::tuple<>,
     typename PredictionArguments = std::tuple<>>
 class kalman