Implement Invariant EKF Floating Base Estimator #85

prashanthr05 · 2020-08-20T21:13:43Z

This PR,

Implements InvariantEKFBaseEstimator (the Contact Aided Invariant EKF algorithm) by inheriting from the FloatingBaseEstimator class. Reference to the original paper is Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation. This implementation does not contain a full EKF SLAM algorithm using also visual factors. Instead, it's limited to estimating the foot positions of a bipedal robot only along with the base pose and IMU biases. For the original (and full) version, please see https://github.com/RossHartley/invariant-ekf.
adds an InvariantEKFBaseEstimatorTest that tests the estimator algorithm for a simple static configuration of iCubGenova04 robot.

Additionally, this PR also

adds https://github.com/artivis/manif as a dependency
modifies the library structure so as to introduce a clean separation between regular Estimators algorithms and FloatingBaseEstimators algorithm
modifies the FloatingBaseEstimator class slightly to add some required features with respect to the model computations.

Pending from the PR,

Documentation wherever required
implementation of resetEstimator() methods

prashanthr05 · 2020-08-20T21:20:04Z

Fails because CI was not updated to add manif as a dependency.

To avoid any merge conflicts, maybe we can wait for this to be merged.
#82

prashanthr05 · 2020-08-25T22:17:38Z

To avoid any merge conflicts, maybe we can wait for this to be merged.

I added manif to CI in a way that merge conflicts will be avoided.

prashanthr05 · 2020-08-25T22:25:09Z

cc @S-Dafarra @GiulioRomualdi, this PR is ready to be reviewed.
I apologize beforehand for a lengthy PR.

prashanthr05 · 2020-08-25T22:33:09Z

CI is failing due to YCM version. See robotology/whole-body-estimators#83.

prashanthr05 · 2020-08-28T13:12:40Z

Comparison plots for a walking experiment from a Gazebo simulated dataset between a Matlab implementation, bipedal locomotion framework implementation and original implementation against the ground truth is below (Please note, This is not the same experiment as the one in the test),

S-Dafarra · 2020-09-03T11:23:27Z

Can you please try to rebase? In this way, the modification relative to the adding of manif should disappear.

prashanthr05 · 2020-09-03T15:51:52Z

Can you please try to rebase? In this way, the modification relative to the adding of manif should disappear.

Done!!

Please note that, this PR also contains a few changes in the parent class FloatingBaseEstimator that were projected upon due to the requirements in InvariantEKFBaseEstimator. I also reorganized the headers to be separated from Estimators folder into a FloatingBaseEstimators folder.

prashanthr05 · 2020-09-03T16:08:30Z

The macOS CI is failing in the InvariantEKF Test, looks like the estimation results are bad (surprising). This needs to be investigated on a macOS machine.

Instead of the windows, CI is failing due to linking the problem of YarpUtilitiesTest with yarp sig Vector in the Build step.

GiulioRomualdi · 2020-09-03T16:24:07Z

On Windows, it's actually failing because of

2020-09-03T15:58:08.8415226Z D:\a\bipedal-locomotion-framework\bipedal-locomotion-framework\src\Estimators\tests\InvariantEKFBaseEstimatorTest.cpp(29,17): error C2065: 'M_PI': undeclared identifier

Here a related issue: robotology/community#81

This patch should solve your problem

diff --git a/cmake/AddBipedalLocomotionUnitTest.cmake b/cmake/AddBipedalLocomotionUnitTest.cmake
index 82430fd..4449f25 100644
--- a/cmake/AddBipedalLocomotionUnitTest.cmake
+++ b/cmake/AddBipedalLocomotionUnitTest.cmake
@@ -69,6 +69,7 @@ function(add_bipedal_test)
       target_link_libraries(${targetname} PRIVATE CatchTestMain ${${prefix}_LINKS})
       target_compile_definitions(${targetname} PRIVATE CATCH_CONFIG_FAST_COMPILE CATCH_CONFIG_DISABLE_MATCHERS)
       target_compile_features(${targetname} PUBLIC cxx_std_17)
