From 3f1bcfe300f49eec2b4e4d0127f434c37301bdfd Mon Sep 17 00:00:00 2001
From: Francois Carouge <francois.carouge@gmail.com>
Date: Sat, 23 Jul 2022 17:58:54 -0700
Subject: [PATCH] [filter] support no control

---
 README.md                            |   2 +-
 include/fcarouge/internal/eigen.hpp  |   5 +-
 include/fcarouge/internal/kalman.hpp | 149 +++++++++++++++++++++++++++
 include/fcarouge/kalman.hpp          |  31 ++++--
 include/fcarouge/kalman_eigen.hpp    |   2 +-
 sample/dog_position.cpp              |   1 +
 test/f.cpp                           |  12 +--
 test/initialization.cpp              |  23 ++++-
 8 files changed, 204 insertions(+), 21 deletions(-)

diff --git a/README.md b/README.md
index ef25383f3..38080e89f 100644
--- a/README.md
+++ b/README.md
@@ -200,7 +200,7 @@ class kalman
 | --- | --- |
 | `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. |
 | `Output` | The type template parameter of the measurement vector z. |
-| `Input` | The type template parameter of the control u. |
+| `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. |
diff --git a/include/fcarouge/internal/eigen.hpp b/include/fcarouge/internal/eigen.hpp
index ef92bb975..cd37c4b43 100644
--- a/include/fcarouge/internal/eigen.hpp
+++ b/include/fcarouge/internal/eigen.hpp
@@ -257,8 +257,9 @@ template <typename Type = double, std::size_t State = 1, std::size_t Output = 1,
 using kalman = fcarouge::kalman<
     std::conditional_t<State == 1, Type, Eigen::Vector<Type, State>>,
     std::conditional_t<Output == 1, Type, Eigen::Vector<Type, Output>>,
-    std::conditional_t<Input == 0 || Input == 1, Type,
-                       Eigen::Vector<Type, Input>>,
+    std::conditional_t<
+        Input == 0, void,
+        std::conditional_t<Input == 1, Type, Eigen::Vector<Type, Input>>>,
     transpose, symmetrize, divide, identity_matrix, UpdateTypes,
     PredictionTypes>;
 
diff --git a/include/fcarouge/internal/kalman.hpp b/include/fcarouge/internal/kalman.hpp
index 2eeb1e2de..7b0ee2f31 100644
--- a/include/fcarouge/internal/kalman.hpp
+++ b/include/fcarouge/internal/kalman.hpp
@@ -67,6 +67,155 @@ struct kalman {
   //! @todo Support some more specializations, all, or disable others?
 };
 
