From 310b0826ecd0a253a1e6676b0b629d3a31bfec30 Mon Sep 17 00:00:00 2001 From: Prashanth Date: Sat, 27 Jul 2019 18:53:37 +0200 Subject: [PATCH] [estimation][ExtendedKalmanFilter] add dynamic reset --- doc/releases/v0_12.md | 1 + .../Estimation/AttitudeQuaternionEKF.h | 1 + .../Estimation/ExtendedKalmanFilter.h | 48 ++++++++++++++++++- src/estimation/src/AttitudeQuaternionEKF.cpp | 9 ++++ src/estimation/src/ExtendedKalmanFilter.cpp | 48 ++++++++++++++++++- 5 files changed, 105 insertions(+), 2 deletions(-) diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md index 3370a7d4212..dc626b7f16b 100644 --- a/doc/releases/v0_12.md +++ b/doc/releases/v0_12.md @@ -42,6 +42,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput * Added a new version of `changeFixedFrame` in `SimpleLeggedOdometry`class. This can be used to set a desired homogeneous transformation for the fixed frame * 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 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) diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h index 15ef2a90a6c..24fc436de5f 100644 --- a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h +++ b/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 diff --git a/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h b/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h index ffd418ed820..d0f93f1df64 100644 --- a/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h +++ b/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& x0, + const iDynTree::Span& P0, + const iDynTree::Span& Q, + const iDynTree::Span& 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 &P) const; + protected: + /** + * function template to ignore unused parameters + */ + template + void ignore(T &&) { } + private: size_t m_dim_X; ///< state dimension size_t m_dim_Y; ///< output dimenstion diff --git a/src/estimation/src/AttitudeQuaternionEKF.cpp b/src/estimation/src/AttitudeQuaternionEKF.cpp index a70c97af6cd..bb8e203368c 100644 --- a/src/estimation/src/AttitudeQuaternionEKF.cpp +++ b/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; diff --git a/src/estimation/src/ExtendedKalmanFilter.cpp b/src/estimation/src/ExtendedKalmanFilter.cpp index 854994343d2..a01c8bebde1 100644 --- a/src/estimation/src/ExtendedKalmanFilter.cpp +++ b/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& x0, const iDynTree::Span& P0, + const iDynTree::Span& Q, const iDynTree::Span& 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;