[Estimation] Dynamic reset functionality for ExtendedKalmanFilter

doc/releases/v0_12.md

@@ -43,6 +43,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Added `bindings` for `AttitudeMahonyFilter`, `AttitudeQuaternionEKF`, `DiscreteExtendedKalmanFilterHelper` (https://github.com/robotology/idyntree/pull/522)
 * Added basic tests for the Attitude Estimator classes (https://github.com/robotology/idyntree/pull/522)
 * Added `DiscreteKalmanFilterHelper` class for an implementation of a discrete, linear time-invariant Kalman Filter  (https://github.com/robotology/idyntree/pull/559)
+* Added dynamic reset functionality to `DiscreteExtendedKalmanFilterHelper` class (https://github.com/robotology/idyntree/pull/553)
 
 #### `bindings`
 * Added high-level Matlab/Octave wrappers of the iDyntree bindings (https://github.com/robotology/idyntree/pull/530)

src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h

@@ -253,6 +253,7 @@ namespace iDynTree
          * @return bool true/false if successful or not
          */
         bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) override;
+        bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F) override;
 
         /**
          * @brief Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance

src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h

@@ -90,15 +90,22 @@ namespace iDynTree
         virtual bool ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one,
                              iDynTree::VectorDynSize& zhat_k_plus_one) = 0;
 
+        /**
+         * @overload
+         */
+        virtual bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) = 0;
+
         /**
          * @brief Describes the system Jacobian necessary for the propagation of predicted state covariance
          *        The analytical Jacobian describing the partial derivative of the system propagation with respect to the state
+         *        and the system propagation with respect to the input
          * @note the detail of this function needs to be implemented by the child class
          * @param[in] x system state
+         * @param[in] u system input
          * @param[out] F system Jacobian
          * @return bool true/false if successful or not
          */
-        virtual bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) = 0;
+        virtual bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u,  iDynTree::MatrixDynSize& F) = 0;
 
         /**
          * @brief Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance
@@ -153,6 +160,19 @@ namespace iDynTree
          */
         bool ekfInit();
 
+        /**
+         * @brief Initializes and resizes the internal buffers of this filter
+         * @warning this is a very crucial method of this class. This method sets the input size through ekfSetInputSize(),
+         *          output size through ekfSetOutputSize() and state dimension through ekfSetStateSize() with the specified parameters,
+         *          such that the corresponding matrices and vectors will resize themselves to their corresponding dimensions.
+         *          Failing to do so might result in memory leaks and may cause the program to crash
+         * @param[in] state_size state size
+         * @param[in] input_size input size
+         * @param[in] output_size output size
+         * @return bool true/false if successful or not
+         */
+        bool ekfInit(const size_t& state_size, const size_t& input_size, const size_t& output_size);
+
         /**
          * @brief Resets the filter flags
          * The filter flags check if the filter was properly initialized, if the initial state was set,
@@ -161,6 +181,25 @@ namespace iDynTree
          */
         void ekfReset();
 
+        /**
+         * @brief Resets the filter flags, initializes and resizes internal buffers of the filter, and
+         *             sets initial state, initial state covariance, and system noise and measurement noise covariance matrices
+         * @warning size of the span for P0 and Q must be of the size (state size*state size), where * is the regular multiplication operator
+         * @warning size of the span for R must be of the size (ouput size*output size),  where * is the regular multiplication operator
+         * @warning the matrices from the span are built in row-major ordering.
+         *
+         * @note this method is particularly useful while working with hybrid systems,
+         *             where the size of the system state or the measurements keep evolving with time
+         *
+         */
+        bool ekfReset(const size_t& state_size,
+                                 const size_t& input_size,
+                                 const size_t& output_size,
+                                 const iDynTree::Span<double>& x0,
+                                 const iDynTree::Span<double>& P0,
+                                 const iDynTree::Span<double>& Q,
+                                 const iDynTree::Span<double>& R);
+
         /**
          * @brief Set measurement vector at every time step
          * the measurement vector size and output size should match
@@ -254,6 +293,13 @@ namespace iDynTree
          */
         bool ekfGetStateCovariance(const iDynTree::Span<double> &P) const;
 
+   protected:
+        /**
+        * function template to ignore unused parameters
+        */
+        template <typename T>
+        void ignore(T &&) { }
+
     private:
         size_t m_dim_X;                                ///< state dimension
         size_t m_dim_Y;                                ///< output dimenstion

src/estimation/src/AttitudeQuaternionEKF.cpp

@@ -287,6 +287,15 @@ bool iDynTree::AttitudeQuaternionEKF::updateFilterWithMeasurements(const iDynTre
     return ok;
 }
 
+bool iDynTree::AttitudeQuaternionEKF::ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F)
+{
+    ignore(u);
+    ignore(x);
+    ignore(F);
+    return false;
+}
+
+
 bool iDynTree::AttitudeQuaternionEKF::ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F)
 {
     using iDynTree::toEigen;

src/estimation/src/ExtendedKalmanFilter.cpp

@@ -24,6 +24,53 @@ void iDynTree::DiscreteExtendedKalmanFilterHelper::ekfReset()
     m_initial_state_covariance_set = false;
 }
 
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfInit(const size_t& state_size, const size_t& input_size, const size_t& output_size)
+{
+    m_dim_X = state_size;
+    m_dim_U = input_size;
+    m_dim_Y = output_size;
+
+    return ekfInit();
+}
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfReset(const size_t& state_size, const size_t& input_size, const size_t& output_size,
+                                                                                                        const iDynTree::Span<double>& x0, const iDynTree::Span<double>& P0,
+                                                                                                        const iDynTree::Span<double>& Q, const iDynTree::Span<double>& R)
+{
+    m_is_initialized = false;
+    m_measurement_updated = false;
+    m_input_updated = false;
+    m_initial_state_set = false;
+    m_initial_state_covariance_set = false;
+
+    if (!ekfInit(state_size, input_size, output_size))
+    {
+        return false;
+    }
+
+    if (!ekfSetInitialState(x0))
+    {
+        return false;
+    }
+
+    if (!ekfSetStateCovariance(P0))
+    {
+        return false;
+    }
+
+    if (!ekfSetSystemNoiseCovariance(Q))
+    {
+        return false;
+    }
+
+    if (!ekfSetMeasurementNoiseCovariance(R))
+    {
+        return false;
+    }
+
+    return true;
+}
+
 
 bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfInit()
 {
@@ -252,7 +299,6 @@ bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfGetStates(const iDynTree::
 
     for (size_t i = 0; i < m_dim_X; i++)
     {
-        double temp{m_x(i)};
         x(i) = m_x(i);
     }
     return true;