[filter] make the innovation explicit (#29)

include/fcarouge/internal/kalman.hpp

@@ -189,7 +189,6 @@ struct kalman {
   inline constexpr void predict(const PredictionArguments &...arguments,
                                 const auto &...input_u)
   {
-    // use member variables
     const auto ff{ predict_state };
     f = transition_state_f(arguments...);
     q = noise_process_q(arguments...);
@@ -198,6 +197,14 @@ struct kalman {
     internal::predict<Transpose, Symmetrize>(x, p, ff, f, q, g, u);
   }
 
+  inline constexpr void predict(const PredictionArguments &...arguments)
+  {
+    const auto ff{ predict_state };
+    f = transition_state_f(arguments...);
+    q = noise_process_q(arguments...);
+    internal::predict<Transpose, Symmetrize>(x, p, ff, f, q);
+  }
+
   //! @}
 };
 

include/fcarouge/internal/kalman_equation.hpp

@@ -55,6 +55,14 @@ extrapolate_state(const auto &x, const auto &ff, const auto &f, const auto &g,
   return state{ ff(x, f) + g * u };
 }
 
+[[nodiscard]] inline constexpr auto
+extrapolate_state(const auto &x, const auto &ff, const auto &f)
+{
+  using state = std::remove_reference_t<std::remove_cv_t<decltype(x)>>;
+
+  return state{ ff(x, f) };
+}
+
 template <template <typename> class Transpose>
 [[nodiscard]] inline constexpr auto
 extrapolate_covariance(const auto &p, const auto &f, const auto &q)
@@ -63,6 +71,7 @@ extrapolate_covariance(const auto &p, const auto &f, const auto &q)
       std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
   using state_transition =
       std::remove_reference_t<std::remove_cv_t<decltype(f)>>;
+
   Transpose<state_transition> transpose;
 
   return estimate_uncertainty{ f * p * transpose(f) + q };
@@ -71,33 +80,39 @@ extrapolate_covariance(const auto &p, const auto &f, const auto &q)
 template <template <typename> typename Transpose,
           template <typename> typename Symmetrize>
 inline constexpr void predict(auto &x, auto &p, const auto &ff, const auto &f,
-                              const auto &q, const auto &g, const auto &u)
+                              const auto &q)
 {
-  x = extrapolate_state(x, ff, f, g, u);
+  x = extrapolate_state(x, ff, f);
 
   using estimate_uncertainty =
       std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
+
   Symmetrize<estimate_uncertainty> symmetrize;
+
   p = symmetrize(extrapolate_covariance<Transpose>(p, f, q));
 }
 
-[[nodiscard]] inline constexpr auto update_state(const auto &x, const auto &k,
-                                                 const auto &z, const auto &h)
+template <template <typename> typename Transpose,
+          template <typename> typename Symmetrize>
+inline constexpr void predict(auto &x, auto &p, const auto &ff, const auto &f,
+                              const auto &q, const auto &g, const auto &u)
 {
-  using state = std::remove_reference_t<std::remove_cv_t<decltype(x)>>;
+  x = extrapolate_state(x, ff, f, g, u);
 
-  return state{ x + k * (z - h * x) };
+  using estimate_uncertainty =
+      std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
+
+  Symmetrize<estimate_uncertainty> symmetrize;
+
+  p = symmetrize(extrapolate_covariance<Transpose>(p, f, q));
 }
 
-//! @todo Do we want to allow the client to view the residual y?
-template <typename State>
-[[nodiscard]] inline constexpr auto
-update_state(const State &x, const auto &k, const auto &z,
-             std::remove_reference_t<std::remove_cv_t<State>> (*h)(State))
+[[nodiscard]] inline constexpr auto update_state(const auto &x, const auto &k,
+                                                 const auto &y)
 {
-  using state = State;
+  using state = std::remove_reference_t<std::remove_cv_t<decltype(x)>>;
 
-  return state{ x + k * (z - h(x)) };
+  return state{ x + k * y };
 }
 
 template <template <typename> typename Transpose,
@@ -108,6 +123,7 @@ update_covariance(const auto &p, const auto &k, const auto &h, const auto &r)
   using estimate_uncertainty =
       std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
   using gain = std::remove_reference_t<std::remove_cv_t<decltype(k)>>;
+
   Transpose<estimate_uncertainty> transpose_p;
   Transpose<gain> transpose_k;
   Identity<estimate_uncertainty> i;
@@ -125,13 +141,27 @@ template <template <typename> typename Transpose,
   using output_uncertainty =
       std::remove_reference_t<std::remove_cv_t<decltype(r)>>;
   using gain = std::invoke_result_t<Transpose<observation>, observation>;
+
   Transpose<observation> transpose_h;
   Divide<gain, output_uncertainty> divide;
 
-  return gain{ divide(p * transpose_h(h), h * p * transpose_h(h) + r) };
+  using innovation_uncertainty =
+      std::remove_reference_t<std::remove_cv_t<decltype(r)>>;
+
+  const innovation_uncertainty s{ h * p * transpose_h(h) + r };
+
+  return gain{ divide(p * transpose_h(h), s) };
 }
 