+template <typename State, typename Output, typename Transpose,
+          typename Symmetrize, typename Divide, typename Identity,
+          typename... UpdateTypes, typename... PredictionTypes>
+struct kalman<State, Output, void, Transpose, Symmetrize, Divide, Identity,
+              pack<UpdateTypes...>, pack<PredictionTypes...>> {
+  struct empty {
+  };
+  using state = State;
+  using output = Output;
+  using input = empty;
+  using estimate_uncertainty =
+      std::decay_t<std::invoke_result_t<Divide, State, State>>;
+  using process_uncertainty =
+      std::decay_t<std::invoke_result_t<Divide, State, State>>;
+  using output_uncertainty =
+      std::decay_t<std::invoke_result_t<Divide, Output, Output>>;
+  using state_transition =
+      std::decay_t<std::invoke_result_t<Divide, State, State>>;
+  using output_model =
+      std::decay_t<std::invoke_result_t<Divide, Output, State>>;
+  using input_control = empty;
+  using gain = std::decay_t<std::invoke_result_t<Transpose, output_model>>;
+  using innovation = output;
+  using innovation_uncertainty = output_uncertainty;
+  using observation_state_function =
+      std::function<output_model(const state &, const UpdateTypes &...)>;
+  using noise_observation_function = std::function<output_uncertainty(
+      const state &, const output &, const UpdateTypes &...)>;
+  using transition_state_function = std::function<state_transition(
+      const state &, const PredictionTypes &...)>;
+  using noise_process_function = std::function<process_uncertainty(
+      const state &, const PredictionTypes &...)>;
+  using transition_control_function = empty;
+  using transition_function =
+      std::function<state(const state &, const PredictionTypes &...)>;
+  using observation_function =
+      std::function<output(const state &, const UpdateTypes &...arguments)>;
+
+  //! @todo Is there a simpler way to initialize to the zero matrix?
+  state x{ 0 * Identity().template operator()<state>() };
+  estimate_uncertainty p{
+    Identity().template operator()<estimate_uncertainty>()
+  };
+  process_uncertainty q{
+    0 * Identity().template operator()<process_uncertainty>()
+  };
+  output_uncertainty r{ 0 *
+                        Identity().template operator()<output_uncertainty>() };
+  output_model h{ Identity().template operator()<output_model>() };
+  state_transition f{ Identity().template operator()<state_transition>() };
+  gain k{ Identity().template operator()<gain>() };
+  innovation y{ 0 * Identity().template operator()<innovation>() };
+  innovation_uncertainty s{
+    Identity().template operator()<innovation_uncertainty>()
+  };
+  output z{ 0 * Identity().template operator()<output>() };
+
+  //! @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
+  //! tradeoff leans toward? For the general case? For the specialized cases?
+  //! Same question applies to other parameters.
+  //! @todo Pass the arguments by universal reference?
+  observation_state_function observation_state_h{
+    [this](const state &x, const UpdateTypes &...arguments) -> output_model {
+      static_cast<void>(x);
+      (static_cast<void>(arguments), ...);
+      return h;
+    }
+  };
+  noise_observation_function noise_observation_r{
+    [this](const state &x, const output &z,
+           const UpdateTypes &...arguments) -> output_uncertainty {
+      static_cast<void>(x);
+      static_cast<void>(z);
+      (static_cast<void>(arguments), ...);
+      return r;
+    }
+  };
+  transition_state_function transition_state_f{
+    [this](const state &x,
+           const PredictionTypes &...arguments) -> state_transition {
+      static_cast<void>(x);
+      (static_cast<void>(arguments), ...);
+      return f;
+    }
+  };
+  noise_process_function noise_process_q{
+    [this](const state &x,
+           const PredictionTypes &...arguments) -> process_uncertainty {
+      static_cast<void>(x);
+      (static_cast<void>(arguments), ...);
+      return q;
+    }
+  };
+  transition_function transition{
+    [this](const state &x, const PredictionTypes &...arguments) -> state {
+      (static_cast<void>(arguments), ...);
+      return f * x;
+    }
+  };
+  observation_function observation{
+    [this](const state &x, const UpdateTypes &...arguments) -> output {
+      (static_cast<void>(arguments), ...);
+      return h * x;
+    }
+  };
+
+  Transpose transpose;
+  Divide divide;
+  Symmetrize symmetrize;
+  Identity identity;
+
+  //! @todo Do we want to store i - k * h in a temporary result for reuse? Or
+  //! does the compiler/linker do it for us?
+  //! @todo Do we want to support extended custom y = output_difference(z,
+  //! observation(x))?
+  inline constexpr void update(const UpdateTypes &...arguments,
+                               const auto &...output_z)
+  {
+    const auto i{ identity.template operator()<estimate_uncertainty>() };
+
+    z = output{ output_z... };
+    h = observation_state_h(x, arguments...); // x, z, args?
+    r = noise_observation_r(x, z, arguments...);
+    s = h * p * transpose(h) + r;
+    k = divide(p * transpose(h), s);
+    y = z - observation(x, arguments...);
+    x = x + k * y;
+    p = symmetrize(estimate_uncertainty{
+        (i - k * h) * p * transpose(i - k * h) + k * r * transpose(k) });
+  }
+
+  inline constexpr void predict(const PredictionTypes &...arguments)
+  {
+    f = transition_state_f(x, arguments...);
+    q = noise_process_q(x, arguments...);
+    x = transition(x, arguments...);
+    p = symmetrize(estimate_uncertainty{ f * p * transpose(f) + q });
+  }
+
+  inline constexpr void
+  operator()(const PredictionTypes &...prediction_arguments,
+             const UpdateTypes &...update_arguments, const auto &...output_z)
+  {
+    update(update_arguments..., output_z...);
+    predict(prediction_arguments...);
+  }
+};
+
 template <typename State, typename Output, typename Input, typename Transpose,
           typename Symmetrize, typename Divide, typename Identity,
           typename... UpdateTypes, typename... PredictionTypes>
diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp
index 9f8e7ce21..53f614527 100644
--- a/include/fcarouge/kalman.hpp
+++ b/include/fcarouge/kalman.hpp
@@ -83,11 +83,14 @@ struct identity_matrix {
 //! 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 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. 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 Transpose The customization point object template parameter of the
 //! matrix transpose functor.
 //! @tparam Symmetrize The customization point object template parameter of the
@@ -150,7 +153,7 @@ struct identity_matrix {
 //! re-initializations but to what default?
 //! @todo Could the Input be void by default? Or empty?
 template <
-    typename State = double, typename Output = State, typename Input = State,
+    typename State = double, typename Output = State, typename Input = void,
     typename Transpose = std::identity, typename Symmetrize = std::identity,
     typename Divide = std::divides<void>, typename Identity = identity_matrix,
     typename UpdateTypes = internal::empty_pack_t,
@@ -185,6 +188,8 @@ class kalman
   using output = typename implementation::output;
 
   //! @brief Type of the control vector U.
+  //!
+  //! @todo Conditionally remove this member type when no input is present.
   using input = typename implementation::input;
 
   //! @brief Type of the estimated correlated variance matrix P.
@@ -211,6 +216,8 @@ class kalman
   //! @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 = typename implementation::input_control;
 
   //! @brief Type of the gain matrix K.
@@ -370,12 +377,14 @@ class kalman
 
   //! @brief Returns the last control vector U.
   //!
+  //! @details Not present when the filter has no input.
+  //!
   //! @return The last control vector U.
   //!
   //! @complexity Constant.
   [[nodiscard("The returned control vector U is unexpectedly "
               "discarded.")]] inline constexpr auto
-  u() const -> input
+  u() const -> input requires(!std::is_void_v<Input>)
   {
     return filter.u;
   }
@@ -860,7 +869,7 @@ class kalman
   //! @complexity Constant.
   [[nodiscard("The returned control transition matrix G is unexpectedly "
               "discarded.")]] inline constexpr auto
-  g() const -> input_control
+  g() const -> input_control requires(!std::is_void_v<Input>)
   {
     return filter.g;
   }
@@ -870,7 +879,8 @@ class kalman
   //! @param value The copied control transition matrix G.
   //!
   //! @complexity Constant.
-  inline constexpr void g(const input_control &value)
+  inline constexpr void
+  g(const input_control &value) requires(!std::is_void_v<Input>)
   {
     filter.g = value;
   }
@@ -880,7 +890,8 @@ class kalman
   //! @param value The moved control transition matrix G.
   //!
   //! @complexity Constant.
-  inline constexpr void g(input_control &&value)
+  inline constexpr void
+  g(input_control &&value) requires(!std::is_void_v<Input>)
   {
     filter.g = std::move(value);
   }
@@ -894,6 +905,7 @@ class kalman
   //!
   //! @complexity Constant.
   inline constexpr void g(const auto &value, const auto &...values) requires(
+      !std::is_void_v<Input> &&
       !std::is_assignable_v<
           typename implementation::transition_control_function,
           std::decay_t<decltype(value)>>)
@@ -910,6 +922,7 @@ class kalman
   //!
   //! @complexity Constant.
   inline constexpr void g(auto &&value, auto &&...values) requires(
+      !std::is_void_v<Input> &&
       !std::is_assignable_v<
           typename implementation::transition_control_function,
           std::decay_t<decltype(value)>>)
@@ -929,6 +942,7 @@ class kalman
   //!
   //! @complexity Constant.
   inline constexpr void g(const auto &callable) requires(
+      !std::is_void_v<Input> &&
       std::is_assignable_v<typename implementation::transition_control_function,
                            std::decay_t<decltype(callable)>>)
   {
@@ -945,6 +959,7 @@ class kalman
   //!
   //! @complexity Constant.
   inline constexpr void g(auto &&callable) requires(
+      !std::is_void_v<Input> &&
       std::is_assignable_v<typename implementation::transition_control_function,
                            std::decay_t<decltype(callable)>>)
   {
diff --git a/include/fcarouge/kalman_eigen.hpp b/include/fcarouge/kalman_eigen.hpp
index 6592149f5..7c4cd9b7b 100644
--- a/include/fcarouge/kalman_eigen.hpp
+++ b/include/fcarouge/kalman_eigen.hpp
@@ -85,7 +85,7 @@ using identity_matrix = internal::identity_matrix;
 //! matrices. The parameters are also propagated to the state transition
 //! function object f.
 template <typename Type = double, std::size_t State = 1, std::size_t Output = 1,
-          std::size_t Input = 1,
+          std::size_t Input = 0,
           typename UpdateTypes = fcarouge::internal::empty_pack_t,
           typename PredictionTypes = fcarouge::internal::empty_pack_t>
 using kalman =
diff --git a/sample/dog_position.cpp b/sample/dog_position.cpp
index aa00dce41..4d6d3c253 100644
--- a/sample/dog_position.cpp
+++ b/sample/dog_position.cpp
@@ -32,6 +32,7 @@ namespace
 //!
 //! @example dog_position.cpp
 [[maybe_unused]] auto dog_position{ [] {
+  using kalman = fcarouge::kalman<double, double, double>;
   kalman k;
 
   // Initialization
diff --git a/test/f.cpp b/test/f.cpp
index 63de1a263..706c9180e 100644
--- a/test/f.cpp
+++ b/test/f.cpp
@@ -77,28 +77,24 @@ namespace
   }
 
   {
-    const auto f{ [](const kalman::state &x,
-                     const kalman::input &u) -> kalman::state_transition {
+    const auto f{ [](const kalman::state &x) -> kalman::state_transition {
       static_cast<void>(x);
-      static_cast<void>(u);
       return 6.;
     } };
     k.f(f);
     assert(k.f() == 5);
-    k.predict(0.);
+    k.predict();
     assert(k.f() == 6);
   }
 
   {
-    const auto f{ [](const kalman::state &x,
-                     const kalman::input &u) -> kalman::state_transition {
+    const auto f{ [](const kalman::state &x) -> kalman::state_transition {
       static_cast<void>(x);
-      static_cast<void>(u);
       return 7.;
     } };
     k.f(std::move(f));
     assert(k.f() == 6);
-    k.predict(0.);
+    k.predict();
     assert(k.f() == 7);
   }
 
diff --git a/test/initialization.cpp b/test/initialization.cpp
index a4ce7ad1e..055986cb6 100644
--- a/test/initialization.cpp
+++ b/test/initialization.cpp
@@ -45,8 +45,29 @@ namespace fcarouge::test
 {
 namespace
 {
-//! @test Verifies default values are initialized for single-dimension filters.
+//! @test Verifies default values are initialized for single-dimension filters
+//! without input control.
+[[maybe_unused]] auto defaults110{ [] {
+  kalman k;
+
+  assert(k.f() == 1);
+  assert(k.h() == 1);
+  assert(k.k() == 1);
+  assert(k.p() == 1);
+  assert(k.q() == 0 && "No process noise by default.");
+  assert(k.r() == 0 && "No observation noise by default.");
+  assert(k.s() == 1);
+  assert(k.x() == 0 && "Origin state.");
+  assert(k.y() == 0);
+  assert(k.z() == 0);
+
+  return 0;
+}() };
+
+//! @test Verifies default values are initialized for single-dimension filters
+//! with input control.
 [[maybe_unused]] auto defaults111{ [] {
+  using kalman = fcarouge::kalman<double, double, double>;
   kalman k;
 
   assert(k.f() == 1);