[AttitudeEstimator] [Span] adding fixes, features, bindings and tests #522

prashanthr05 · 2019-02-13T13:55:52Z

This PR helps to avoid the NaN values generated during normalizing the vectors for AttitudeEstimators.
This PR adds fixes to the AttitudeEstimator classes and refactors minor parts of code in order to be compatible with SWIG bindings
This PR modifies Span header file to make it compatible with SWIG bindings (called as DynamicSpan)
This PR adds Matlab tests for AttitudeEstimator

traversaro · 2019-02-15T09:02:18Z

I am a bit confused by these changes:

In composeQuaternion and composeQuaternion2, the input quaternion are supposed to be unit quaternions, right? If they are not, it seems to me that the check should be on the input data, not to silently return a zero non-unit quaternion. However, as the function are not part of the public API, it should be sufficient to make sure that we are not passing them to the function inside iDynTree
All the functions that are normalizing the measurements of accelerometers and magnetometers measurements, if the accelerometer or magnetometer are zero, instead of returning/signaling in some way an error, they just leave it as zero. But I guess that the algorithm still assume that they are unit vector/direction vectors, so more problem can arise, right?

In general, I am not sure what is the problem that caused this change. In some condition you are providing invalid data to the algorithm?

prashanthr05 · 2019-02-15T09:14:51Z

In general, I am not sure what is the problem that caused this change. In some condition you are providing invalid data to the algorithm?

The problem actually arose with the initial angular velocity state estimate being set to all zeros.
In which case, the norm is zero and when we try to normalize, the results were NaN. NaNs started to get propagated throughout the runtime of the algorithm.

And true, it doesnt make sense to normalize the quaternion in the composeQuaternion() methods.
Since, if angular velocity is zero, then there is no change acting on the actual quaternion.
Also, composeQuaternion() methods should simply act as quaternionic multiplication.

All the functions that are normalizing the measurements of accelerometers and magnetometers measurements, if the accelerometer or magnetometer are zero, instead of returning/signaling in some way an error, they just leave it as zero. But I guess that the algorithm still assume that they are unit vector/direction vectors, so more problem can arise, right?

That's true. We need to add a check, if they are unit vectors or not, and act accordingly.

My bad, I was not thinking straight. I shall make the changes.

traversaro · 2019-02-15T09:56:23Z

Wipping for now.

prashanthr05 · 2019-02-15T12:45:10Z

cc @GiulioRomualdi @traversaro added checks on measurements and unit quaternion.
Please review the changes.

prashanthr05 · 2019-02-15T12:46:36Z

src/estimation/src/AttitudeMahonyFilter.cpp

@@ -95,7 +123,7 @@ bool iDynTree::AttitudeMahonyFilter::propagateStates()

    // system dynamics equations
    q = q + (dq*(m_params.time_step_in_seconds*0.5));
-    if (q.norm() != 0)
+    if (q.norm() != 0 || q.norm() != 1)


Realized that this chould be && condition to be a stronger check. At the moment, still could pass as a weaker check! Would you like me to change this to && ?

I am confused. How can the norm be both 0.0 and 1.0 ?

Normalize only if the norm is not zero (to avoid nan) and the norm is not one(to become unit quaternion if already not)

Does it make sense ? Or should we actually split it into two conditions ?

Why the quaternion should be ever 0.0?

Because we only typedef UnitQuaternion and not type check. We don't establish anywhere that the vector will have unit norm or not have zero norm at all.
Worst case check!

If the quaternion norm is zero, then an error should be emitted at some point, and the propagateStates method shoul return false, right?

True. Makes sense. I will change the condition.

@traversaro fixed this condition and improved the test.

prashanthr05 · 2019-02-15T12:48:22Z

