AbsolutePosition: Enable for UKF

rpavlik · Nov 8, 2019 · e1c8ed8 · e1c8ed8
1 parent feef33c
commit e1c8ed8
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 6 deletions.
diff --git a/generated/FlexKalman.h b/generated/FlexKalman.h
@@ -2358,7 +2358,16 @@ class AbsolutePositionMeasurementBase {
     MeasurementMatrix getCovariance(State const &) const {
         return m_covariance;
     }
-
+    template <typename State>
+    MeasurementVector predictMeasurement(State const &s) const {
+        return s.position();
+    }
+    template <typename State>
+    MeasurementVector getResidual(MeasurementVector const &prediction,
+                                  State const &s) const {
+        MeasurementVector residual = m_pos - prediction;
+        return residual;
+    }
     /*!
      * Gets the measurement residual, also known as innovation: predicts
      * the measurement from the predicted state, and returns the
@@ -2368,8 +2377,7 @@ class AbsolutePositionMeasurementBase {
      */
     template <typename State>
     MeasurementVector getResidual(State const &s) const {
-        MeasurementVector residual = m_pos - s.position();
-        return residual;
+        return getResidual(predictMeasurement(s), s);
     }
 
     //! Convenience method to be able to store and re-use measurements.

diff --git a/inc/FlexKalman/AbsolutePositionMeasurement.h b/inc/FlexKalman/AbsolutePositionMeasurement.h
@@ -52,7 +52,16 @@ class AbsolutePositionMeasurementBase {
     MeasurementMatrix getCovariance(State const &) const {
         return m_covariance;
     }
-
+    template <typename State>
+    MeasurementVector predictMeasurement(State const &s) const {
+        return s.position();
+    }
+    template <typename State>
+    MeasurementVector getResidual(MeasurementVector const &prediction,
+                                  State const &s) const {
+        MeasurementVector residual = m_pos - prediction;
+        return residual;
+    }
     /*!
      * Gets the measurement residual, also known as innovation: predicts
      * the measurement from the predicted state, and returns the
@@ -62,8 +71,7 @@ class AbsolutePositionMeasurementBase {
      */
     template <typename State>
     MeasurementVector getResidual(State const &s) const {
-        MeasurementVector residual = m_pos - s.position();
-        return residual;
+        return getResidual(predictMeasurement(s), s);
     }
 
     //! Convenience method to be able to store and re-use measurements.