[attitudeObserver] Add gyro-bias management

README.md

@@ -168,7 +168,7 @@ const auto & X_Camera_Object = datastore().call<const sva::PTransformd &>(name_+
 ## Dependencies
 
 - [gram_savitzky_golay](https://github.com/arntanguy/gram_savitzky_golay)
-- [state-observation](https://github.com/jrl-umi3218/state-observation) > 1.3.3
+- [state-observation](https://github.com/jrl-umi3218/state-observation) > 1.4.1
 - [mc_rtc](https://github.com/jrl-umi3218/mc_rtc)
 - Eigen3
 - Boost

include/mc_state_observation/AttitudeObserver.h

@@ -11,9 +11,9 @@ namespace mc_state_observation
 struct AttitudeObserver : public mc_observers::Observer
 {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  using indexes = stateObservation::kine::indexes<stateObservation::kine::rotationVector>;
+  using indexes = stateObservation::IMUDynamicalSystem::indexes;
 
- public:
+public:
   AttitudeObserver(const std::string & type, double dt);
 
   void configure(const mc_control::MCController & ctl, const mc_rtc::Configuration &) override;
@@ -32,7 +32,10 @@ struct AttitudeObserver : public mc_observers::Observer
     double gyroCovariance = 1e-10;
     double orientationAccCov = 0.003;
     double linearAccCov = 1e-13;
+    double biasDriftCov = 1e-10;
+    double biasInitCov = 1e-7;
     double stateCov = 3e-14;
+    double oriCov = 1e-12;
     double stateInitCov = 1e-8;
     Eigen::Matrix3d offset = Eigen::Matrix3d::Identity(); ///< Offset to apply to the estimation result
     void addToLogger(mc_rtc::Logger & logger, const std::string & category);
@@ -62,6 +65,11 @@ struct AttitudeObserver : public mc_observers::Observer
                 mc_rtc::gui::StateBuilder &,
                 const std::vector<std::string> & /* category */) override;
 
+  void switchWithGyroBias(bool);
+
+  void setGyroBiasToMeanValue(); /// set the value of the bias to the mean identified value
+  void startGyroBiasIdentification(); /// start the identification of the bias from the curren gyro readings
+
 protected:
   /// @{
   std::string robot_ = ""; ///< Name of robot to which the IMU sensor belongs
@@ -72,12 +80,14 @@ struct AttitudeObserver : public mc_observers::Observer
   KalmanFilterConfig config_; ///< Current configuration for the KF (GUI, etc...)
   bool log_kf_ = false; ///< Whether to log the parameters of the kalman filter
   bool initFromControl_ = true; ///< Whether to initialize from the control state
-  /// @}
+  bool withGyroBias_ = false; ///< Whether the estimator will be considering gyro bias
 
   /// Sizes of the states for the state, the measurement, and the input vector
-  static constexpr unsigned STATE_SIZE = 18;
-  static constexpr unsigned MEASUREMENT_SIZE = 6;
-  static constexpr unsigned INPUT_SIZE = 6;
+  static constexpr stateObservation::Index STATE_SIZE_BASE = 18;
+  static constexpr stateObservation::Index MEASUREMENT_SIZE = 6;
+  static constexpr stateObservation::Index INPUT_SIZE = 6;
+
+  stateObservation::Index stateSize_ = STATE_SIZE_BASE;
 
   double lastStateInitCovariance_;
 
@@ -93,12 +103,23 @@ struct AttitudeObserver : public mc_observers::Observer
   stateObservation::Vector uk_;
   stateObservation::Vector xk_;
 
-  stateObservation::Matrix3 Kpt_, Kdt_;
-  stateObservation::Matrix3 Kpo_, Kdo_;
+  stateObservation::Matrix3 Kpt_, Kdt_, Kat_;
+  stateObservation::Matrix3 Kpo_, Kdo_, Kao_;
 
   Eigen::Matrix3d m_orientation = Eigen::Matrix3d::Identity(); ///< Result
-};
+  Eigen::Vector3d m_gyrobias = Eigen::Vector3d::Zero();
+  Eigen::Vector3d m_gyroMeasurementTotal =
+      Eigen::Vector3d::Zero(); /// the sum of values of gyro measurements (used to compute the mean value)
+  unsigned m_numberOfValues = 0;
+
+  bool m_resetGUI = false;
+  std::vector<std::string> m_category;
 
+  bool m_gyroBiasFromMeasurement_running = false; /// sets if the identification of the gyro bias is stoped
+
+  Eigen::Vector3d m_accIn = Eigen::Vector3d::Zero();
+  Eigen::Vector3d m_rateIn = Eigen::Vector3d::Zero();
+};
 
 } // namespace mc_state_observation
 
@@ -116,6 +137,7 @@ struct ConfigurationLoader<mc_state_observation::AttitudeObserver::KalmanFilterC
     config("gyr_cov", c.gyroCovariance);
     config("ori_acc_cov", c.orientationAccCov);
     config("lin_acc_cov", c.linearAccCov);
+    config("bias_drift_cov", c.biasDriftCov);
     config("state_cov", c.stateCov);
     config("state_init_cov", c.stateInitCov);
     return c;
@@ -128,6 +150,7 @@ struct ConfigurationLoader<mc_state_observation::AttitudeObserver::KalmanFilterC
     config.add("acc_cov", c.acceleroCovariance);
     config.add("gyr_cov", c.gyroCovariance);
     config.add("ori_acc_cov", c.orientationAccCov);
+    config.add("bias_drift_cov", c.biasDriftCov);
     config.add("lin_acc_cov", c.linearAccCov);
     config.add("state_cov", c.stateCov);
     config.add("state_init_cov", c.stateInitCov);

include/mc_state_observation/gui_helpers.h

@@ -35,6 +35,13 @@ auto make_number_input(std::string name, T & value)
   return NumberInputImpl<decltype(GetF), decltype(SetF)>(name, GetF, SetF);
 }
 
+auto make_xyz_input(std::string name, Eigen::Vector3d & vec)
+{
+  auto GetF = [&vec]() -> Eigen::Vector3d { return vec; };
+  auto SetF = [&vec](const Eigen::Vector3d & vecIn) { vec = vecIn; };
+  return ArrayInputImpl<decltype(GetF), decltype(SetF)>(name, {"x", "y", "z"}, GetF, SetF);
+}
+
 auto make_rpy_input(std::string name, Eigen::Matrix3d & orientation)
 {
   auto GetF = [&orientation]() -> Eigen::Vector3d