src/estimation/src/AttitudeQuaternionEKF.cpp

    {
-        // to avoid nan
-        toEigen(m_Acc_y).normalize();
+        iDynTree::reportError("AttitudeMahonyFilter", "updateFilterWithMeasurements", "Cannot retrieve unit vector from linear acceleration measuremnts.");


AttitudeQuaternionEKF

prashanthr05 · 2019-02-15T12:48:34Z

src/estimation/src/AttitudeQuaternionEKF.cpp

+    iDynTree::Vector3 magMeasUnitVector;
+    if (!getUnitVector(magMeas, magMeasUnitVector))
+    {
+        iDynTree::reportError("AttitudeMahonyFilter", "updateFilterWithMeasurements", "Cannot retrieve unit vector from magnetometer measuremnts.");


prashanthr05 · 2019-02-15T12:49:05Z

src/estimation/src/AttitudeQuaternionEKF.cpp

@@ -434,6 +449,10 @@ bool iDynTree::AttitudeQuaternionEKF::ekf_f(const iDynTree::VectorDynSize& x_k,
    auto dq(toEigen(composeQuaternion2(orientation, correction)));

    x_hat_plus.block<4,1>(0, 0) = q + (dq*(m_params.time_step_in_seconds*0.5));
+    if (q.norm() != 0 || q.norm() != 1)


same as above

traversaro · 2019-02-20T14:37:42Z

Is this WIP or it is ready for review?

prashanthr05 · 2019-02-20T15:09:19Z

Yes, by itself, it should be ready.

However, I am running some tests with actual data to see if the implementation checks. Also, if it is ok to add generated matlab bindings along with this PR, then I think it can still be wip-ped.

traversaro · 2019-02-28T14:41:30Z

You have some specific settings to compile this? On my Ubuntu 18.04 machine this branch is failing with error:

Scanning dependencies of target SparseMatrixUnitTest
[ 20%] Building CXX object src/core/tests/CMakeFiles/SparseMatrixUnitTest.dir/SparseMatrixUnitTest.cpp.o
[ 20%] Linking CXX executable ../../../bin/MatrixDynSizeUnitTest
In file included from /home/straversaro/src/robotology-superbuild/robotology/iDynTree/src/core/tests/SpanUnitTest.cpp:30:0:
/home/straversaro/src/robotology-superbuild/robotology/iDynTree/src/core/include/iDynTree/Core/Span.h: In instantiation of ‘class iDynTree::Span<int [3]>’:
/home/straversaro/src/robotology-superbuild/robotology/iDynTree/src/core/tests/SpanUnitTest.cpp:341:22:   required from here
/home/straversaro/src/robotology-superbuild/robotology/iDynTree/src/core/include/iDynTree/Core/Span.h:498:37: error: function returning an array
     IDYNTREE_CONSTEXPR element_type getVal(index_type idx) const { return this->operator[](idx);}
                                     ^~~~~~
/home/straversaro/src/robotology-superbuild/robotology/iDynTree/src/core/include/iDynTree/Core/Span.h: In instantiation of ‘class iDynTree::Span<int [3][2]>’:
/home/straversaro/src/robotology-superbuild/robotology/iDynTree/src/core/tests/SpanUnitTest.cpp:370:25:   required from here
/home/straversaro/src/robotology-superbuild/robotology/iDynTree/src/core/include/iDynTree/Core/Span.h:498:37: error: function returning an array
src/core/tests/CMakeFiles/SpanUnitTest.dir/build.make:62: recipe for target 'src/core/tests/CMakeFiles/SpanUnitTest.dir/SpanUnitTest.cpp.o' failed
make[2]: *** [src/core/tests/CMakeFiles/SpanUnitTest.dir/SpanUnitTest.cpp.o] Error 1
CMakeFiles/Makefile2:1655: recipe for target 'src/core/tests/CMakeFiles/SpanUnitTest.dir/all' failed
make[1]: *** [src/core/tests/CMakeFiles/SpanUnitTest.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[ 20%] Built target MatrixDynSizeUnitTest
[ 20%] Linking CXX executable ../../../bin/SparseMatrixUnitTest
[ 20%] Built target SparseMatrixUnitTest
[ 20%] Linking CXX executable ../../../bin/EigenHelpersUnitTest
[ 20%] Built target EigenHelpersUnitTest
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2

prashanthr05 · 2019-03-22T08:32:15Z

@traversaro I think this PR is good to go, except that the Travis is complaining for some iDynTree Lua bindings. Please take a look and let me know.

traversaro · 2019-03-22T08:56:52Z

The failures are just on Xenial, as we are phasing out Xenial support, I think there is no problem. Can you disable bindings testing on the Xenial jobs? Thanks!

prashanthr05 · 2019-03-22T09:21:27Z

Can you disable bindings testing on the Xenial jobs?

Could you please tell me how to do that?

prashanthr05 · 2019-03-25T14:43:41Z

friendly ping @traversaro

traversaro · 2019-03-25T14:47:50Z

Sorry, I guess the message was not sent. You need to change the COMPILE_BINDINGS option in the Xenial lines in https://github.com/robotology/idyntree/blob/master/.travis.yml#L23 .

prashanthr05 · 2019-03-25T15:47:43Z

Th travis still complains,

E: Failed to fetch http://archive.ubuntu.com/ubuntu/pool/main/b/boost1.65.1/libboost-serialization1.65.1_1.65.1+dfsg-0ubuntu5_amd64.deb  Undetermined Error [IP: 91.189.88.149 80]
E: Unable to fetch some archives, maybe run apt-get update or try with --fix-missing?

Server issues?

traversaro · 2019-03-25T16:02:47Z

There is a similar feailure in the devel branch, I think it is something independent from the PR.

traversaro · 2019-03-26T07:18:15Z

Last request: can you update the release notes https://github.com/robotology/idyntree/blob/devel/doc/releases/v0_12.md ?

traversaro · 2019-03-26T07:19:21Z

For the future: when you re-generate Matlab bindings, please commit them in a separate commit w.r.t. to the changes in the .i interface, as described in https://github.com/robotology/idyntree/blob/master/doc/dev/faqs.md#how-to-add-wrap-a-new-class-or-function-with-swig .

prashanthr05 · 2019-03-26T15:03:50Z

Last request: can you update the release notes https://github.com/robotology/idyntree/blob/devel/doc/releases/v0_12.md ?

Done. Thank you.

Excuse me for not adhering to the conventional way of committing bindings. I did not pay attention. I will take care of that in the future.

traversaro · 2019-03-26T15:44:15Z

Thanks!

prashanthr05 · 2019-03-26T16:04:20Z

Thanks to you!

[AttitudeEstimator] fix for nans while normalizing vector

77c0593

traversaro changed the title ~~[AttitudeEstimator] fix for nans while normalizing vector~~ [WI[AttitudeEstimator] fix for nans while normalizing vector Feb 15, 2019

traversaro changed the title ~~[WI[AttitudeEstimator] fix for nans while normalizing vector~~ [WIP] [AttitudeEstimator] fix for nans while normalizing vector Feb 15, 2019

[estimation] check valid measurements and unit norm of quaternion

53f31c5

prashanthr05 commented Feb 15, 2019

View reviewed changes

prashanthr05 added 2 commits February 15, 2019 19:46

[estimation][AttEst] fix state propagation check, fix test

580e1fa

[estimation]handle properly useMagnetometerMeasurements()

9ab309e

[estimation][core] add span/attEst bindings

3a12d14

prashanthr05 added 2 commits March 11, 2019 17:30

[estimation][core] adding span test

3c0fbae

[estimation] AttEst tested bindings and bugfix

4f993e2

prashanthr05 changed the title ~~[WIP] [AttitudeEstimator] fix for nans while normalizing vector~~ [AttitudeEstimator] fix for nans while normalizing vector Mar 25, 2019

prashanthr05 changed the title ~~[AttitudeEstimator] fix for nans while normalizing vector~~ [AttitudeEstimator] [Span] adding fixes, features, bindings and tests Mar 25, 2019

[bindings][travis] turn off xenial compilation

730d4aa

[doc] update release doc v0_12

107a95d

Merge branch 'devel' into fix/AttEst

c20c37f

traversaro approved these changes Mar 26, 2019

View reviewed changes

traversaro merged commit d5bac2a into robotology:devel Mar 26, 2019

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[AttitudeEstimator] [Span] adding fixes, features, bindings and tests #522

[AttitudeEstimator] [Span] adding fixes, features, bindings and tests #522

prashanthr05 commented Feb 13, 2019 •

edited

Loading

traversaro commented Feb 15, 2019

prashanthr05 commented Feb 15, 2019 •

edited

Loading

traversaro commented Feb 15, 2019

prashanthr05 commented Feb 15, 2019

prashanthr05 Feb 15, 2019 •

edited

Loading

traversaro Feb 15, 2019

prashanthr05 Feb 15, 2019

prashanthr05 Feb 15, 2019

traversaro Feb 15, 2019

prashanthr05 Feb 15, 2019

traversaro Feb 15, 2019

prashanthr05 Feb 15, 2019

prashanthr05 Feb 15, 2019

prashanthr05 Feb 15, 2019

prashanthr05 Feb 15, 2019

prashanthr05 Feb 15, 2019

traversaro commented Feb 20, 2019

prashanthr05 commented Feb 20, 2019

traversaro commented Feb 28, 2019

prashanthr05 commented Mar 22, 2019

traversaro commented Mar 22, 2019

prashanthr05 commented Mar 22, 2019

prashanthr05 commented Mar 25, 2019

traversaro commented Mar 25, 2019

prashanthr05 commented Mar 25, 2019

traversaro commented Mar 25, 2019

traversaro commented Mar 26, 2019

traversaro commented Mar 26, 2019

prashanthr05 commented Mar 26, 2019

traversaro commented Mar 26, 2019

prashanthr05 commented Mar 26, 2019

[AttitudeEstimator] [Span] adding fixes, features, bindings and tests #522

[AttitudeEstimator] [Span] adding fixes, features, bindings and tests #522

Conversation

prashanthr05 commented Feb 13, 2019 • edited Loading

traversaro commented Feb 15, 2019

prashanthr05 commented Feb 15, 2019 • edited Loading

traversaro commented Feb 15, 2019

prashanthr05 commented Feb 15, 2019

prashanthr05 Feb 15, 2019 • edited Loading

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

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

traversaro commented Feb 20, 2019

prashanthr05 commented Feb 20, 2019

traversaro commented Feb 28, 2019

prashanthr05 commented Mar 22, 2019

traversaro commented Mar 22, 2019

prashanthr05 commented Mar 22, 2019

prashanthr05 commented Mar 25, 2019

traversaro commented Mar 25, 2019

prashanthr05 commented Mar 25, 2019

traversaro commented Mar 25, 2019

traversaro commented Mar 26, 2019

traversaro commented Mar 26, 2019

prashanthr05 commented Mar 26, 2019

traversaro commented Mar 26, 2019

prashanthr05 commented Mar 26, 2019

prashanthr05 commented Feb 13, 2019 •

edited

Loading

prashanthr05 commented Feb 15, 2019 •

edited

Loading

prashanthr05 Feb 15, 2019 •

edited

Loading