From 2b760b5d4675c6eee3c58e9f64e76955ccc79ef1 Mon Sep 17 00:00:00 2001
From: Mateusz Pusz <mateusz.pusz@gmail.com>
Date: Tue, 20 Dec 2022 17:29:31 +0100
Subject: [PATCH] refactor(example): kalman filter 1-5 examples refactored for
 V2

---
 example/kalman_filter/kalman.h                | 49 +++++++++----------
 .../kalman_filter/kalman_filter-example_1.cpp | 22 ++++-----
 .../kalman_filter/kalman_filter-example_2.cpp | 28 ++++++-----
 .../kalman_filter/kalman_filter-example_3.cpp | 28 ++++++-----
 .../kalman_filter/kalman_filter-example_4.cpp | 33 +++++++------
 .../kalman_filter/kalman_filter-example_5.cpp | 23 +++++----
 6 files changed, 97 insertions(+), 86 deletions(-)

diff --git a/example/kalman_filter/kalman.h b/example/kalman_filter/kalman.h
index f878335b..eb2a23ce 100644
--- a/example/kalman_filter/kalman.h
+++ b/example/kalman_filter/kalman.h
@@ -24,8 +24,7 @@
 
 #include <units/bits/fmt_hacks.h>
 #include <units/format.h>
-#include <units/generic/dimensionless.h>
-#include <units/isq/dimensions/time.h>
+#include <units/isq/space_and_time.h>
 #include <units/math.h>
 #include <units/quantity.h>
 #include <units/quantity_point.h>
@@ -37,21 +36,19 @@ template<typename T>
 concept QuantityOrQuantityPoint =
   units::Quantity<T> || units::QuantityPoint<T>;  // TODO Should it also account for `kinds`?
 
-template<typename... Qs>
+template<units::Dimension auto... Ds>
 inline constexpr bool are_time_derivatives = false;
 
-template<typename Q>
-inline constexpr bool are_time_derivatives<Q> = true;
+template<units::Dimension auto D>
+inline constexpr bool are_time_derivatives<D> = true;
 
-template<typename Q1, typename Q2, typename... Qs>
-inline constexpr bool are_time_derivatives<Q1, Q2, Qs...> =
-  units::DimensionOfT<typename decltype(Q1::reference / Q2::reference)::dimension,
-                      units::isq::dim_time> &&  // TODO Think on how to simplify this
-  are_time_derivatives<Q2, Qs...>;
+template<units::Dimension auto D1, units::Dimension auto D2, units::Dimension auto... Ds>
+inline constexpr bool are_time_derivatives<D1, D2, Ds...> =
+  (D1 / D2 == units::isq::dim_time) && are_time_derivatives<D2, Ds...>;
 
 // state
 template<QuantityOrQuantityPoint... QQPs>
