[filter] declarative paradigm

FrancoisCarouge · Nov 19, 2023 · 0ff3159 · 0ff3159
1 parent 033285c
commit 0ff3159
Show file tree

Hide file tree

Showing 7 changed files with 611 additions and 0 deletions.
diff --git a/include/fcarouge/internal/kalman.hpp b/include/fcarouge/internal/kalman.hpp
@@ -51,6 +51,8 @@ struct kalman final {
   //! @todo Add a pretty compilation error?
 };
 
+// Add many safer specialization...
+
 template <typename State, typename Output, typename... UpdateTypes,
           typename... PredictionTypes>
 struct kalman<State, Output, void, pack<UpdateTypes...>,

diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp
@@ -243,6 +243,13 @@ class kalman final {
   //! @complexity Constant.
   inline constexpr kalman() = default;
 
+  // wip
+  inline constexpr kalman([[maybe_unused]] const auto &...wip_sink) {
+    // implement based on type of arguments against CTAD for type safety (least
+    // worst but poor user xp)? strong type (probably not because coupling)?
+    // tags (probably not because instrusive)?
+  }
+
   //! @brief Copy constructs a filter.
   //!
   //! @details Constructs the filter with the copy of the contents of the
@@ -628,6 +635,53 @@ class kalman final {
   template <std::size_t Position> inline constexpr auto update() const;
   //! @}
 };
+
+//! @name Deduction Guides
+//! @{
+
+// Reference:
+// template <typename State = double, typename Output = double,
+//           typename Input = void, typename UpdateTypes = empty_pack,
+//           typename PredictionTypes = empty_pack>
+// class kalman final {
+
+// Needed?
+// kalman() -> kalman<double, double, void, empty_pack, empty_pack>;
+
+// If ordering and inputs get confusing, we can use tagged types, ifconstexpr.
+
+// assert p is x by x ?
+
+template <typename State, typename EstimateUncertainty,
+          arithmetic OutputUncertainty>
+kalman(const State &x, const EstimateUncertainty &p, const OutputUncertainty &r)
+    -> kalman<State, OutputUncertainty, void, empty_pack, empty_pack>;
+
+template <typename State, typename EstimateUncertainty,
+          arithmetic ProcessUncertainty, arithmetic OutputUncertainty>
+kalman(const State &x, const EstimateUncertainty &p,
+       const ProcessUncertainty &q, const OutputUncertainty &r)
+    -> kalman<State, OutputUncertainty, void, empty_pack, empty_pack>;
+
+template <typename State, typename EstimateUncertainty,
+          arithmetic ProcessUncertainty, arithmetic OutputUncertainty,
+          typename Input>
+kalman(const State &x, const EstimateUncertainty &p,
+       const ProcessUncertainty &q, const OutputUncertainty &r, const Input &u)
+    -> kalman<State, OutputUncertainty, Input, empty_pack, empty_pack>;
+
+template <typename State, typename EstimateUncertainty,
+          typename ProcessUncertainty, typename OutputUncertainty,
+          typename Transition, typename Observation, typename OutputModel,
+          typename UpdateTypes, typename PredictionTypes>
+kalman(const State &x, const EstimateUncertainty &p,
+       const ProcessUncertainty &q, const OutputUncertainty &r,
+       const Transition &transition, const Observation &observation,
+       const OutputModel &h, const UpdateTypes &update_types,
+       const PredictionTypes &prediction_types)
+    -> kalman<State, OutputUncertainty, void, UpdateTypes, PredictionTypes>;
+
+//! @}
 } // namespace fcarouge
 
 #include "internal/kalman.tpp"

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
@@ -40,6 +40,9 @@ foreach(
   TEST
   "kalman_constructor_default_float_1x1x1.cpp"
   "kalman_constructor_default.cpp"
+  "kalman_declarative_1x1x0_building_height.cpp"
+  "kalman_declarative_1x1x0_liquid_temperature.cpp"
+  "kalman_declarative_1x1x1_dog_position.cpp"
   "kalman_f.cpp"
   "kalman_format_arguments.cpp"
   "kalman_format_float_1x1x1.cpp"
@@ -111,6 +114,7 @@ foreach(BACKEND IN ITEMS "eigen")
     "kalman_constructor_default_5x4x1.cpp"
     "kalman_constructor_default_5x4x3.cpp"
     "kalman_constructor_move_5x4x3.cpp"
+    "kalman_declarative_4x1x0_soaring.cpp"
     "kalman_f_5x4x3.cpp"
     "kalman_h_5x4x3.cpp")
     get_filename_component(NAME ${TEST} NAME_WE)

