WIP: save work.

oddkiva · Nov 1, 2023 · 9e0a3ae · 9e0a3ae
1 parent d763793
commit 9e0a3ae
Showing 1 changed file with 10 additions and 37 deletions.
diff --git 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