[tests] h and f management overloads (#57)

include/fcarouge/internal/kalman.hpp

@@ -301,7 +301,7 @@ struct kalman<State, Output, Input, Transpose, Symmetrize, Divide, Identity,
     const auto i{ identity.template operator()<estimate_uncertainty>() };
 
     z = output{ output_z... };
-    h = observation_state_h(x, arguments...);
+    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);
@@ -316,9 +316,9 @@ struct kalman<State, Output, Input, Transpose, Symmetrize, Divide, Identity,
                                 const auto &...input_u)
   {
     u = input{ input_u... };
-    f = transition_state_f(x, arguments..., u);
-    q = noise_process_q(x, arguments...);
-    g = transition_control_g(arguments...);
+    f = transition_state_f(x, arguments..., u); // x, u, args?
+    q = noise_process_q(x, arguments...); // x, u, args?
+    g = transition_control_g(arguments...); // extend?
     x = f * x + g * u;
     p = symmetrize(estimate_uncertainty{ f * p * transpose(f) + q });
   }

include/fcarouge/kalman.hpp

@@ -120,14 +120,19 @@ using identity_matrix = internal::identity_matrix;
 //! @todo Support mux pipes https://github.com/joboccara/pipes operator filter?
 //! @todo Reproduce Ardupilot's inertial navigation EKF and comparison
 //! benchmarks in SITL (software in the loop simulation).
-//! @todo Can we provide the operator[] for the vector characteristics
+//! @todo Should we provide the operator[] for the vector characteristics
 //! regardless of implementation? And for the matrix ones too? It could simplify
 //! client code.
+//! @todo Should we provide the operator[] for state directly on the filter? Is
+//! the state X always what the user would want?
 //! @todo Consider if a fluent interface would be preferable for
 //! characteristics?
 //! @todo Consider additional constructors?
 //! @todo Consider additional characteristics method overloads?
 //! @todo Could we do away with std::tuple, replaced by a template template?
+//! @todo A clear or reset member equivalent may be useful for real-time
+//! re-initializations but to what default?
+//! @todo Could the Input be void by default? Or empty?
 template <
     typename Type = double, typename State = Type, typename Output = State,
     typename Input = State, typename Transpose = std::identity,
@@ -876,6 +881,7 @@ class kalman<Type, State, Output, Input, Transpose, Symmetrize, Divide,
   //! @todo Consider if returning the state vector X would be preferable? Or
   //! fluent interface? Would be compatible with an ES-EKF implementation?
   //! @todo Understand why the implementation cannot be moved out of the class.
