[attitudeObserver] Add gyro-bias management #17

mehdi-benallegue · 2022-08-12T09:32:57Z

This PR allows to calibrate the gyro bias for attitude esitmation while the controller is running. There are two methods to estimating the bias:
-while the robot is static, the user can compute the average measurement (that should be zero). This is accessible in the GUI of the state observer.
-using the gyro bias estimator embedded in the same Kalman filter used to estimate the attitude. This estimator can also be initialized with the average estimation coming from the first method. This estimator can be activated in the GUI of the state observer too.

This requires this PR
jrl-umi3218/mc_rtc#282

arntanguy

Sorry for the delay.

I just gave this a try, most comments are inline in the code.

Main points:

We should check the state-observation minimum version in cmake
When I click on EKF bias estimation I get 10e13 bias values and the robot falls immediately
Compensate Mode also makes the robot fall
I fixed a bug with the gui_helpers in the main branch (0fd1bb4) which fixes the GUI here (rebase needed)

Note to easily test the stabilization failures, you can simply edit <mc_rtc_install_prefix>/lib/mc_controller/etc/LIPMStabilizer.yaml and add the Attitude observer:

ObserverPipelines:
...
- type: Attitude
- type: KinematicInertial

And you can try in choreonoid with any robot and the LIPMStabilizer controller (I only tried HRP2KAI).

Once all issues are resolved here I'll see about integrating the Attitude observer directly in mc_rtc and using it as the default observer.

arntanguy · 2022-09-02T14:36:59Z

include/mc_state_observation/AttitudeObserver.h

+  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


Suggested change

void startGyroBiasIdentification(); /// start the identification of the bias from the curren gyro readings

void startGyroBiasIdentification(); /// start the identification of the bias from the current gyro readings

arntanguy · 2022-09-02T15:51:18Z

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


We should make this a version requirement in cmake find_package.
While trying this PR locally, I didn't update the state-observation package. Viciously, this builds, but the estimation is nonsense (with the stabilizer just standing, the robot estimate leans backwards and the robot falls forward). I assume this is because of an index mismatch.

Enforcing the minimal version of state-observation would prevent such subtle errors.

arntanguy · 2022-09-02T15:52:58Z

include/mc_state_observation/AttitudeObserver.h

    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;


Not directly related to this PR, but I found a bug in gui_helpers (dangling reference making the GUI for these parameters meaningless), fixed by 0fd1bb4. You'll need to rebase to fix the GUI.

arntanguy · 2022-09-02T15:56:47Z

src/AttitudeObserver.cpp

+                   Checkbox(
+                       "EKF bias\nestimation", [this]() { return withGyroBias_; },
+                       [this]() { switchWithGyroBias(!withGyroBias_); }),


There seems to be an issue with the EKF bias estimation. If I switch it on while using the LIPMStabilizer controller (with hrp2), the robot immediately falls down and the gyro bias becomes on the order of 10e13.

Probably unrelated by Compensate mode also makes the robot fall.

arntanguy · 2022-09-02T16:06:39Z

src/AttitudeObserver.cpp