+      target_compile_definitions(${targetname} PRIVATE -D_USE_MATH_DEFINES)
 
       add_test(NAME ${targetname} COMMAND ${targetname})

On my laptop, the test (in debug) is still failing. Normalizing the simImuQuat solves the problem.

diff --git a/src/Estimators/tests/InvariantEKFBaseEstimatorTest.cpp b/src/Estimators/tests/InvariantEKFBaseEstimatorTest.cpp
index 9b51ff0..c86684d 100644
--- a/src/Estimators/tests/InvariantEKFBaseEstimatorTest.cpp
+++ b/src/Estimators/tests/InvariantEKFBaseEstimatorTest.cpp
@@ -135,6 +135,7 @@ TEST_CASE("Invariant EKF Base Estimator")
     // ground truth
     Eigen::Vector3d simIMUPos;
     Eigen::Quaterniond simImuQuat = Eigen::Quaterniond(0.3218, -0.6304, -0.6292, 0.3212);
+    simImuQuat.normalize();
     simIMUPos << 0.0296, -0.1439,  0.4915;
 
     // IMU measures
@@ -222,4 +223,3 @@ TEST_CASE("Invariant EKF Base Estimator")
 
     REQUIRE( std::abs(newOut.stateStdDev.imuPosition(0) - resetStateStdDev.imuPosition(0)) < 2e-2);
 }
-

prashanthr05 · 2020-09-03T16:34:23Z

On my laptop, the test (in debug) is still failing. Normalizing the simImuQuat solves the problem.

Yes, yet again I had failed to run the tests in Debug. I will keep this in mind. I will make relevant changes. Let's see if that fixes the macOS tests as well.

This patch should solve your problem

I shall open a separate PR for that.

S-Dafarra

Some initial comments

README.md

src/Estimators/CMakeLists.txt

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

prashanthr05 · 2020-09-03T17:25:53Z

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

+                                     iDynTree::Transform& basePose,
+                                     iDynTree::Twist& baseTwist);


This should be changed to manif and Eigen objects.

I opened an issue for this. #107.

This will be dealt with later in a new PR.

prashanthr05 · 2020-09-03T19:00:43Z

Ciao @kouroshD !! May I ask you a favor of testing this PR by running the InvariantEKFBaseEstimatorTest after compiling on Debug build on your computer?

You might have to install the dependency "manif" on your computer for testing this PR.

prashanthr05 · 2020-09-04T08:29:59Z

Ciao @kouroshD !! May I ask you a favor of testing this PR by running the InvariantEKFBaseEstimatorTest after compiling on Debug build on your computer?

You might have to install the dependency "manif" on your computer for testing this PR.

Ciao @kouroshD .. do not worry about this. We managed to procure a Mac workstation in the lab. I shall do the necessary testing myself :)

… estimator

…tests

prashanthr05 · 2020-09-09T09:16:55Z

Strangely, running the InvariantEKFBaseEstimatorTest on macOS Catalina by compiling the repository in Debug, the test passes successfully. But with Release, it fails.

I am installing valgrind according to https://stackoverflow.com/questions/58360093/how-to-install-valgrind-on-macos-catalina-10-15-with-homebrew by tapping into LouisBrunner/valgrind, since I was not able to directly install it from brew, which threw an error that it is not supported or function as expected in Catalina.

prashanthr05 · 2020-09-09T10:08:11Z

Looks like using auto on a manif to Eigen conversion was causing uninitialized value condition. This was caught by valgrind on macOS Catalina local workstation.

I changed this in 3020229 and the tests seem to pass both on Debug and Release on the local workstation.

S-Dafarra · 2020-09-09T10:17:06Z

Looks like using auto on a manif to Eigen conversion was causing uninitialized value condition. This was caught by valgrind on macOS Catalina local workstation.

I changed this in 3020229 and the tests seem to pass both on Debug and Release on the local workstation.

Take home message, never use auto with Eigen types https://eigen.tuxfamily.org/dox/TopicPitfalls.html 😁

prashanthr05 · 2020-09-09T11:18:14Z