+  //! @todo What should be the order of the parameters? Update first?
   inline constexpr void
   operator()(const PredictionArguments &...prediction_arguments,
              const UpdateArguments &...update_arguments, const auto &...input_u,

test/f.cpp

@@ -0,0 +1,185 @@
+/*_  __          _      __  __          _   _
+ | |/ /    /\   | |    |  \/  |   /\   | \ | |
+ | ' /    /  \  | |    | \  / |  /  \  |  \| |
+ |  <    / /\ \ | |    | |\/| | / /\ \ | . ` |
+ | . \  / ____ \| |____| |  | |/ ____ \| |\  |
+ |_|\_\/_/    \_\______|_|  |_/_/    \_\_| \_|
+
+Kalman Filter for C++
+Version 0.1.0
+fttps://github.com/FrancoisCarouge/Kalman
+
+SPDX-License-Identifier: Unlicense
+
+This is free and unencumbered software released into the public domain.
+
+Anyone is free to copy, modify, publish, use, compile, sell, or
+distribute this software, either in source code form or as a compiled
+binary, for any purpose, commercial or non-commercial, and by any
+means.
+
+In jurisdictions that recognize copyright laws, the author or authors
+of this software dedicate any and all copyright interest in the
+software to the public domain. We make this dedication for the benefit
+of the public at large and to the detriment of our heirs and
+successors. We intend this dedication to be an overt act of
+relinquishment in perpetuity of all present and future rights to this
+software under copyright law.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+OTHER DEALINGS IN THE SOFTWARE.
+
+For more information, please refer to <https://unlicense.org> */
+
+#include "fcarouge/kalman_eigen.hpp"
+
+#include <cassert>
+
+namespace fcarouge::test
+{
+namespace
+{
+//! @test Verify the state transition matrix F management overloads for
+//! the default filter type.
+[[maybe_unused]] auto f111{ [] {
+  fcarouge::kalman k;
+  using kalman = decltype(k);
+
+  assert(k.f() == 1);
+
+  {
+    const auto f{ 2. };
+    k.f(f);
+    assert(k.f() == 2);
+  }
+
+  {
+    const auto f{ 3. };
+    k.f(std::move(f));
+    assert(k.f() == 3);
+  }
+
+  {
+    const auto f{ 4. };
+    k.f(f);
+    assert(k.f() == 4);
+  }
+
+  {
+    const auto f{ 5. };
+    k.f(std::move(f));
+    assert(k.f() == 5);
+  }
+
+  {
+    const auto f{ [](const kalman::state &x,
+                     const kalman::input &u) -> kalman::state_transition {
+      static_cast<void>(x);
+      static_cast<void>(u);
+      return 6.;
+    } };
+    k.f(f);
+    assert(k.f() == 5);
+    k.predict(0.);
+    assert(k.f() == 6);
+  }
+
+  {
+    const auto f{ [](const kalman::state &x,
+                     const kalman::input &u) -> 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.);
+    assert(k.f() == 7);
+  }
+
+  return 0;
+}() };
+
+//! @test Verify the state transition matrix F management overloads for
+//! the Eigen filter type.
+[[maybe_unused]] auto f543{ [] {
+  using kalman =
+      eigen::kalman<double, 5, 4, 3, std::tuple<double, float, int, char>,
+                    std::tuple<char, int, float, double>>;
+
+  kalman k;
+  const auto i5x5{ Eigen::Matrix<double, 5, 5>::Identity() };
+  const auto z5x5{ Eigen::Matrix<double, 5, 5>::Zero() };
+
+  assert(k.f() == i5x5);
+
+  {
+    const auto f{ i5x5 };
+    k.f(f);
+    assert(k.f() == i5x5);
+  }
+
+  {
+    const auto f{ z5x5 };
+    k.f(std::move(f));
+    assert(k.f() == z5x5);
+  }
+
+  {
+    const auto f{ i5x5 };
+    k.f(f);
+    assert(k.f() == i5x5);
+  }
+
+  {
+    const auto f{ z5x5 };
+    k.f(std::move(f));
+    assert(k.f() == z5x5);
+  }
+
+  {
+    const auto f{ [](const kalman::state &x, const char &c, const int &i,
+                     const float &f, const double &d,
+                     const kalman::input &u) -> kalman::state_transition {
+      static_cast<void>(x);
+      static_cast<void>(d);
+      static_cast<void>(f);
+      static_cast<void>(i);
+      static_cast<void>(c);
+      static_cast<void>(u);
+      return Eigen::Matrix<double, 5, 5>::Identity();
+    } };
+    k.f(f);
+    assert(k.f() == z5x5);
+    k.predict(0, 0, 0.f, 0.);
+    assert(k.f() == i5x5);
+  }
+
+  {
+    const auto f{ [](const kalman::state &x, const char &c, const int &i,
+                     const float &f, const double &d,
+                     const kalman::input &u) -> kalman::state_transition {
+      static_cast<void>(x);
+      static_cast<void>(d);
+      static_cast<void>(f);
+      static_cast<void>(i);
+      static_cast<void>(c);
+      static_cast<void>(u);
+      return Eigen::Matrix<double, 5, 5>::Zero();
+    } };
+    k.f(std::move(f));
+    assert(k.f() == i5x5);
+    k.predict(0, 0, 0.f, 0.);
+    assert(k.f() == z5x5);
+  }
+
+  return 0;
+}() };
+
+} // namespace
+} // namespace fcarouge::test