[FloatingBaseEstimators] General improvements

src/Contacts/include/BipedalLocomotion/Contacts/Contact.h

@@ -111,6 +111,7 @@ struct EstimatedContact : ContactBase
 
 };
 
+using EstimatedLandmark = EstimatedContact;
 
 }
 }

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimator.h

@@ -5,8 +5,8 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_H
-#define BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_H
+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_H
 
 #include <BipedalLocomotion/System/Advanceable.h>
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
@@ -45,9 +45,9 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Advanceable<Floa
  public:
  /**
  * Set the shared kindyn object
- * @param[in] kinDyn shared pointer of the common KinDynComputations resource 
+ * @param[in] kinDyn shared pointer of the common KinDynComputations resource
  * @return True in case of success, false otherwise.
- * 
+ *
  * @note Expects only a valid pointer to the object, need not be loaded with the robot model.
  */
  bool setKinDynObject(std::shared_ptr<iDynTree::KinDynComputations> kinDyn);
@@ -157,10 +157,17 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Advanceable<Floa
  * it must be done in customInitialization() by the child class implementing the algorithm
  * @return True in case of success, false otherwise.
  */
- bool initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler, 
+ bool initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
  std::shared_ptr<iDynTree::KinDynComputations> kindyn);
 
 
+ /**
+ * Configure generic parameters, calling this overloaded method assumes model information is not going to be used.
+ * @param[in] handler configure the generic parameters for the estimator
+ * @return True in case of success, false otherwise.
+ */
+ bool initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
+
  /**
  * Set the polled IMU measurement
  * @param[in] accMeas linear accelerometer measurement expressed in the local IMU frame (m/s^2)
@@ -180,14 +187,14 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Advanceable<Floa
 
  /**
  * Set contact status
- * 
+ *
  * @param[in] name contact frame name
  * @param[in] contactStatus flag to check active contact
  * @param[in] switchTime time of switching contact
  * @param[in] timeNow current measurement update time
  */
- bool setContactStatus(const std::string& name, 
- const bool& contactStatus, 
+ bool setContactStatus(const std::string& name,
+ const bool& contactStatus,
  const double& switchTime,
  double timeNow = 0.);
 
@@ -201,6 +208,20 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Advanceable<Floa
  bool setKinematics(const Eigen::VectorXd& encoders,
  const Eigen::VectorXd& encoderSpeeds);
 
+ /**
+ * Set the relative pose of a landmark relative to the base link
+ *
+ * @param[in] landmarkID unqiue landmark ID
+ * @param[in] quat relative orientation of the landmark as a quaternion
+ * @param[in] pos relative position of the landmark
+ * @param[in] timeNow relative position of the landmark
+ * @return true in case of success, false otherwise
+ */
+ bool setLandmarkRelativePose(const int& landmarkID,
+ const Eigen::Quaterniond& quat,
+ const Eigen::Vector3d& pos,
+ const double& timeNow);
+
  /**
  * Compute one step of the estimator
  * @return True in case of success, false otherwise.
@@ -247,13 +268,6 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Advanceable<Floa
  ModelComputations& modelComputations();
 
 protected:
- /**
- * Configure generic parameters
- * @param[in] handler configure the generic parameters for the estimator
- * @return True in case of success, false otherwise.
- */
- bool initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
-
  /**
  * These custom parameter specifications should be specified by the derived class.
  * - If setupOptions() is called from within customInitialization(), ensure the group "Options" exist.
@@ -289,6 +303,15 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Advanceable<Floa
  virtual bool updateKinematics(FloatingBaseEstimators::Measurements& meas,
  const double& dt) { return true; };
 
+ /**
+ * Update the predicted state estimates using relative pose measurements of static landmarks in the environment
+ * @param[in] meas measurements to update the predicted states
+ * @param[in] dt sampling period in seconds
+ * @return True in case of success, false otherwise.
+ */
+ virtual bool updateLandmarkRelativePoses(FloatingBaseEstimators::Measurements& meas,
+ const double& dt) { return true; };
+
  /**
  * Setup estimator options. The parameters in the Options group are,
  * - enable_imu_bias_estimation [PARAMETER|-|Enable/disable IMU bias estimation]
@@ -381,6 +404,8 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Advanceable<Floa
  double m_dt{0.01}; /**< Fixed time step of the estimator, in seconds */
  bool m_useIMUForAngVelEstimate{true}; /**< Use IMU measurements as internal state imu angular velocity by default set to true for strap down IMU based EKF implementations, if IMU measurements not used, corresponding impl can set to false */
  bool m_useIMUVelForBaseVelComputation{true}; /**< Compute base velocity using inertnal state IMU velocity. by default set to true for strap down IMU based EKF implementations, if IMU measurements not used, corresponding impl can set to false */
+ bool m_useModelInfo{true}; /**< Flag to enable running the estimator without using the URDF model information*/
+ bool m_isInvEKF{false}; /**< Flag to maintain soon to be deprecated functionalities currently existing only in InvEKFBaseEstimator */
 private:
  /**
  * Setup model related parameters
@@ -420,5 +445,5 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Advanceable<Floa
 } // namespace Estimators
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_H
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_H
 

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimatorIO.h

@@ -5,13 +5,14 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_IO_H
-#define BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_IO_H
+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_IO_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_IO_H
 
 #include <Eigen/Dense>
 #include <manif/manif.h>
 
 #include <BipedalLocomotion/Contacts/Contact.h>
+#include <manif/manif.h>
 #include <map>
 
 namespace BipedalLocomotion
@@ -38,11 +39,12 @@ struct InternalState
  Eigen::Vector3d gyroscopeBias; /**< Bias of the gyroscope expressed in the IMU frame */
 
  Eigen::Vector3d imuAngularVelocity; /**< angular velocity of the IMU with respect to the inertial frame expressed in inertial frame, typically unused for strap-down IMU based EKF implementations*/
- std::map<int, BipedalLocomotion::Contacts::EstimatedContact> supportFrameData; /**< contact measurements */
+ std::map<int, BipedalLocomotion::Contacts::EstimatedContact> supportFrameData; /**< estimated contacts */
+ std::map<int, BipedalLocomotion::Contacts::EstimatedLandmark> landmarkData; /**< estimated landmarks */
 };
 
 /**
-* @brief Struct holding the elements of the state representation
+* @brief Struct holding the elements of the estimator output
 *
 */
 struct Output