Looks like using auto on a manif to Eigen conversion was causing uninitialized value condition. This was caught by valgrind on macOS Catalina local workstation.
I changed this in 3020229 and the tests seem to pass both on Debug and Release on the local workstation.

Take home message, never use auto with Eigen types https://eigen.tuxfamily.org/dox/TopicPitfalls.html

Changed auto on Eigen types to relevant types in b5deac5

GiulioRomualdi · 2020-09-14T14:41:20Z

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

+    virtual bool resetEstimator(const Eigen::Quaterniond& newBaseOrientation,
+                                const Eigen::Vector3d& newBasePosition) final;


here you can use:

Suggested change

virtual bool resetEstimator(const Eigen::Quaterniond& newBaseOrientation,

const Eigen::Vector3d& newBasePosition) final;

virtual bool resetEstimator(Eigen::Ref<const Eigen::Quaterniond> newBaseOrientation,

Eigen::Ref<const Eigen::Vector3d> newBasePosition) final;

Eigen::Ref doesn't seem to work on Quaternions.

Following compilation error is thrown,

error: ‘IsVectorAtCompileTime’ is not a member of ‘const Eigen::Quaternion<double>’

src/Estimators/src/FloatingBaseEstimator.cpp

GiulioRomualdi · 2020-09-14T14:46:18Z

src/Estimators/src/InvariantEKFBaseEstimator.cpp

+    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dims, dims);
+
+    Eigen::MatrixXd Fk = I + (Fc*dt);
+    Eigen::MatrixXd Qk = (Fk*Lc*Qc*(Lc.transpose())*(Fk.transpose()))*dt;


Do you think we may avoid continuous memory allocation if I ,Fk and Qk are class members?

Actually, here the catch is that these matrices do not have a fixed dimension throughout the run time. The dimensions vary if bias estimation is toggled during run time.

In this case, storing the matrices as class members means that we need to modify the mechanics of current implementation in a way that these matrix dimensions are updated at every step. It is not difficult to implement, however, in any case, we need to be reallocating the memory during the resize. That's why I chose this way instead.

Correct me if my rationale is not good enough.

I think that some resizing from time to time are better than allocating memory all the time. Can you provide some more insights on why a continuous resize may be necessary?

Continuous resize won't be necessary. Just at events of bias estimation toggle. I can make the relevant changes.

