[Estimation] Implement AttitudeEstimator class as a part of iDynTree

[prashanthr05]

Prashanth said:

In order to have a benchmark of the non-linear estimation algorithms we might develop, it will be helpful to have a working implementation of a few well-known estimators from the literature in order to compare the performance. This task will aim at developing a few orientation estimators from the literature. The first implementations will include

• [x] Mahony filter
• [x] Quaternion EKF

Silvio's pointers on the interface

On the top of the SWIG bindings already provided by the iDynTree infrastructure, it may be worth investigating if we could wrap some of this estimation classes using the new support in WB-Toolbox for creating custom toolboxes: https://robotology.github.io/wb-toolbox/mkdocs/create_new_library/ . As for the SWIG interfaces, it would make sense to keep this in the iDynTree repo, and that could be a good reason to split the WB-Toolbox base BlockBuilder API in a separated repo (as @diegoferigo already suggested in the past), to avoid circular dependency.

Diego's comments,

If:

• iDynTree is gaining Simulink support through a library
• The WB-Toolbox get split probably we should discuss what will be the destiny of the remaining components of WB-Toolbox itself. Under these circumstances, I would rather move all the iDynTree blocks to the new library, leaving only the Yarp-related stuff (not much) and a bunch of utilities.

Silvio replied

Under these circumstances, I would rather move all the iDynTree blocks to the new library, leaving only the Yarp-related stuff (not much) and a bunch of utilities.

I agree, but that would have a big impact on existing Simulink projects and existing WB-Toolbox users, so I think the two steps (separation of WB-Toolbox core, moving WB-Toolbox blocks in iDynTree) can be done in separate stages.

Prashanth said

I have prepared a skeleton of what this class might look like. It is still missing a few important methods and members,. However, the current skeleton conveys the idea of the block that we intend to prepare.

This will be accompanied by a header file AttitudeEstimatorParams.h containing the structs related to all the filter parameters. For instance,

struct MahonyParams
{
double kp;
double ki;
};

https://github.com/prashanthr05/idyntree/blob/feature/AttitudeEstimator/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h Please let me know if you have some ideas or modifications you might want to see.

SIlvio commented

Some comments:

• Please remove #include <Eigen/Core> from public headers, thanks!
• An alternative design you may consider is to have one C++ interface (i.e. virtual class) IAttitudeEstimator, and then three concrete classes implementing the actual filters. This is the so-called classical polymorphism (https://www.tutorialspoint.com/cplusplus/cpp_polymorphism.htm). See the filters implementation in iCub::ctrl to have a good example of this design pattern: https://github.com/robotology/icub-main/blob/c3ad642ed0aacdb3216f692cc73916936079714b/src/libraries/ctrlLib/include/iCub/ctrl/filters.h#L42 and the rest of the file. In general virtual functions involve a runtime performance cost over normal functions (see https://codeyarns.com/2017/06/13/a-dissection-of-c-virtual-functions/ and https://eli.thegreenplace.net/2013/12/05/the-cost-of-dynamic-virtual-calls-vs-static-crtp-dispatch-in-c) but in this case my intuition is that it should not be a big problem (but it would be better to check with a small test. See the code in the WB-Toolbox to see how such a design may be used in practice: https://github.com/robotology/wb-toolbox/blob/ce58905b3aa5eb2b6f19da800f52fff9202290a6/toolbox/library/src/DiscreteFilter.cpp#L227 .
• Again, based on the IFilter design, I would avoid to have any configuration specified during the constructor, for several reasons: 1) A constructor cannot fail, unless with an exception, 2) Calling again a constructor on an axisting object is tricky/complicated. I think it is easier to have a configure/init call in which this options can be specified.

This will be accompanied by a header file AttitudeEstimatorParams.h containing the structs related to all the filter parameters.

Why a separate header? If you go to have a separate class for each orientation, a clean solution is to have a structure nested inside the class for handling the class options. In the past I avoided them because they gave problems with SWIG, but with SWIG 3.0 we should not have this problems anymore: http://www.swig.org/Doc3.0/SWIGPlus.html#SWIGPlus_nested_classes .

Additional comment: I think it would be good to have a method to get/set any kind of internal state of the filter, even in an opaque way. Rationale for this: I would like eventually to support generating FMUs from WB-Toolbox blocks, and supporting setting and getting the complete state of the system (https://fmi-standard.org/docs/2.0.1-develop/#GetSetCompleteFMUState) is what you need for rewindable simulations (one of the coolest/most convenient possible features for simulators).

[prashanthr05]

An alternative design you may consider is to have one C++ interface (i.e. virtual class) IAttitudeEstimator, and then three concrete classes implementing the actual filters.

My instincts were leaning towards this virtual class design. However, I had this idea of composition over inheritance, as well. But then, since we will be defining only a virtual class, it shouldnt come with the usual bottlenecks of inheritance. I will redesign the AttitudeEstimator class to implement the virtual class interface, so that the generic methods are easily extensible to different types of filter.

In general virtual functions involve a runtime performance cost over normal functions

True. In the current design I proposed, I already had started thinking about the time constraints imposed by too many nested function calls. I think it would rather be efficient to have virtual functions and the derived classes implementing their own functionalities within these.

I think it is easier to have a configure/init call in which this options can be specified.

If you go to have a separate class for each orientation, a clean solution is to have a structure nested inside the class for handling the class options.

I think it would be good to have a method to get/set any kind of internal state of the filter, even in an opaque way.

I agree.

[prashanthr05]

A first version implementation of IAttitudeEstimator interface implementing AttitudeMahonyFIlter can be found here. It's missing a clear documentation. It's yet to be tested. However, I will be testing it soon. I will update docs after running first tests on the filter.

[prashanthr05]

A tested implementation of AttitudeMahonyFilter (explicit complementary filter on quaternions) and AttitudeQuaternionEKF is available in this branch. I shall make a PR once I cleanly document the code and the usage. Meanwhile if someone wants to test the filters, QuaternionEKF can be as easily implemented with the steps described in this test. However, the propagateStates() and updateFilterWithMeasurements() method should be run in a loop , as opposed to the implementation in the test. Attention should be paid to the gains and initial state in the QEKF for getting desirable performance. These parameters are available in the AttitudeQuaternionEKFParameters.

The Mahony Filter follows similar steps, except the fact that the update step is run first and the propagate states after. The gains are critical in getting a good performance here as well. However, the filter exhibits an asymptoticallly stable behavior, so it converges to the actual state whatever the inital state is (i.e. if the gains are good). The parameters are available in AttitudeMahonyFilterParameters.

[prashanthr05]

These classes were made available through the PR (https://github.com/robotology/idyntree/pull/516) merged into devel with this commit.

Closing this issue.