Allow direct process model (#63)

* Add 'NotUsedIntegrator' placeholder

to express that the integrator is not needed when process model is
implemented directly (instead of derivative process model)

* Allow to implement direct process model

instead of derivative process model

---------

Co-authored-by: Remi Peuchot <remi.peuchot@ecorobotix.com>

Author: remipch

include/UKF/Core.h

@@ -27,6 +27,7 @@ SOFTWARE.
 #include <Eigen/Geometry>
 #include "UKF/Types.h"
 #include "UKF/StateVector.h"
+#include <type_traits>
 
 namespace UKF {
 
@@ -45,7 +46,7 @@ The attitude-related code makes use of the MRP method described in the paper
 "Unscented Filtering for Spacecraft Attitude Estimation" by John L. Crassidis
 and F. Landis Markley.
 */
-template <typename StateVectorType, typename MeasurementVectorType, typename IntegratorType>
+template <typename StateVectorType, typename MeasurementVectorType, typename IntegratorType=NotUsedIntegrator>
 class Core {
 protected:
     typename StateVectorType::SigmaPointDistribution sigma_points;
@@ -111,8 +112,13 @@ class Core {
 
         /* Propagate the sigma points through the process model. */
         for(std::size_t i = 0; i < StateVectorType::num_sigma(); i++) {
-            sigma_points.col(i) <<
-                StateVectorType(sigma_points.col(i)).template process_model<IntegratorType>(delta, input...);
+            if (std::is_same_v<IntegratorType, NotUsedIntegrator>) {
+                sigma_points.col(i) << StateVectorType(sigma_points.col(i)).process_model(delta, input...);
+            }
+            else {
+                sigma_points.col(i) << StateVectorType(sigma_points.col(i))
+                    .template process_model<IntegratorType>(delta, input...);
+            }
         }
 
         /* Calculate the a priori estimate mean, deltas and covariance. */

include/UKF/Integrator.h

@@ -27,6 +27,15 @@ SOFTWARE.
 
 namespace UKF {
 
+/* When we want a direct process model (avoid to integrate the process model derivative). */
+class NotUsedIntegrator {
+   public:
+    template <typename S, typename... U>
+    static S integrate(real_t delta, const S& state, const U&... input) {
+        throw std::runtime_error("NotUsedIntegrator is not supposed to be used");
+    }
+};
+
 /* Fourth-order integrator. */
 class IntegratorRK4 {
 public:

include/UKF/StateVector.h

@@ -428,6 +428,7 @@ class StateVector : public StateVectorBaseType<typename Fields::type...> {
     This function calculates the derivative of the state vector. The
     derivative can be a function of the current state and any number of user-
     supplied non-state inputs.
+    It must be implemented if the core does not use "NotUsedIntegrator"
     */
     template <typename... U>
     StateVector derivative(const U&... input) const;
@@ -437,11 +438,18 @@ class StateVector : public StateVectorBaseType<typename Fields::type...> {
     state based on the supplied time delta. This is achieved by using a
     numerical integrator and the derivative function.
     */
-    template <typename IntegratorType = IntegratorRK4, typename... U>
+    template <typename IntegratorType , typename... U>
     StateVector process_model(real_t delta, const U&... input) const {
         return IntegratorType::integrate(delta, *this, input...);
     }
 
+    /*
+    Apply the direct process model (without derivative).
+    It must be implemented if the core use "NotUsedIntegrator"
+    */
+    template <typename... U>
+    StateVector process_model(real_t delta, const U&... input) const;
+
     /* Update the state vector using a delta vector. */
     void apply_delta(const StateVectorDelta& delta) {
         apply_field_deltas<Fields...>(delta);