@@ -55,7 +57,7 @@ struct Output
 };
 
 /**
-* @brief Struct holding the elements of the state representation
+* @brief Struct holding the elements of the measurements data
 *
 */
 struct Measurements
@@ -64,11 +66,18 @@ struct Measurements
  Eigen::VectorXd encoders, encodersSpeed; /**< Joint position and joint velocity measurements */
  bool lfInContact{false}; /**< left foot contact state */
  bool rfInContact{false}; /**< right foot contact state */
-
- /** stamped contact status, 
- * the usage of this map must be in a way 
- * that every time an element is used, 
- * it must be erased from the map 
+
+ /** stamped relative poses,
+ * the usage of this map must be in a way
+ * that every time an element is used,
+ * it must be erased from the map
+ */
+ std::map<int, BipedalLocomotion::Contacts::EstimatedContact > stampedRelLandmarkPoses;
+
+ /** stamped contact status,
+ * the usage of this map must be in a way
+ * that every time an element is used,
+ * it must be erased from the map
  */
  std::map<int, BipedalLocomotion::Contacts::EstimatedContact > stampedContactsStatus;
 };
@@ -77,5 +86,5 @@ struct Measurements
 } // namespace Estimators
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_IO_H
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_IO_H
 

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimatorParams.h

@@ -5,17 +5,23 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_PARAMS_H
-#define BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_PARAMS_H
+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_PARAMS_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_PARAMS_H
 
 #include <Eigen/Dense>
+#include <map>
 
 namespace BipedalLocomotion
 {
 namespace Estimators
 {
 namespace FloatingBaseEstimators
 {
+
+// see https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html
+using PoseCovariance = std::map<int, Eigen::Matrix<double, 6, 1>, std::less<int>,
+ Eigen::aligned_allocator<std::pair<const int, Eigen::Matrix<double, 6, 1> > > >;
+
 /**
 * @brief Struct containing sensor measurement deviation parameters of floating base estimators
 *
@@ -80,6 +86,28 @@ struct SensorsStdDev
  * @brief White Gaussian noise deviation for encoder measurements in continuous time
  */
  Eigen::VectorXd encodersNoise;
+
+ /**
+ * @brief White Gaussian noise deviation for landmark relative pose predictions in continuous time
+ */
+ Eigen::Matrix<double, 6, 1> landmarkPredictionNoise;
+
+ /**
+ * @brief White Gaussian noise deviation for landmark relative pose measurments in continuous time
+ */
+ Eigen::Matrix<double, 6, 1> landmarkMeasurementNoise;
+
+ /**
+ * @brief White Gaussian noise deviation for linear velocity of IMU frame wrt inertial frame
+ * Expressed in local frame as m/s, in continuous time
+ */
+ Eigen::Vector3d imuFrameLinearVelocityNoise;
+
+ /**
+ * @brief White Gaussian noise deviation for angular velocity of IMU frame wrt inertial frame
+ * Expressed in local frame as rad/s, in continuous time
+ */
+ Eigen::Vector3d imuFrameAngularVelocityNoise;
 };
 
 /**
@@ -106,6 +134,12 @@ struct StateStdDev
  */
  Eigen::Vector3d imuLinearVelocity;
 
+ /**
+ * @brief Prior deviation of mixed-trivialized IMU angular velocity
+ * expressed in rad/s
+ */
+ Eigen::Vector3d imuAngularVelocity;
+
  /**
  * @brief Prior deviation of left foot contact frame orientation in inertial frame
  * expressed in radians as rpy
@@ -142,6 +176,16 @@ struct StateStdDev
  * expressed in rad/s
  */
  Eigen::Vector3d gyroscopeBias;
+
+ /**
+ * @brief Container of deviations of support frame pose in inertial frame
+ */
+ PoseCovariance supportFramePose;
+
+ /**
+ * @brief Container of deviations of landmark pose in inertial frame
+ */
+ PoseCovariance landmarkPose;
 };
 
 
@@ -178,6 +222,17 @@ struct Options
  */
  bool ekfUpdateEnabled{true};
 
+ /**
+ * @brief Enable/disable kinematics based correction updates
+ */
+ bool kinematicsUpdateEnabled{true};
+
+ /**
+ * @brief Enable/disable landmarks based correction updates
+ */
+ bool staticLandmarksUpdateEnabled{false};
+
+
  /**
  * @brief Acceleration vector due to gravity
  *
@@ -189,4 +244,4 @@ struct Options
 } // namespace Estimators
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_PARAMS_H
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_PARAMS_H