+                   Checkbox(
+                       "EKF bias\nestimation", [this]() { return withGyroBias_; },
+                       [this]() { switchWithGyroBias(!withGyroBias_); }),
+                   Button("Calibrate\nfrom data\n(static robot)",


If I understood correctly, one needs to manually stop the calibration process? Isn't there a way to automatically detect when calibration is complete?

mehdi-benallegue · 2022-09-09T02:25:49Z

Thank you very much for your reply and sorry for the delay

Unfortunately, for technical reasons I cannot immediately check the code, but I can reply to some remarks and questions

Note to easily test the stabilization failures, you can simply (...) and the LIPMStabilizer controller (I only tried HRP2KAI).

Thank you, this is actually how I test it too. I used HRP4CR and it worked very well. This is why I am surprised by that:

When I click on EKF bias estimation I get 10e13 bias values and the robot falls immediately

I will test it again ASAP

Compensate Mode also makes the robot fall

Is there anybody currently using the Compensate mode? this feature was implemented to keep the compatibility with regard to the old KF. If nobody use it I think it should be deprecated and eventually removed.

I fixed a bug with the gui_helpers in the main branch ([0fd1bb4](https://github.com/jrl-umi3218/mc_state_observation/commit/0fd1bb4ed37463d762232feef8c0d990130cf2fb)) which fixes the GUI here (rebase needed)

I will check that

Once all issues are resolved here I'll see about integrating the Attitude observer directly in mc_rtc and using it as the default observer.

Sounds nice. I think it deserves to be cleaned up a bit though. I planned to ask Arnaud Demont to work on it to finish it by the end of this year.

arntanguy · 2023-01-25T11:51:20Z

I gave this another try today, and it fails as previously reported:

As soon as EKF bias estimation is selected in the GUI, the gyro bias diverges and the robot starts spinning around increasingly faster
The static gyro bias estimation seems to be working fine, however it would need to be activated from the start, otherwise the initial drift accumulated before its estimation remains forever.

For example on HRP2012c, you can see that it initially drifts (gyro bias not estimated yet), and as soon as I trigger the static estimation of the gyro bias the drifiting rate is reduced (although not completely removed).

Regarding the EFK issue, it is reproducible in choreonoid and can be tested very simply with a posture controller and the observer activated. To do so, put the following in ~/.config/mc_rtc/controllers/Posture.yaml:

ObserverPipelines:
  name: "TestAttitudeBug"
  gui: true
  observers:
    - type: Encoder
    - type: Attitude
    - type: BodySensor

Then run a posture controller

MainRobot: HRP2DRC
Enabled: Posture

Note that this also fails with the JVRC robot.

I've also rebased this branch on master here: https://github.com/arntanguy/mc_state_observation/tree/topic/GyroBias

@mehdi-benallegue Could we look into solving this issue? I'm available to test any solution on the real HRP2012 robot.

mehdi-benallegue · 2023-01-26T05:39:23Z

Thank you very much @arntanguy to look into that. This is now my main priority.

mehdi-benallegue added 11 commits August 12, 2022 12:01

[AttitudeObserver] Switch to the class-specific indexes.

22fde99

[guiHelpers] Add a helper for a "x, y, z" vector

e8ff421

Remove a vscode-related configutation file

3de4565

[attitudeObserver] Improve code legibility

caa44df

[attitudeObserver] make reset put back the time to zero

ba39b14

[attitudeObserver] Add gyro-bias management

a40996e

[attitudeEstimator] Add acceleration gains for the jerk imput of IMU

853013b

[attitudeObserver] Add logger for gyroBias

66d754e

[doc] upate the minimal version of state-observation

7674685

[AttitudeObserver] fix typo

4f9ba84

[AttitudeObserver] Give text output for the calibration of the gyro bias

b7ee02f

mehdi-benallegue marked this pull request as ready for review September 2, 2022 06:49

arntanguy requested changes Sep 2, 2022

View reviewed changes

mehdi-benallegue mentioned this pull request Sep 8, 2022

ZMP and CoM error increasing in StabilizerTask when using Attitude observer #6

Open

mehdi-benallegue mentioned this pull request Nov 27, 2022

Question about IMU dynamics input in AttitudeObserver #21

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[attitudeObserver] Add gyro-bias management #17

[attitudeObserver] Add gyro-bias management #17

mehdi-benallegue commented Aug 12, 2022 •

edited

Loading

arntanguy left a comment

arntanguy Sep 2, 2022

arntanguy Sep 2, 2022

arntanguy Sep 2, 2022

arntanguy Sep 2, 2022

arntanguy Sep 2, 2022

arntanguy Sep 2, 2022

mehdi-benallegue commented Sep 9, 2022 •

edited

Loading

arntanguy commented Jan 25, 2023

mehdi-benallegue commented Jan 26, 2023 •

edited

Loading

	void startGyroBiasIdentification(); /// start the identification of the bias from the curren gyro readings
	void startGyroBiasIdentification(); /// start the identification of the bias from the current gyro readings

[attitudeObserver] Add gyro-bias management #17

Are you sure you want to change the base?

[attitudeObserver] Add gyro-bias management #17

Conversation

mehdi-benallegue commented Aug 12, 2022 • edited Loading

arntanguy left a comment

Choose a reason for hiding this comment

arntanguy Sep 2, 2022

Choose a reason for hiding this comment

arntanguy Sep 2, 2022

Choose a reason for hiding this comment

arntanguy Sep 2, 2022

Choose a reason for hiding this comment

arntanguy Sep 2, 2022

Choose a reason for hiding this comment

arntanguy Sep 2, 2022

Choose a reason for hiding this comment

arntanguy Sep 2, 2022

Choose a reason for hiding this comment

mehdi-benallegue commented Sep 9, 2022 • edited Loading

arntanguy commented Jan 25, 2023

mehdi-benallegue commented Jan 26, 2023 • edited Loading

mehdi-benallegue commented Aug 12, 2022 •

edited

Loading

mehdi-benallegue commented Sep 9, 2022 •

edited

Loading

mehdi-benallegue commented Jan 26, 2023 •

edited

Loading