diff --git a/test/kalman_declarative_1x1x0_building_height.cpp b/test/kalman_declarative_1x1x0_building_height.cpp
@@ -0,0 +1,73 @@
+/*  __          _      __  __          _   _
+| |/ /    /\   | |    |  \/  |   /\   | \ | |
+| ' /    /  \  | |    | \  / |  /  \  |  \| |
+|  <    / /\ \ | |    | |\/| | / /\ \ | . ` |
+| . \  / ____ \| |____| |  | |/ ____ \| |\  |
+|_|\_\/_/    \_\______|_|  |_/_/    \_\_| \_|
+
+Kalman Filter
+Version 0.3.0
+https://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.hpp"
+
+#include <cassert>
+#include <cmath>
+
+namespace fcarouge::test {
+namespace {
+//! @test Verify a 1x1x0 filter created with declarative syntax.
+[[maybe_unused]] auto test{[] {
+  using state = double;
+  using estimate_uncertainty = double;
+  using output_uncertainty = double;
+
+  auto filter{
+      kalman{state{60.}, estimate_uncertainty{225.}, output_uncertainty{25.}}};
+
+  filter.update(48.54);
+  filter.update(47.11);
+  filter.update(55.01);
+  filter.update(55.15);
+  filter.update(49.89);
+  filter.update(40.85);
+  filter.update(46.72);
+  filter.update(50.05);
+  filter.update(51.27);
+  filter.update(49.95);
+
+  assert(std::abs(1 - filter.x() / 49.57) < 0.001 &&
+         "After 10 measurement and update iterations, the building estimated "
+         "height is: 49.57m.");
+
+  return 0;
+}()};
+} // namespace
+} // namespace fcarouge::test
diff --git a/test/kalman_declarative_1x1x0_liquid_temperature.cpp b/test/kalman_declarative_1x1x0_liquid_temperature.cpp
@@ -0,0 +1,87 @@
+/*  __          _      __  __          _   _
+| |/ /    /\   | |    |  \/  |   /\   | \ | |
+| ' /    /  \  | |    | \  / |  /  \  |  \| |
+|  <    / /\ \ | |    | |\/| | / /\ \ | . ` |
+| . \  / ____ \| |____| |  | |/ ____ \| |\  |
+|_|\_\/_/    \_\______|_|  |_/_/    \_\_| \_|
+
+Kalman Filter
+Version 0.3.0
+https://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.hpp"
+
+#include <cassert>
+#include <cmath>
+
+namespace fcarouge::test {
+namespace {
+//! @test Verify a 1x1x0 filter created with declarative syntax with process
+//! uncertainty.
+[[maybe_unused]] auto test{[] {
+  using state = double;
+  using estimate_uncertainty = double;
+  using process_uncertainty = double;
+  using output_uncertainty = double;
+
+  auto filter{kalman{state{10.}, estimate_uncertainty{100 * 100.},
+                     process_uncertainty{0.0001},
+                     output_uncertainty{0.1 * 0.1}}};
+
+  const auto step{[&filter](double temperature) {
+    filter.predict();
+    filter.update(temperature);
+  }};
+
+  step(49.95);
+  step(49.967);
+  step(50.1);
+  step(50.106);
+  step(49.992);
+  step(49.819);
+  step(49.933);
+  step(50.007);
+  step(50.023);
+  step(49.99);
+
+  assert(std::abs(1 - filter.p() / 0.0013) < 0.05 &&
+         "The estimate uncertainty expected at 5% accuracy."
+         "The estimate uncertainty is 0.0013, i.e. the estimate error standard "
+         "deviation is: 0.036°C.");
+  assert(std::abs(1 - filter.x() / 49.988) < 0.001 &&
+         "The state estimates expected at 0.1% accuracy."
+         "The filter estimates the liquid temperature at 49.988°C.");
+  assert(std::abs(1 - filter.k() / 0.1265) < 0.001 &&
+         "The gain expected at 0.1% accuracy.");
+
+  return 0;
+}()};
+} // namespace
+} // namespace fcarouge::test
diff --git a/test/kalman_declarative_1x1x1_dog_position.cpp b/test/kalman_declarative_1x1x1_dog_position.cpp
@@ -0,0 +1,88 @@
+/*  __          _      __  __          _   _
+| |/ /    /\   | |    |  \/  |   /\   | \ | |
+| ' /    /  \  | |    | \  / |  /  \  |  \| |
+|  <    / /\ \ | |    | |\/| | / /\ \ | . ` |
+| . \  / ____ \| |____| |  | |/ ____ \| |\  |
+|_|\_\/_/    \_\______|_|  |_/_/    \_\_| \_|
+
+Kalman Filter
+Version 0.3.0
+https://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.hpp"
+
+#include <cassert>
+#include <cmath>
+
+namespace fcarouge::test {
+namespace {
+//! @test Verify a 1x1x1 filter created with declarative syntax.
+[[maybe_unused]] auto test{[] {
+  using state = double;
+  using estimate_uncertainty = double;
+  using process_uncertainty = double;
+  using output_uncertainty = double;
+  using input = double;
+
+  auto filter{kalman{state{1.}, estimate_uncertainty{20 * 20.},
+                     process_uncertainty{2.}, output_uncertainty{1.}, input{}}};
+
+  filter.predict(1.);
+  filter.update(1.354);
+  filter.predict(1.);
+  filter.update(1.882);
+  filter.predict(1.);
+  filter.update(4.341);
+  filter.predict(1.);
+  filter.update(7.156);
+  filter.predict(1.);
+  filter.update(6.939);
+  filter.predict(1.);
+  filter.update(6.844);
+  filter.predict(1.);
+  filter.update(9.847);
+  filter.predict(1.);
+  filter.update(12.553);
+  filter.predict(1.);
+  filter.update(16.273);
+  filter.predict(1.);
+  filter.update(14.8);
+
+  assert(
+      std::abs(1 - filter.x() / 15.053) < 0.001 &&
+      "The state estimates expected at 0.1% accuracy."
+      "Here we can see that the variance converges to 2.1623 in 9 steps. This "
+      "means that we have become very confident in our position estimate. It "
+      "is equal to meters. Contrast this to the sensor's meters.");
+
+  return 0;
+}()};
+} // namespace
+} // namespace fcarouge::test