Bug fix in icubWholeBodySensor::close

the method returned false because it tried to close twice the same
PolyDriver, this cause the wholeBodyInterface not to close correctly
(i.e. the model interface did not close its ports).
Print err msg if wholeBodyInterface doesn't close fine
First implementation of recursiveLinearEstimator

To be tested.