-  requires(sizeof...(QQPs) > 0) && (sizeof...(QQPs) <= 3) && are_time_derivatives<QQPs...>
+  requires(sizeof...(QQPs) > 0) && (sizeof...(QQPs) <= 3) && are_time_derivatives<QQPs::dimension...>
 struct state {
   std::tuple<QQPs...> variables_;
   constexpr state(QQPs... qqps) : variables_(std::move(qqps)...) {}
@@ -76,11 +73,10 @@ constexpr const auto& get(const state<Qs...>& s)
 template<QuantityOrQuantityPoint QQP, QuantityOrQuantityPoint... QQPs>
 struct estimation {
 private:
-  using uncertainty_ref = decltype(QQP::reference * QQP::reference);
-  using uncertainty_type =
-    units::quantity<typename uncertainty_ref::dimension, typename uncertainty_ref::unit, typename QQP::rep>;
+  static constexpr auto uncertainty_ref = QQP::reference * QQP::reference;
+  using uncertainty_type = units::quantity<uncertainty_ref, typename QQP::rep>;
 public:
-  kalman::state<QQP, QQPs...> state;  // TODO extend kalman functions to work with this variadic patermater list
+  kalman::state<QQP, QQPs...> state;  // TODO extend kalman functions to work with this variadic parameter list
   uncertainty_type uncertainty;
 };
 
@@ -93,21 +89,23 @@ estimation(state<QQP>, U) -> estimation<QQP>;
 
 // kalman gain
 template<units::Quantity Q>
-constexpr units::dimensionless<units::one> kalman_gain(Q estimate_uncertainty, Q measurement_uncertainty)
+constexpr units::quantity<units::dimensionless[units::one]> kalman_gain(Q estimate_uncertainty,
+                                                                        Q measurement_uncertainty)
 {
   return estimate_uncertainty / (estimate_uncertainty + measurement_uncertainty);
 }
 
 // state update
-template<typename Q, QuantityOrQuantityPoint QM, units::Dimensionless K>
-  requires units::equivalent<typename Q::dimension, typename QM::dimension>
+template<typename Q, QuantityOrQuantityPoint QM, units::quantity_of<units::dimensionless> K>
+  requires(Q::quantity_spec == QM::quantity_spec)
 constexpr state<Q> state_update(const state<Q>& predicted, QM measured, K gain)
 {
   return {get<0>(predicted) + gain * (measured - get<0>(predicted))};
 }
 
-template<typename Q1, typename Q2, QuantityOrQuantityPoint QM, units::Dimensionless K, units::isq::Time T>
-  requires units::equivalent<typename Q1::dimension, typename QM::dimension>
+template<typename Q1, typename Q2, QuantityOrQuantityPoint QM, units::quantity_of<units::dimensionless> K,
+         units::quantity_of<units::isq::time> T>
+  requires(Q1::quantity_spec == QM::quantity_spec)
 constexpr state<Q1, Q2> state_update(const state<Q1, Q2>& predicted, QM measured, std::array<K, 2> gain, T interval)
 {
   const auto q1 = get<0>(predicted) + get<0>(gain) * (measured - get<0>(predicted));
@@ -115,8 +113,9 @@ constexpr state<Q1, Q2> state_update(const state<Q1, Q2>& predicted, QM measured
   return {q1, q2};
 }
 
-template<typename Q1, typename Q2, typename Q3, QuantityOrQuantityPoint QM, units::Dimensionless K, units::isq::Time T>
-  requires units::equivalent<typename Q1::dimension, typename QM::dimension>
+template<typename Q1, typename Q2, typename Q3, QuantityOrQuantityPoint QM, units::quantity_of<units::dimensionless> K,
+         units::quantity_of<units::isq::time> T>
+  requires(Q1::quantity_spec == QM::quantity_spec)
 constexpr state<Q1, Q2, Q3> state_update(const state<Q1, Q2, Q3>& predicted, QM measured, std::array<K, 3> gain,
                                          T interval)
 {
@@ -127,14 +126,14 @@ constexpr state<Q1, Q2, Q3> state_update(const state<Q1, Q2, Q3>& predicted, QM
 }
 
 // covariance update
-template<units::Quantity Q, units::Dimensionless K>
+template<units::Quantity Q, units::quantity_of<units::dimensionless> K>
 constexpr Q covariance_update(Q uncertainty, K gain)
 {
   return (1 - gain) * uncertainty;
 }
 
 // state extrapolation
-template<typename Q1, typename Q2, units::isq::Time T>
+template<typename Q1, typename Q2, units::quantity_of<units::isq::time> T>
 constexpr state<Q1, Q2> state_extrapolation(const state<Q1, Q2>& estimated, T interval)
 {
   const auto q1 = get<0>(estimated) + get<1>(estimated) * interval;
@@ -142,7 +141,7 @@ constexpr state<Q1, Q2> state_extrapolation(const state<Q1, Q2>& estimated, T in
   return {q1, q2};
 }
 
-template<typename Q1, typename Q2, typename Q3, units::isq::Time T>
+template<typename Q1, typename Q2, typename Q3, units::quantity_of<units::isq::time> T>
 constexpr state<Q1, Q2, Q3> state_extrapolation(const state<Q1, Q2, Q3>& estimated, T interval)
 {
   const auto q1 = get<0>(estimated) + get<1>(estimated) * interval + get<2>(estimated) * pow<2>(interval) / 2;
diff --git a/example/kalman_filter/kalman_filter-example_1.cpp b/example/kalman_filter/kalman_filter-example_1.cpp
index efd624ae..47d8f6b7 100644
--- a/example/kalman_filter/kalman_filter-example_1.cpp
+++ b/example/kalman_filter/kalman_filter-example_1.cpp
@@ -22,8 +22,8 @@
 
 #include "kalman.h"
 #include <units/format.h>
-#include <units/generic/dimensionless.h>
-#include <units/isq/si/mass.h>
+#include <units/isq/space_and_time.h>
+#include <units/si/unit_symbols.h>
 #include <array>
 #include <iostream>
 
@@ -38,27 +38,27 @@ void print_header(const kalman::State auto& initial)
                                "Next Estimate");
 }
 
-void print(auto iteration, Dimensionless auto gain, Quantity auto measured, const kalman::State auto& current,
-           const kalman::State auto& next)
+void print(auto iteration, quantity_of<dimensionless> auto gain, Quantity auto measured,
+           const kalman::State auto& current, const kalman::State auto& next)
 {
   std::cout << STD_FMT::format("{:2} | {:9} | {:8} | {:14} | {:14}\n", iteration, gain, measured, current, next);
 }
 
 int main()
 {
-  using namespace units::isq;
-  using namespace units::isq::si::references;
-  using state = kalman::state<si::mass<si::gram>>;
+  using namespace units::si::unit_symbols;
+  using state = kalman::state<quantity<isq::mass[g]>>;
 
-  const state initial = {1 * kg};
-  const std::array measurements = {1030 * g, 989 * g,  1017 * g, 1009 * g, 1013 * g,
-                                   979 * g,  1008 * g, 1042 * g, 1012 * g, 1011 * g};
+  const state initial = {1 * isq::mass[kg]};
+  const std::array measurements = {1030 * isq::mass[g], 989 * isq::mass[g], 1017 * isq::mass[g], 1009 * isq::mass[g],
+                                   1013 * isq::mass[g], 979 * isq::mass[g], 1008 * isq::mass[g], 1042 * isq::mass[g],
+                                   1012 * isq::mass[g], 1011 * isq::mass[g]};
 
   print_header(initial);
   state next = initial;
   for (int index = 1; const auto& m : measurements) {
     const auto& previous = next;
-    const dimensionless<one> gain = 1. / index;
+    const quantity gain = 1. / index;
     const auto current = state_update(previous, m, gain);
     next = current;
     print(index++, gain, m, current, next);
diff --git a/example/kalman_filter/kalman_filter-example_2.cpp b/example/kalman_filter/kalman_filter-example_2.cpp
index 23ea2bbf..b188c250 100644
--- a/example/kalman_filter/kalman_filter-example_2.cpp
+++ b/example/kalman_filter/kalman_filter-example_2.cpp
@@ -22,15 +22,17 @@
 
 #include "kalman.h"
 #include <units/format.h>
-#include <units/generic/dimensionless.h>
-#include <units/isq/si/length.h>
-#include <units/isq/si/speed.h>
-#include <units/isq/si/time.h>
+#include <units/isq/space_and_time.h>
+#include <units/si/unit_symbols.h>
 #include <array>
 #include <iostream>
 
 // Based on: https://www.kalmanfilter.net/alphabeta.html#ex2
 
+template<class T>
+  requires units::is_scalar<T>
+inline constexpr bool units::is_vector<T> = true;
+
 using namespace units;
 
 void print_header(const kalman::State auto& initial)
@@ -46,15 +48,17 @@ void print(auto iteration, Quantity auto measured, const kalman::State auto& cur
 
 int main()
 {
-  using namespace units::isq;
-  using namespace units::isq::si::references;
-  using state = kalman::state<si::length<si::metre>, si::speed<si::metre_per_second>>;
+  using namespace units::si::unit_symbols;
+  using state = kalman::state<quantity<isq::position_vector[m]>, quantity<isq::velocity[m / s]>>;
 
-  const auto interval = 5 * s;
-  const state initial = {30 * km, 40 * (m / s)};
-  const std::array measurements = {30110 * m, 30265 * m, 30740 * m, 30750 * m, 31135 * m,
-                                   31015 * m, 31180 * m, 31610 * m, 31960 * m, 31865 * m};
-  std::array gain = {dimensionless<one>(0.2), dimensionless<one>(0.1)};
+  const auto interval = 5 * isq::period_duration[s];
+  const state initial = {30 * isq::position_vector[km], 40 * isq::velocity[m / s]};
+  const std::array measurements = {30110 * isq::position_vector[m], 30265 * isq::position_vector[m],
+                                   30740 * isq::position_vector[m], 30750 * isq::position_vector[m],
+                                   31135 * isq::position_vector[m], 31015 * isq::position_vector[m],
+                                   31180 * isq::position_vector[m], 31610 * isq::position_vector[m],
+                                   31960 * isq::position_vector[m], 31865 * isq::position_vector[m]};
+  std::array gain = {quantity{0.2}, quantity{0.1}};
 
   print_header(initial);
   state next = state_extrapolation(initial, interval);
diff --git a/example/kalman_filter/kalman_filter-example_3.cpp b/example/kalman_filter/kalman_filter-example_3.cpp
index 267e8a3d..193526f7 100644
--- a/example/kalman_filter/kalman_filter-example_3.cpp
+++ b/example/kalman_filter/kalman_filter-example_3.cpp
@@ -22,15 +22,17 @@
 
 #include "kalman.h"
 #include <units/format.h>
-#include <units/generic/dimensionless.h>
-#include <units/isq/si/length.h>
-#include <units/isq/si/speed.h>
-#include <units/isq/si/time.h>
+#include <units/isq/space_and_time.h>
+#include <units/si/unit_symbols.h>
 #include <array>
 #include <iostream>
 
 // Based on: https://www.kalmanfilter.net/alphabeta.html#ex3
 
+template<class T>
+  requires units::is_scalar<T>
+inline constexpr bool units::is_vector<T> = true;
+
 using namespace units;
 
 void print_header(const kalman::State auto& initial)
@@ -46,15 +48,17 @@ void print(auto iteration, Quantity auto measured, const kalman::State auto& cur
 
 int main()
 {
-  using namespace units::isq;
-  using namespace units::isq::si::references;
-  using state = kalman::state<si::length<si::metre>, si::speed<si::metre_per_second>>;
+  using namespace units::si::unit_symbols;
+  using state = kalman::state<quantity<isq::position_vector[m]>, quantity<isq::velocity[m / s]>>;
 
-  const auto interval = 5 * s;
-  const state initial = {30 * km, 50 * (m / s)};
-  const std::array measurements = {30160 * m, 30365 * m, 30890 * m, 31050 * m, 31785 * m,
-                                   32215 * m, 33130 * m, 34510 * m, 36010 * m, 37265 * m};
-  std::array gain = {dimensionless<one>(0.2), dimensionless<one>(0.1)};
+  const auto interval = 5 * isq::period_duration[s];
+  const state initial = {30 * isq::position_vector[km], 50 * isq::velocity[m / s]};
+  const std::array measurements = {30160 * isq::position_vector[m], 30365 * isq::position_vector[m],
+                                   30890 * isq::position_vector[m], 31050 * isq::position_vector[m],
+                                   31785 * isq::position_vector[m], 32215 * isq::position_vector[m],
+                                   33130 * isq::position_vector[m], 34510 * isq::position_vector[m],
+                                   36010 * isq::position_vector[m], 37265 * isq::position_vector[m]};
+  std::array gain = {quantity{0.2}, quantity{0.1}};
 
   print_header(initial);
   state next = state_extrapolation(initial, interval);
diff --git a/example/kalman_filter/kalman_filter-example_4.cpp b/example/kalman_filter/kalman_filter-example_4.cpp
index 0ba7e36d..34c27e7a 100644
--- a/example/kalman_filter/kalman_filter-example_4.cpp
+++ b/example/kalman_filter/kalman_filter-example_4.cpp
@@ -22,16 +22,17 @@
 
 #include "kalman.h"
 #include <units/format.h>
-#include <units/generic/dimensionless.h>
-#include <units/isq/si/acceleration.h>
-#include <units/isq/si/length.h>
-#include <units/isq/si/speed.h>
-#include <units/isq/si/time.h>
+#include <units/isq/space_and_time.h>
+#include <units/si/unit_symbols.h>
 #include <array>
 #include <iostream>
 
 // Based on: https://www.kalmanfilter.net/alphabeta.html#ex4
 
+template<class T>
+  requires units::is_scalar<T>
+inline constexpr bool units::is_vector<T> = true;
+
 using namespace units;
 
 void print_header(const kalman::State auto& initial)
@@ -47,18 +48,18 @@ void print(auto iteration, Quantity auto measured, const kalman::State auto& cur
 
 int main()
 {
-  using namespace units::isq;
-  using namespace units::isq::si::references;
-  using state =
-    kalman::state<si::length<si::metre>, si::speed<si::metre_per_second>, si::acceleration<si::metre_per_second_sq>>;
-  constexpr auto mps = m / s;
-  constexpr auto mps2 = mps / s;
+  using namespace units::si::unit_symbols;
+  using state = kalman::state<quantity<isq::position_vector[m]>, quantity<isq::velocity[m / s]>,
+                              quantity<isq::acceleration[m / s2]>>;
+  const auto interval = 5. * isq::period_duration[s];
+  const state initial = {30 * isq::position_vector[km], 50 * isq::velocity[m / s], 0 * isq::acceleration[m / s2]};
 
-  const auto interval = 5. * s;
-  const state initial = {30 * km, 50 * mps, 0 * mps2};
-  const std::array measurements = {30160 * m, 30365 * m, 30890 * m, 31050 * m, 31785 * m,
-                                   32215 * m, 33130 * m, 34510 * m, 36010 * m, 37265 * m};
-  std::array gain = {dimensionless<one>(0.5), dimensionless<one>(0.4), dimensionless<one>(0.1)};
+  const std::array measurements = {30160 * isq::position_vector[m], 30365 * isq::position_vector[m],
+                                   30890 * isq::position_vector[m], 31050 * isq::position_vector[m],
+                                   31785 * isq::position_vector[m], 32215 * isq::position_vector[m],
+                                   33130 * isq::position_vector[m], 34510 * isq::position_vector[m],
+                                   36010 * isq::position_vector[m], 37265 * isq::position_vector[m]};
+  std::array gain = {quantity{0.5}, quantity{0.4}, quantity{0.1}};
 
   print_header(initial);
   state next = state_extrapolation(initial, interval);
diff --git a/example/kalman_filter/kalman_filter-example_5.cpp b/example/kalman_filter/kalman_filter-example_5.cpp
index d68f9601..4efcc0ce 100644
--- a/example/kalman_filter/kalman_filter-example_5.cpp
+++ b/example/kalman_filter/kalman_filter-example_5.cpp
@@ -22,8 +22,9 @@
 
 #include "kalman.h"
 #include <units/format.h>
-#include <units/isq/si/length.h>
+#include <units/isq/space_and_time.h>
 #include <units/math.h>
+#include <units/si/unit_symbols.h>
 #include <array>
 #include <iostream>
 
@@ -39,7 +40,7 @@ void print_header(kalman::estimation<Q> initial)
                                "Next Estimate");
 }
 
-template<Quantity Q, Dimensionless K>
+template<Quantity Q, quantity_of<dimensionless> K>
 void print(auto iteration, K gain, Q measured, kalman::estimation<Q> current, kalman::estimation<Q> next)
 {
   std::cout << STD_FMT::format("{:2} | {:5%.2Q} | {:8} | {:>16.2} | {:>16.2}\n", iteration, gain, measured, current,
@@ -49,16 +50,18 @@ void print(auto iteration, K gain, Q measured, kalman::estimation<Q> current, ka
 int main()
 {
   using namespace kalman;
-  using namespace units::isq;
-  using namespace units::isq::si::references;
+  using namespace units::si::unit_symbols;
 
-  const estimation initial = {state{60. * m}, pow<2>(15. * m)};
-  const std::array measurements = {48.54 * m, 47.11 * m, 55.01 * m, 55.15 * m, 49.89 * m,
-                                   40.85 * m, 46.72 * m, 50.05 * m, 51.27 * m, 49.95 * m};
-  const auto measurement_uncertainty = pow<2>(5. * m);
+  const estimation initial = {state{60. * isq::height[m]}, pow<2>(15. * isq::height[m])};
+  const std::array measurements = {48.54 * isq::height[m], 47.11 * isq::height[m], 55.01 * isq::height[m],
+                                   55.15 * isq::height[m], 49.89 * isq::height[m], 40.85 * isq::height[m],
+                                   46.72 * isq::height[m], 50.05 * isq::height[m], 51.27 * isq::height[m],
+                                   49.95 * isq::height[m]};
+  const auto measurement_uncertainty = pow<2>(5. * isq::height[m]);
 
-  auto update = [=]<Quantity Q>(const estimation<Q>& previous, const Q& meassurement, Dimensionless auto gain) {
-    return estimation{state_update(previous.state, meassurement, gain), covariance_update(previous.uncertainty, gain)};
+  auto update = [=]<Quantity Q>(const estimation<Q>& previous, const Q& measurement,
+                                quantity_of<dimensionless> auto gain) {
+    return estimation{state_update(previous.state, measurement, gain), covariance_update(previous.uncertainty, gain)};
   };
 
   auto predict = []<Quantity Q>(const estimation<Q>& current) { return current; };