Here, we do not even consider arbitrary number of contacts, we consider only 2. (so we don't need to continuously resize the matrices.)

Done in 6abd678

src/Estimators/src/InvariantEKFBaseEstimator.cpp

GiulioRomualdi · 2020-09-14T14:52:43Z

src/Estimators/src/InvariantEKFBaseEstimator.cpp

+    Eigen::MatrixXd PHT = P*measModelJacobian.transpose();
+    Eigen::MatrixXd S = measModelJacobian*PHT + measNoiseVar;
+    Eigen::MatrixXd K = PHT*(S.inverse());


As before, probably we may avoid having this continuous memory allocation.

I'd prefer to keep this part as it is since this also depends on the number of feet in contact with the ground which seems to be changing "almost continuously". This means anyways we might have to do an almost continuous resizing of these matrices. Instead for the above comment, I made the matrices Fc, Qc and Lc as class members to avoid continuous memory allocation, since their dimensions would only change at very rare events of toggling the bias estimation.

What do you think @GiulioRomualdi?

To be honest, I am with @GiulioRomualdi on this. It is always better to avoid memory allocations, especially in pieces of code that are called at every control iteration. In principle, these should have no memory allocation at all.
It is acceptable to have some resize from time to time, also considering that the container may have some capacity (hence memory may not be allocated also in case of resizes).
If the number of members blows up, then we can discuss alternative solutions.

Also, I have noticed that in the code below there are several temporary matrices (X, BigX, innovation, delta, dX, IminusKH). Also this should be avoided as much as possible.

I've partially done this in 6abd678. Let me know if this is in line with what is suggested. However, about the resizing of the matrices related to the measurement model/updates, it would require a bit more time to do it in a cleaner way.

I also completely agree with what is suggested. In fact, I had an earlier design in the element repo which considered these things. But not sure why I changed my mind to do things in a different way here.

Eigen automatically resizes the matrix when using the assignment operator. So you don't need to deal with the explicit resize for a first iteration I think. Then, later on, we can understand if it should be better to resize these matrices beforehand in a specific place.

Fixed in f456476

GiulioRomualdi · 2020-09-14T14:53:37Z

src/Estimators/src/InvariantEKFBaseEstimator.cpp

+    // X =    |R  v  p pr pl|
+    //        |   1         |
+    //        |      1      |
+    //        |         1   |
+    //        |            1|
+    X.resize(7, 7);
+    X.block<3, 3>(0, 0) = state.imuOrientation.toRotationMatrix();
+    X.block<3, 1>(0, 3) = state.imuLinearVelocity;
+    X.block<3, 1>(0, 4) = state.imuPosition;
+    X.block<3, 1>(0, 5) = state.rContactFramePosition;
+    X.block<3, 1>(0, 6) = state.lContactFramePosition;
+    X.bottomLeftCorner<4, 3>().setZero();
+    X.bottomRightCorner<4, 4>() = Eigen::Matrix4d::Identity();


Probably it makes sense to store X as sparse matrix

Given it's only a 7x7 matrix, do you think we can achieve higher gain margins using a sparse matrix?

In our use cases, sparse matrices are useful only if need in some calculations involving inversions (e.g solving linear systems or when interfacing to solvers). Another possible application is to reduce the occupation in memory, but this is not an issue for basically all our applications. Hence, if we are not in the first case, I would avoid using sparse matrices.

The most we do with this matrix is matrix multiplication. For inversions, we exploit with Lie group properties of direct product/semi-direct-product to only involve submatrix operations.

Ok, I would vote for keeping it dense then.

S-Dafarra

The PR is very complex for me to delve into every detail, it is above my head 😁 But on the software side, I did not spot anything worth changing. In addition, the implementation can change easily later if needed. Hence, it looks good to me

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

traversaro

I did not checked everithing in detail, but overall it seems fine!

prashanthr05 · 2020-10-01T20:49:16Z

cc @GiulioRomualdi do you think we can merge this?

prashanthr05 · 2020-10-02T07:08:17Z

@GiulioRomualdi I've squashed the commits. You could merge it once the CI passes, thank you! :)

GiulioRomualdi · 2020-10-02T08:02:00Z

Merging

prashanthr05 requested review from GiulioRomualdi, HosameldinMohamed, S-Dafarra and traversaro August 28, 2020 09:21

GiulioRomualdi assigned prashanthr05 Sep 3, 2020

GiulioRomualdi added this to the Iteration 50 - 50!! milestone Sep 3, 2020

GiulioRomualdi added the enhancement New feature or request label Sep 3, 2020

prashanthr05 force-pushed the feature/estimators branch from 5239eb9 to d10adf2 Compare September 3, 2020 15:48

GiulioRomualdi mentioned this pull request Sep 3, 2020

Enable the compilation in Debug for the CI #91

Closed

This was referenced Sep 3, 2020

Add the Debug compilation in GitHub Action #92

Merged

Add USE_MATH_DEFINES for tests and library compile definitions #93

Merged

S-Dafarra reviewed Sep 3, 2020

View reviewed changes

README.md Outdated Show resolved Hide resolved

src/Estimators/CMakeLists.txt Outdated Show resolved Hide resolved

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimator.h Show resolved Hide resolved

prashanthr05 commented Sep 3, 2020

View reviewed changes

prashanthr05 force-pushed the feature/estimators branch from d10adf2 to 280f2a6 Compare September 3, 2020 18:22

prashanthr05 added 4 commits September 9, 2020 10:28

[FloatingBaseEstimators] add invariant EKF base estimator

93ab1e0

[FloatingBaseEstimators] Add invariant EKF base estimator test

45fc741

[FloatingBaseEstimators] update docs and reset methods in invEKF base…

e26b4b0

… estimator

[FloatingBaseEstimators] add buffer initializations - fixes valgrind …

aaf1be7

…tests

prashanthr05 added 2 commits September 9, 2020 10:28

[FloatingBaseEstimators] update InvariantEKFBaseEstimatorTest

cac4137

[FloatingBaseEstimators] InvEKF normalize initial state quaternions

2694a0b

prashanthr05 force-pushed the feature/estimators branch from 280f2a6 to 2694a0b Compare September 9, 2020 08:29

prashanthr05 added 3 commits September 9, 2020 10:37

Update Estimators CMakeLists to handle test compilation properly

2e3124f

[Estimators] fix tests CMakeLists

0de5b76

[Estimators] fix tests folder organization and cmake

5b2623a

[Estimators] change auto to EigenType for proper initialization

3020229

S-Dafarra modified the milestones: Iteration 50 - 50!!, Iteration 51 - Back to school Sep 10, 2020

GiulioRomualdi reviewed Sep 14, 2020

View reviewed changes

prashanthr05 force-pushed the feature/estimators branch from d311aa2 to f456476 Compare September 21, 2020 13:49

S-Dafarra approved these changes Sep 25, 2020

View reviewed changes

traversaro reviewed Sep 26, 2020

View reviewed changes

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimator.h Outdated Show resolved Hide resolved

traversaro approved these changes Sep 26, 2020

View reviewed changes

prashanthr05 force-pushed the feature/estimators branch from 0756894 to c5b51a1 Compare September 28, 2020 11:10

GiulioRomualdi approved these changes Oct 1, 2020

View reviewed changes

[Estimators] avoid using auto for Eigen types

f7a3f93

prashanthr05 force-pushed the feature/estimators branch from c5b51a1 to f7a3f93 Compare October 2, 2020 07:07

GiulioRomualdi merged commit 5a1b3d9 into ami-iit:master Oct 2, 2020

prashanthr05 deleted the feature/estimators branch October 2, 2020 08:02

		virtual bool resetEstimator(const Eigen::Quaterniond& newBaseOrientation,
		const Eigen::Vector3d& newBasePosition) final;

Implement Invariant EKF Floating Base Estimator #85

Implement Invariant EKF Floating Base Estimator #85

Conversation

prashanthr05 commented Aug 20, 2020 • edited

prashanthr05 commented Aug 20, 2020

prashanthr05 commented Aug 25, 2020

prashanthr05 commented Aug 25, 2020

prashanthr05 commented Aug 25, 2020

prashanthr05 commented Aug 28, 2020 • edited

S-Dafarra commented Sep 3, 2020

prashanthr05 commented Sep 3, 2020

prashanthr05 commented Sep 3, 2020

GiulioRomualdi commented Sep 3, 2020 • edited

prashanthr05 commented Sep 3, 2020

S-Dafarra left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

prashanthr05 Sep 16, 2020 • edited

Choose a reason for hiding this comment

prashanthr05 commented Sep 3, 2020 • edited

prashanthr05 commented Sep 4, 2020

prashanthr05 commented Sep 9, 2020 • edited

prashanthr05 commented Sep 9, 2020 • edited

S-Dafarra commented Sep 9, 2020

prashanthr05 commented Sep 9, 2020

Choose a reason for hiding this comment

prashanthr05 Sep 16, 2020 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

prashanthr05 Sep 16, 2020 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

prashanthr05 Sep 21, 2020 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

S-Dafarra left a comment

Choose a reason for hiding this comment

traversaro left a comment

Choose a reason for hiding this comment

prashanthr05 commented Oct 1, 2020

prashanthr05 commented Oct 2, 2020

GiulioRomualdi commented Oct 2, 2020

prashanthr05 commented Aug 20, 2020 •

edited

prashanthr05 commented Aug 28, 2020 •

edited

GiulioRomualdi commented Sep 3, 2020 •

edited

prashanthr05 Sep 16, 2020 •

edited

prashanthr05 commented Sep 3, 2020 •

edited

prashanthr05 commented Sep 9, 2020 •

edited

prashanthr05 commented Sep 9, 2020 •

edited

prashanthr05 Sep 16, 2020 •

edited

prashanthr05 Sep 16, 2020 •

edited

prashanthr05 Sep 21, 2020 •

edited