-//! @todo Do we want to allow the client to view K?
+[[nodiscard]] inline constexpr auto innovate(const auto &x, const auto &z,
+                                             const auto &h)
+{
+  using innovation = std::remove_reference_t<std::remove_cv_t<decltype(z)>>;
+
+  return innovation{ z - h * x };
+}
+
+//! @todo Do we want to allow the client to view the gain k? And the residual y?
 template <template <typename> typename Transpose,
           template <typename> typename Symmetrize,
           template <typename, typename> typename Divide,
@@ -141,11 +171,15 @@ inline constexpr void update(auto &x, auto &p, const auto &h, const auto &r,
 {
   const auto k{ weight_gain<Transpose, Divide>(p, h, r) };
 
-  x = update_state(x, k, z, h);
+  const auto y{ innovate(x, z, h) };
+
+  x = update_state(x, k, y);
 
   using estimate_uncertainty =
       std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
+
   Symmetrize<estimate_uncertainty> symmetrize;
+
   p = symmetrize(update_covariance<Transpose, Identity>(p, k, h, r));
 }
 

include/fcarouge/kalman.hpp

@@ -45,6 +45,7 @@ For more information, please refer to <https://unlicense.org> */
 #include "internal/kalman.hpp"
 
 #include <concepts>
+#include <functional>
 
 namespace fcarouge
 {
@@ -54,6 +55,7 @@ namespace fcarouge
 //! update estimates by multiplying Gaussians and predict 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 State The type template parameter of the state vector x. State
 //! variables can be observed (measured), or hidden variables (infeered). This
@@ -79,6 +81,11 @@ namespace fcarouge
 //! systems?
 //! @todo Would it be beneficial to support `Type` and `value_type` prior to the
 //! `State` type template parameter?
+//! @todo Would it be beneficial to support initialization list for
+//! characteristis?
+//! @todo Symmetrization support might be superflous. How to confirm it is safe
+//! to remove?
+//! @todo Would we want to support smoothers?
 template <typename State, typename Output = State, typename Input = State,
           template <typename> typename Transpose = internal::transpose,
           template <typename> typename Symmetrize = internal::symmetrize,
@@ -540,6 +547,7 @@ inline constexpr
   return filter.q;
 }
 
+//! @todo Don't we need to reset functions or values when the other is set?
 template <typename State, typename Output, typename Input,
           template <typename> typename Transpose,
           template <typename> typename Symmetrize,
@@ -551,7 +559,6 @@ kalman<State, Output, Input, Transpose, Symmetrize, Divide, Identity,
        PredictionArguments...>::q(const auto &value, const auto &...values)
 {
   filter.q = process_uncertainty{ value, values... };
-  // +reset function
 }
 
 template <typename State, typename Output, typename Input,
@@ -580,7 +587,6 @@ kalman<State, Output, Input, Transpose, Symmetrize, Divide, Identity,
        PredictionArguments...>::r(const auto &value, const auto &...values)
 {
   filter.r = output_uncertainty{ value, values... };
-  // +reset function
 }
 
 template <typename State, typename Output, typename Input,
@@ -609,7 +615,6 @@ kalman<State, Output, Input, Transpose, Symmetrize, Divide, Identity,
        PredictionArguments...>::f(const auto &value, const auto &...values)
 {
   filter.f = state_transition{ value, values... };
-  // +reset function
 }
 
 template <typename State, typename Output, typename Input,
@@ -638,7 +643,6 @@ kalman<State, Output, Input, Transpose, Symmetrize, Divide, Identity,
        PredictionArguments...>::h(const auto &value, const auto &...values)
 {
   filter.h = output_model{ value, values... };
-  // +reset function
 }
 
 template <typename State, typename Output, typename Input,
@@ -667,7 +671,6 @@ kalman<State, Output, Input, Transpose, Symmetrize, Divide, Identity,
        PredictionArguments...>::g(const auto &value, const auto &...values)
 {
   filter.g = input_control{ value, values... };
-  // +reset function
 }
 
 template <typename State, typename Output, typename Input,