From 9e0a3ae0e337a97b884d8b4ce11db631df3618f4 Mon Sep 17 00:00:00 2001
From: David OK <david.ok8@gmail.com>
Date: Wed, 1 Nov 2023 15:22:14 +0000
Subject: [PATCH] WIP: save work.

---
 cpp/src/DO/Sara/KalmanFilter/KalmanFilter.hpp | 47 ++++---------------
 1 file changed, 10 insertions(+), 37 deletions(-)

diff --git a/cpp/src/DO/Sara/KalmanFilter/KalmanFilter.hpp b/cpp/src/DO/Sara/KalmanFilter/KalmanFilter.hpp
index de1f95881..c2a727862 100644
--- a/cpp/src/DO/Sara/KalmanFilter/KalmanFilter.hpp
+++ b/cpp/src/DO/Sara/KalmanFilter/KalmanFilter.hpp
@@ -3,51 +3,24 @@
 
 namespace DO::Sara::KalmanFilter {
 
-  //! @brief The step-by-step evolution of the 3D bounding box of the
-  //! pedestrian.
-  template <typename StateVector,            //
-            typename StateTransitionMatrix,  //
-            typename ProcessNoiseVector>
-  struct StateTransitionEquation
+  template <typename StateTransitionEquation, typename ObservationEquation>
+  struct KalmanFilter
   {
-    StateTransitionMatrix F;
-    ProcessNoiseVector w;
+    using State = typename StateTransitionEquation::State;
+    using Observation = typename StateTransitionEquation::Observation;
 
-    inline auto operator()(const StateVector& x) const -> StateVector
+    auto predict(const State& x) -> State
     {
-      return F * x + w;
+      return _state_transition_equation.predict(x);
     }
-  };
 
-  //! @brief The projection of the 3D bounding box into the image plane.
-  //! Sort of...
-  template <typename StateVector,        //
-            typename ObservationVector,  //
-            typename ObservationMatrix,  //
-            typename MeasurementNoiseVector>
-  struct ObservationEquation
-  {
-    ObservationMatrix H;
-    MeasurementNoiseVector v;
-
-    inline auto operator()(const StateVector& x) const -> ObservationVector
+    auto update(const State& x_predicted, const Observation& z) -> State
     {
-      // z = observation vector.
-      const auto z = H * x + v;
-      return z;
+      return _observation_equation.update(x_predicted, z);
     }
-  };
-
-
-  template <typename StateTransitionEquation, typename ObservationEquation>
-  struct KalmanFilter
-  {
-    template <typename StateVector>
-    auto predict(const StateVector& x)
-        -> std::pair<APrioriStateMeanVector, APrioriStateCovarianceMatrix>;
 
-    auto update() -> std::pair<APosterioriStateMeanVector,
-                               APosterioriStateCovarianceMatrix>;
+    StateTransitionEquation _state_transition_equation;
+    ObservationEquation _observation_equation;
   };
 
 }  // namespace DO::Sara::KalmanFilter