From bf7d5ff892c38ed2346857477b603a3dd386e36e Mon Sep 17 00:00:00 2001
From: InesSorrentino <43743081+isorrentino@users.noreply.github.com>
Date: Wed, 23 Feb 2022 17:26:09 +0100
Subject: [PATCH] Add kalman filter joint velocity and acceleration estimation
 (#139)

* Add kalman filter joint velocity and acceleration estimation
Modify joint torque and external wrench estimation setting the correct joint velocity and acceleration when the KF estimation is used
Modify configuration files adding paramenters required for the KF inititalization

* Remove unused variables and includes

* Use only joint position for kf measurement

* Modify README

* Change default value for estimateJointVelocityAcceleration parameter

* Use unique_ptr for kalman filter object

* Modify documentation

* Update devices/wholeBodyDynamics/README.md

Co-authored-by: Prashanth Ramadoss <prashanthramadoss@gmail.com>

* Update CHANGELOG

* Fix covariances in configuration files
Update documentation

Co-authored-by: Prashanth Ramadoss <prashanthramadoss@gmail.com>
---
 CHANGELOG.md                                  |   2 +
 devices/wholeBodyDynamics/CMakeLists.txt      |   1 -
 devices/wholeBodyDynamics/README.md           |   6 +-
 .../WholeBodyDynamicsDevice.cpp               | 251 +++++++++++++++++-
 .../WholeBodyDynamicsDevice.h                 |  20 +-
 .../wholebodydynamics-icub-eth-six-fts.xml    |  50 +++-
 ...ynamics-icub-external-and-can-four-fts.xml |  52 +++-
 ...dynamics-icub-external-and-can-six-fts.xml |  52 +++-
 ...bodydynamics-icub-external-six-fts-sim.xml |  50 +++-
 .../wholebodydynamics-icub3-external-sim.xml  |  50 +++-
 ...bodydynamics-icubheidelberg01-external.xml |  28 +-
 ...olebodydynamics-icubheidelberg01-robot.xml |  28 +-
 .../wholeBodyDynamicsSettings.thrift          |   1 +
 13 files changed, 569 insertions(+), 22 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index e646ae4..21b0b06 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -5,6 +5,8 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
 and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
 
 ## Unreleased
+### Added
+- Add a steady-state Kalman filter with a null jerk model for the joint velocity and acceleration estimation in `WholeBodyDynamics`. (See [!139](https://github.com/robotology/whole-body-estimators/pull/139)).
 
 ### Changed
 - Updated the `yarp rpc` command `calibStandingWithJetsiRonCubMk1` to be `calibStandingWithJetsiRonCub`, in order to perform `calibStanding` for the models `iRonCub-Mk1` and `iRonCub-Mk1_1` (See [!136](https://github.com/robotology/whole-body-estimators/pull/136)).
diff --git a/devices/wholeBodyDynamics/CMakeLists.txt b/devices/wholeBodyDynamics/CMakeLists.txt
index 4087af1..b075062 100644
--- a/devices/wholeBodyDynamics/CMakeLists.txt
+++ b/devices/wholeBodyDynamics/CMakeLists.txt
@@ -7,7 +7,6 @@ find_package(YARP REQUIRED)
 find_package(ICUB REQUIRED)
 find_package(iDynTree REQUIRED)
 
-
 yarp_prepare_plugin(wholebodydynamics CATEGORY device
                                       TYPE yarp::dev::WholeBodyDynamicsDevice
                                       INCLUDE "WholeBodyDynamicsDevice.h"
diff --git a/devices/wholeBodyDynamics/README.md b/devices/wholeBodyDynamics/README.md
index a158627..5f760d0 100644
--- a/devices/wholeBodyDynamics/README.md
+++ b/devices/wholeBodyDynamics/README.md
@@ -36,6 +36,10 @@ For an overview on `wholeBodyDynamics` and to understand how to run the device,
 | streamFilteredFT     |        - | bool              |  -    |      false    |  No      | Select if the filtered and offset removed forces will be streamed or not. The name of the ports have the following syntax:  portname=(portPrefix+"/filteredFT/"+sensorName). Example: "myPrefix/filteredFT/l_leg_ft_sensor" | The value streamed by this ports is affected by the secondary calibration matrix, the estimated offset and temperature coefficients ( if any ). |
 | publishNetExternalWrenches |        - | bool          |  -    |      false   |  No      | Flag to stream the net external wrenches acting on each link on the port portPrefix+"/netExternalWrenches:o". The content is a bottle of n pairs, where n is the number of links. The first element of each pair is the name of the link. The second element is yet another list of 6 elements containing the value of the net external wrench, defined in the link frame. The first three elements are the linear part, while the other three are the angular part.| |
 | useSkinForContacts     |        - | bool              |  -    |      true    |  No      | Flag to skip using tactile skin sensors for updating contact points and external force (pressure) information | |
+| estimateJointVelocityAcceleration     |        - | bool              |  -    |      false    |  No      | Flag to estimate the joint velocities and accelerations using Kalman filter. If true, the same measurements from the low level are ignored. | |
+| processNoiseCovariance |      -   | vector of doubles |  -    |    -          | Yes if 'estimateJointVelocityAcceleration' is true      | It should contain 3 values per each joint, in total number_of_joints*3 values. The joint order follows the one in axesNames.  |     |
+| measurementNoiseCovariance |      -   | vector of doubles |  -    |    -          | Yes if 'estimateJointVelocityAcceleration' is true      | It should contain number_of_joints values. The joint order follows the one in axesNames.  |     |
+| stateCovariance |      -   | vector of doubles |  -    |    -          | Yes if 'estimateJointVelocityAcceleration' is true      | It should contain 3 values per each joint, in total number_of_joints*3 values. The joint order follows the one in axesNames. |     |
 | IDYNTREE_SKINDYNLIB_LINKS |  -  | group             | -     | -             | Yes      |  Group describing the mapping between link names and skinDynLib identifiers. | |
 |                |   linkName_1   | string (name of a link in the model) | - | - | Yes   | Bottle of three elements describing how the link with linkName is described in skinDynLib: the first element is the name of the frame in which the contact info is expressed in skinDynLib (tipically DH frames), the second a integer describing the skinDynLib BodyPart , and the third a integer describing the skinDynLib LinkIndex  | |
 |                |   ...   | string (name of a link in the model) | - | -     | Yes      | Bottle of three elements describing how the link with linkName is described in skinDynLib: the first element is the name of the frame in which the contact info is expressed in skinDynLib (tipically DH frames), the second a integer describing the skinDynLib BodyPart , and the third a integer describing the skinDynLib LinkIndex  | |
@@ -225,7 +229,7 @@ For a detailed explanation on their usage, please see the document [Using temper
       </device>
   </devices>
   ```
-  
+
 ### RPC commands
 You can access the device while running via `YARP RPC`. You can run the following command to access into the RPC mode of the device.
 
diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
index 9b08185..5ea9fe9 100644
--- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
+++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
@@ -176,6 +176,35 @@ bool getConfigParamsAsList(os::Searchable& config,std::string propertyName , std
     return true;
 }
 
+bool getConfigParamsAsList(const yarp::os::Searchable& config,std::string propertyName , iDynTree::VectorDynSize & list, bool isRequired)
+{
+    yarp::os::Property prop;
+    prop.fromString(config.toString().c_str());
+    yarp::os::Bottle *propNames=prop.find(propertyName).asList();
+    if(propNames==nullptr)
+    {
+        if(isRequired)
+        {
+            yError() <<"WholeBodyDynamicsDevice: Error parsing parameters: \" "<<propertyName<<" \" should be followed by a list\n";
+        }
+        return false;
+    }
+
+    list.resize(propNames->size());
+    for(auto elem=0u; elem < propNames->size(); elem++)
+    {
+        const auto value = propNames->get(elem);
+        if( !value.isFloat64() && !value.isInt32() )
+        {
+            yError() << "WholeBodyDynamicsDevice: Error parsing parameters: \" "<<propertyName<<" \". Expected double or integer array.\n";
+            return false;
+        }
+        list(elem) = value.asFloat64();
+    }
+
+    return true;
+}
+
 bool getUsedDOFsList(os::Searchable& config, std::vector<std::string> & usedDOFs)
 {
     bool required{true};
@@ -194,7 +223,6 @@ bool getGravityCompensationDOFsList(os::Searchable& config, std::vector<std::str
     return getConfigParamsAsList(config,"gravityCompensationAxesNames",gravityCompensationDOFs, required);
 }
 
-
 bool WholeBodyDynamicsDevice::openRemapperControlBoard(os::Searchable& config)
 {
     // Pass to the remapper just the relevant parameters (axesList)
@@ -824,6 +852,11 @@ void WholeBodyDynamicsDevice::resizeBuffers()
     this->estimatedJointTorques.resize(estimator.model());
     this->estimatedJointTorquesYARP.resize(this->estimatedJointTorques.size(),0.0);
     this->estimateExternalContactWrenches.resize(estimator.model());
+    this->jointPosKF.resize(estimator.model());
+    this->jointVelKF.resize(estimator.model());
+    this->jointAccKF.resize(estimator.model());
+    jointVelKF.zero();
+    jointAccKF.zero();
 
     // Resize F/T stuff
     size_t nrOfFTSensors = estimator.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE);
@@ -1010,6 +1043,18 @@ bool WholeBodyDynamicsDevice::loadSettingsFromConfig(os::Searchable& config)
         settings.useJointAcceleration = prop.find(useJointAccelerationOptionName.c_str()).asBool();
     }
 
+    std::string estimateJointVelocityAccelerationOptionName = "estimateJointVelocityAcceleration";
+    if( !(prop.check(estimateJointVelocityAccelerationOptionName.c_str()) && prop.find(estimateJointVelocityAccelerationOptionName.c_str()).isBool()) )
+    {
+        yWarning() << "wholeBodyDynamics: estimateVelocityAccelerationOptionName bool parameter missing, please specify it.";
+        yWarning() << "wholeBodyDynamics: setting estimateVelocityAccelerationOptionName to the default value of true, but this is a deprecated behaviour that will be removed in the future.";
+        settings.estimateJointVelocityAcceleration = false;
+    }
+    else
+    {
+        settings.estimateJointVelocityAcceleration = prop.find(estimateJointVelocityAccelerationOptionName.c_str()).asBool();
+    }
+
     // Check for measurements low pass filter cutoff frequencies
     if (!applyLPFSettingsFromConfig(prop, "imuFilterCutoffInHz"))
     {
@@ -1421,6 +1466,18 @@ bool WholeBodyDynamicsDevice::open(os::Searchable& config)
     // resize internal buffers
     this->resizeBuffers();
 
+    // initialize kalman filter
+    if(settings.estimateJointVelocityAcceleration)
+    {
+        filters.jntVelAccKFFilter = std::make_unique<iDynTree::DiscreteKalmanFilterHelper>();
+
+        if( !filters.initKalmanFilter(config, filters.jntVelAccKFFilter, getPeriod(), estimator.model().getNrOfDOFs()))
+        {
+            yError() << "wholeBodyDynamics: unable to init kalman filter.";
+            return false;
+        }
+    }
+
     // Open settings related to gravity compensation (we need the estimator to be open)
     ok = this->loadGravityCompensationSettingsFromConfig(config);
     if( !ok )
@@ -2075,7 +2132,6 @@ void WholeBodyDynamicsDevice::readSensors()
     }
 
     // At the moment we are assuming that all joints are revolute
-
     if( settings.useJointVelocity )
     {
         ok = remappedControlBoardInterfaces.encs->getEncoderSpeeds(jointVel.data());
@@ -2104,7 +2160,6 @@ void WholeBodyDynamicsDevice::readSensors()
 
         // Convert from degrees (used on wire by YARP) to radians (used by iDynTree)
         convertVectorFromDegreesToRadians(jointAcc);
-
     }
     else
     {
@@ -2153,6 +2208,49 @@ void WholeBodyDynamicsDevice::filterSensorsAndRemoveSensorOffsets()
         filteredSensorMeasurements.setMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,ft,filteredFTMeasure);
     }
 
+    if( settings.estimateJointVelocityAcceleration )
+    {
+        iDynTree::VectorDynSize kfState;
+        kfState.resize(estimator.model().getNrOfDOFs()*3);
+
+        iDynTree::VectorDynSize measurement(estimator.model().getNrOfDOFs());
+
+        iDynTree::toEigen(measurement) = iDynTree::toEigen(jointPos);
+
+        if (!filters.jntVelAccKFFilter->kfPredict())
+        {
+            yError() << " wholeBodyDynamics : kf predict step failed ";
+        }
+
+        if (!filters.jntVelAccKFFilter->kfSetMeasurementVector(measurement))
+        {
+            yError() << " wholeBodyDynamics : kf cannot set measurement ";
+        }
+
+        if (!filters.jntVelAccKFFilter->kfUpdate())
+        {
+            yError() << " wholeBodyDynamics : kf update step failed ";
+        }
+
+        // The first estimator.model().getNrOfDOFs() values of the state are the joint positions
+        // the following estimator.model().getNrOfDOFs() values are the joint velocities
+        // the last estimator.model().getNrOfDOFs() values are the joint accelerations
+
+        filters.jntVelAccKFFilter->kfGetStates(kfState);
+
+        iDynTree::toEigen(jointPosKF) = iDynTree::toEigen(kfState).head(estimator.model().getNrOfDOFs());
+
+        if( settings.useJointVelocity )
+        {
+            iDynTree::toEigen(jointVelKF) = iDynTree::toEigen(kfState).segment(estimator.model().getNrOfDOFs(),estimator.model().getNrOfDOFs());
+        }
+
+        if( settings.useJointAcceleration )
+        {
+            iDynTree::toEigen(jointAccKF) = iDynTree::toEigen(kfState).tail(estimator.model().getNrOfDOFs());
+        }
+    }
+
     // Filter joint vel
     if( settings.useJointVelocity )
     {
@@ -2201,8 +2299,16 @@ void WholeBodyDynamicsDevice::updateKinematics()
         // Hardcode for the meanwhile
         iDynTree::FrameIndex imuFrameIndex = estimator.model().getFrameIndex(settings.imuFrameName);
 
-        estimator.updateKinematicsFromFloatingBase(jointPos,jointVel,jointAcc,imuFrameIndex,
-                                                   filteredIMUMeasurements.linProperAcc,filteredIMUMeasurements.angularVel,filteredIMUMeasurements.angularAcc);
+        if(settings.estimateJointVelocityAcceleration)
+        {
+            estimator.updateKinematicsFromFloatingBase(jointPos,jointVelKF,jointAccKF,imuFrameIndex,
+                                                       filteredIMUMeasurements.linProperAcc,filteredIMUMeasurements.angularVel,filteredIMUMeasurements.angularAcc);
+        }
+        else
+        {
+            estimator.updateKinematicsFromFloatingBase(jointPos,jointVel,jointAcc,imuFrameIndex,
+                                                       filteredIMUMeasurements.linProperAcc,filteredIMUMeasurements.angularVel,filteredIMUMeasurements.angularAcc);
+        }
 
         if( m_gravityCompensationEnabled )
         {
@@ -2222,7 +2328,14 @@ void WholeBodyDynamicsDevice::updateKinematics()
         gravity(1) = settings.fixedFrameGravity.y;
         gravity(2) = settings.fixedFrameGravity.z;
 
-        estimator.updateKinematicsFromFixedBase(jointPos,jointVel,jointAcc,fixedFrameIndex,gravity);
+        if( settings.estimateJointVelocityAcceleration )
+        {
+            estimator.updateKinematicsFromFixedBase(jointPos,jointVelKF,jointAccKF,fixedFrameIndex,gravity);
+        }
+        else
+        {
+            estimator.updateKinematicsFromFixedBase(jointPos,jointVel,jointAcc,fixedFrameIndex,gravity);
+        }
 
         if( m_gravityCompensationEnabled )
         {
@@ -2611,7 +2724,10 @@ void WholeBodyDynamicsDevice::publishExternalWrenches()
         // Update kinDynComp model
         iDynTree::Vector3 dummyGravity;
         dummyGravity.zero();
-        this->kinDynComp.setRobotState(this->jointPos,this->jointVel,dummyGravity);
+        if ( settings.estimateJointVelocityAcceleration )
+            this->kinDynComp.setRobotState(this->jointPos,this->jointVelKF,dummyGravity);
+        else
+            this->kinDynComp.setRobotState(this->jointPos,this->jointVel,dummyGravity);
     }
 
     if (enablePublishNetExternalWrenches || this->outputWrenchPorts.size() > 0)
@@ -2896,7 +3012,7 @@ bool WholeBodyDynamicsDevice::setupCalibrationWithVerticalForcesOnTheFeetAndJets
     {
         yError() << "Model name is invalid, choose either mk1 or mk1.1" ;
         return false;
-    } 
+    }
 
     // Check if the iRonCub-XX jets frames exist
     std::string leftArmJetFrame = {"l_arm_jet_turbine"};
@@ -3262,6 +3378,15 @@ bool WholeBodyDynamicsDevice::setUseOfJointAccelerations(const bool enable)
     return true;
 }
 
+bool WholeBodyDynamicsDevice::setUseOfJointVelocityAccelerationEstimation(const bool enable)
+{
+    std::lock_guard<std::mutex> guard(this->deviceMutex);
+
+    this->settings.estimateJointVelocityAcceleration = enable;
+
+    return true;
+}
+
 std::string WholeBodyDynamicsDevice::getCurrentSettingsString()
 {
    std::lock_guard<std::mutex> guard(this->deviceMutex);
@@ -3305,6 +3430,7 @@ wholeBodyDynamicsDeviceFilters::wholeBodyDynamicsDeviceFilters(): imuLinearAccel
                                                                   forcetorqueFilters(0),
                                                                   jntVelFilter(0),
                                                                   jntAccFilter(0),
+                                                                  jntVelAccKFFilter(nullptr),
                                                                   bufferYarp3(0),
                                                                   bufferYarp6(0),
                                                                   bufferYarpDofs(0)
@@ -3312,6 +3438,110 @@ wholeBodyDynamicsDeviceFilters::wholeBodyDynamicsDeviceFilters(): imuLinearAccel
 
 }
 
+bool wholeBodyDynamicsDeviceFilters::initCovarianceMatrix(const yarp::os::Searchable &config, std::string parameterName, iDynTree::MatrixDynSize & matrix)
+{
+    iDynTree::VectorDynSize covariances;
+    if(!getConfigParamsAsList(config, parameterName, covariances, true))
+    {
+        yError() << " wholeBodyDynamics : covariance vector not valid. ";
+        return false;
+    }
+
+    if( (matrix.cols() != matrix.rows()) || (covariances.size() != matrix.cols()) )
+    {
+        yError() << " wholeBodyDynamics : size of covariance vector does not match covariance matrix size. Expected " << matrix.cols() << " . Read " << covariances.size();
+        return false;
+    }
+
+    matrix.zero();
+    iDynTree::toEigen(matrix).diagonal() = iDynTree::toEigen(covariances);
+
+    return true;
+}
+
+bool wholeBodyDynamicsDeviceFilters::initKalmanFilter(const yarp::os::Searchable &config, std::unique_ptr<iDynTree::DiscreteKalmanFilterHelper> &kf, double periodInSeconds, int nrOfDOFsProcessed)
+{
+    // Steady-state Kalman Filter with a null jerk model.
+    //
+    // The state vector contains the joint positions, velocities and accelerations.
+    // x = [q0,...,qn,dq0,...,dqn,ddq0,...,ddqn]
+    //
+    // There is no input. The measures are the joint positions.
+    // y = [q0,...,qn]
+    //
+    // State space model:
+    // x{k+1} = A x{k}
+    // y{k}   = C x{k}
+
+    iDynTree::MatrixDynSize A(3*nrOfDOFsProcessed, 3*nrOfDOFsProcessed);
+    A.zero();
+    for (int i = 0; i < nrOfDOFsProcessed; i++)
+    {
+        A(i, i) = 1;
+        A(i, i+nrOfDOFsProcessed) = periodInSeconds;
+        A(i, i+2*nrOfDOFsProcessed) = 0.5*periodInSeconds*periodInSeconds;
+
+        A(i+nrOfDOFsProcessed, i+nrOfDOFsProcessed) = 1;
+        A(i+nrOfDOFsProcessed, i+2*nrOfDOFsProcessed) = periodInSeconds;
+
+        A(i+2*nrOfDOFsProcessed, i+2*nrOfDOFsProcessed) = 1;
+    }
+
+    iDynTree::MatrixDynSize C(nrOfDOFsProcessed, 3*nrOfDOFsProcessed);
+    iDynTree::toEigen(C).setIdentity();
+
+    iDynTree::MatrixDynSize Q(3*nrOfDOFsProcessed,3*nrOfDOFsProcessed);
+    if(!this->initCovarianceMatrix(config, "processNoiseCovariance", Q))
+    {
+        yError() << " wholeBodyDynamics : failed to set process noise covariance matrix. ";
+        return false;
+    }
+
+    iDynTree::MatrixDynSize R(nrOfDOFsProcessed,nrOfDOFsProcessed);
+    if(!this->initCovarianceMatrix(config, "measurementNoiseCovariance", R))
+    {
+        yError() << " wholeBodyDynamics : failed to set measurement noise covariance matrix. ";
+        return false;
+    }
+
+    iDynTree::MatrixDynSize P0(3*nrOfDOFsProcessed,3*nrOfDOFsProcessed);
+    if(!this->initCovarianceMatrix(config, "stateCovariance", P0))
+    {
+        yError() << " wholeBodyDynamics : failed to set initial state covariance matrix. ";
+        return false;
+    }
+
+    if (!kf->constructKalmanFilter(A, C))
+    {
+        yError() << " wholeBodyDynamics : kalman filter cannot be constructed. ";
+        return false;
+    }
+
+    bool ok = kf->kfSetSystemNoiseCovariance(Q) && kf->kfSetMeasurementNoiseCovariance(R) && kf->kfSetStateCovariance(P0);
+    if(!ok)
+    {
+        yError() << " wholeBodyDynamics : unable to initialize the kf covariances. ";
+        return false;
+    }
+
+    iDynTree::VectorDynSize x0;
+    x0.resize(3*nrOfDOFsProcessed);
+    x0.zero();
+    if (!kf->kfSetInitialState(x0))
+    {
+        yError() << " wholeBodyDynamics : Impossible to set initial state. ";
+        return false;
+    }
+
+    if (!kf->kfInit())
+    {
+        yError() << " wholeBodyDynamics : Impossible to initialize kalman fitler. ";
+        return false;
+    }
+
+    return true;
+}
+
 void wholeBodyDynamicsDeviceFilters::init(int nrOfFTSensors,
                                           double initialCutOffForFTInHz,
                                           double initialCutOffForIMUInHz,
@@ -3394,6 +3624,11 @@ void wholeBodyDynamicsDeviceFilters::fini()
         delete jntAccFilter;
         jntAccFilter = 0;
     }
+
+    if( jntVelAccKFFilter )
+    {
+        jntVelAccKFFilter.reset(nullptr);
+    }
 }
 
 wholeBodyDynamicsDeviceFilters::~wholeBodyDynamicsDeviceFilters()
diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h
index e1fa683..777a646 100644
--- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h
+++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h
@@ -22,6 +22,7 @@
 #include <iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h>
 #include <iDynTree/skinDynLibConversions.h>
 #include <iDynTree/KinDynComputations.h>
+#include <iDynTree/Estimation/KalmanFilter.h>
 
 // Filters
 #include "ctrlLibRT/filters.h"
@@ -59,6 +60,10 @@ struct outputWrenchPortInformation
 
 class wholeBodyDynamicsDeviceFilters
 {
+    private:
+
+    bool initCovarianceMatrix(const yarp::os::Searchable &config, std::string parameterName, iDynTree::MatrixDynSize & matrix);
+
     public:
 
     wholeBodyDynamicsDeviceFilters();
@@ -78,11 +83,14 @@ class wholeBodyDynamicsDeviceFilters
                                double cutOffForIMUInHz,
                                double cutOffForJointVelInHz,
                                double cutOffForJointAccInHz);
+
     /**
      * Deallocate the filters
      */
     void fini();
 
+    bool initKalmanFilter(const yarp::os::Searchable &config, std::unique_ptr<iDynTree::DiscreteKalmanFilterHelper> &kf, double periodInSeconds, int nrOfDOFsProcessed);
+
 
     ~wholeBodyDynamicsDeviceFilters();
 
@@ -101,6 +109,9 @@ class wholeBodyDynamicsDeviceFilters
     ///< low pass filter for Joint accelerations
     iCub::ctrl::realTime::FirstOrderLowPassFilter * jntAccFilter;
 
+    ///< KF filter for Joint velocity and accelerations
+    std::unique_ptr<iDynTree::DiscreteKalmanFilterHelper> jntVelAccKFFilter;
+
     ///< Yarp vector buffer of dimension 3
     yarp::sig::Vector bufferYarp3;
 
@@ -390,7 +401,10 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
     yarp::sig::Vector              ftMeasurement;
     yarp::sig::Vector              imuMeasurement;
     yarp::sig::Vector              estimatedJointTorquesYARP;
-    yarp::sig::VectorOf<double>              tempMeasurements;
+    yarp::sig::VectorOf<double>    tempMeasurements;
+    iDynTree::JointDOFsDoubleArray jointPosKF;
+    iDynTree::JointDOFsDoubleArray jointVelKF;
+    iDynTree::JointDOFsDoubleArray jointAccKF;
 
     /***
      * Buffer for raw sensors measurements.
@@ -594,6 +608,10 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
        * Set if to use or not the joint velocities in estimation.
        */
       virtual bool setUseOfJointAccelerations(const bool enable);
+      /**
+      * Set if to estimate or not the joint velocities and acceleration.
+      */
+      virtual bool setUseOfJointVelocityAccelerationEstimation(const bool enable);
       /**
        * Get the current settings in the form of a string.
        * @return the current settings as a human readable string.
diff --git a/devices/wholeBodyDynamics/app/wholebodydynamics-icub-eth-six-fts.xml b/devices/wholeBodyDynamics/app/wholebodydynamics-icub-eth-six-fts.xml
index 0b648bf..b3991cf 100644
--- a/devices/wholeBodyDynamics/app/wholebodydynamics-icub-eth-six-fts.xml
+++ b/devices/wholeBodyDynamics/app/wholebodydynamics-icub-eth-six-fts.xml
@@ -3,7 +3,12 @@
 
 <devices robot="@WBD_YARP_ROBOT_NAME@" build="1">
     <device name="wholebodydynamics" type="wholebodydynamics">
-        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,
+                                 neck_pitch, neck_roll,neck_yaw,
+                                 l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,
+                                 r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,
+                                 l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,
+                                 r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
         <param name="modelFile">model.urdf</param>
         <param name="fixedFrameGravity">(0,0,-9.81)</param>
         <param name="defaultContactFrames">(l_hand,r_hand,root_link,l_sole,r_sole,l_lower_leg,r_lower_leg,l_elbow_1,r_elbow_1)</param>
@@ -15,6 +20,49 @@
         <param name="jointVelFilterCutoffInHz">3.0</param> <!-- used if useJointVelocity is set to true -->
         <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
         <param name="devicePeriodInSeconds">0.01</param>
+        <param name="estimateJointVelocityAcceleration">false</param>  <!-- if true a kf will estimate the joint velocities and accelerations -->
+        <param name="processNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1)</param>
+        <param name="measurementNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3)</param>
+        <param name="stateCovariance">(5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1)</param>
 
         <!-- map between iDynTree links (identified by a name)
              and skinDynLib links (identified by their frame name, a BodyPart enum
diff --git a/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-and-can-four-fts.xml b/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-and-can-four-fts.xml
index 9503352..d6622d2 100644
--- a/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-and-can-four-fts.xml
+++ b/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-and-can-four-fts.xml
@@ -3,7 +3,12 @@
 
 <devices robot="@WBD_YARP_ROBOT_NAME@" build="1">
     <device name="wholebodydynamics" type="wholebodydynamics">
-        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,
+                                 neck_pitch,neck_roll,neck_yaw,
+                                 l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,
+                                 r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,
+                                 l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,
+                                 r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
         <param name="modelFile">model.urdf</param>
         <param name="fixedFrameGravity">(0,0,-9.81)</param>
         <param name="defaultContactFrames">(l_hand,r_hand,root_link,l_sole,r_sole,l_lower_leg,r_lower_leg,l_elbow_1,r_elbow_1)</param>
@@ -13,8 +18,51 @@
         <param name="imuFilterCutoffInHz">3.0</param>
         <param name="forceTorqueFilterCutoffInHz">3.0</param>
         <param name="jointVelFilterCutoffInHz">3.0</param> <!-- used if useJointVelocity is set to true -->
-        <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->        
+        <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
         <param name="devicePeriodInSeconds">0.01</param>
+        <param name="estimateJointVelocityAcceleration">false</param>  <!-- if true a kf will estimate the joint velocities and accelerations -->
+        <param name="processNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1)</param>
+        <param name="measurementNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3)</param>
+        <param name="stateCovariance">(5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1)</param>
 
         <!-- map between iDynTree links (identified by a name)
              and skinDynLib links (identified by their frame name, a BodyPart enum
diff --git a/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-and-can-six-fts.xml b/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-and-can-six-fts.xml
index 28024e5..d19a14f 100644
--- a/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-and-can-six-fts.xml
+++ b/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-and-can-six-fts.xml
@@ -3,7 +3,12 @@
 
 <devices robot="@WBD_YARP_ROBOT_NAME@" build="1">
     <device name="wholebodydynamics" type="wholebodydynamics">
-        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,
+                                 neck_pitch,neck_roll,neck_yaw,
+                                 l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,
+                                 r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,
+                                 l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,
+                                 r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
         <param name="modelFile">model.urdf</param>
         <param name="fixedFrameGravity">(0,0,-9.81)</param>
         <param name="defaultContactFrames">(l_hand,r_hand,root_link,l_sole,r_sole,l_lower_leg,r_lower_leg,l_elbow_1,r_elbow_1)</param>
@@ -13,8 +18,51 @@
         <param name="imuFilterCutoffInHz">3.0</param>
         <param name="forceTorqueFilterCutoffInHz">3.0</param>
         <param name="jointVelFilterCutoffInHz">3.0</param> <!-- used if useJointVelocity is set to true -->
-        <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->        
+        <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
         <param name="devicePeriodInSeconds">0.01</param>
+        <param name="estimateJointVelocityAcceleration">false</param>  <!-- if true a kf will estimate the joint velocities and accelerations -->
+        <param name="processNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1)</param>
+        <param name="measurementNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3)</param>
+        <param name="stateCovariance">(5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1)</param>
 
         <!-- map between iDynTree links (identified by a name)
              and skinDynLib links (identified by their frame name, a BodyPart enum
diff --git a/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-six-fts-sim.xml b/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-six-fts-sim.xml
index 8019e1c..c6b1e1e 100644
--- a/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-six-fts-sim.xml
+++ b/devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-six-fts-sim.xml
@@ -3,7 +3,12 @@
 
 <devices robot="@WBD_YARP_ROBOT_NAME@" build="1">
     <device name="wholebodydynamics" type="wholebodydynamics">
-        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,
+                                 neck_pitch,neck_roll,neck_yaw,
+                                 l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw,
+                                 r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_yaw,
+                                 l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,
+                                 r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
         <param name="modelFile">model.urdf</param>
         <param name="fixedFrameGravity">(0,0,-9.81)</param>
         <param name="defaultContactFrames">(l_hand,r_hand,root_link,l_sole,r_sole,l_upper_leg,r_upper_leg,l_elbow_1,r_elbow_1)</param>
@@ -18,6 +23,49 @@
         <param name="useSkinForContacts">true</param> <!-- bypass handling skinDynLib for robots without skin -->
         <param name="publishNetExternalWrenches">true</param>
         <param name="devicePeriodInSeconds">0.01</param>
+        <param name="estimateJointVelocityAcceleration">false</param>  <!-- if true a kf will estimate the joint velocities and accelerations -->
+        <param name="processNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1)</param>
+        <param name="measurementNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3)</param>
+        <param name="stateCovariance">(5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1)</param>
 
         <!-- map between iDynTree links (identified by a name)
              and skinDynLib links (identified by their frame name, a BodyPart enum
diff --git a/devices/wholeBodyDynamics/app/wholebodydynamics-icub3-external-sim.xml b/devices/wholeBodyDynamics/app/wholebodydynamics-icub3-external-sim.xml
index 70a0c1c..c57fb22 100644
--- a/devices/wholeBodyDynamics/app/wholebodydynamics-icub3-external-sim.xml
+++ b/devices/wholeBodyDynamics/app/wholebodydynamics-icub3-external-sim.xml
@@ -3,7 +3,12 @@
 
 <devices robot="@WBD_YARP_ROBOT_NAME@" build="1">
     <device name="wholebodydynamics" type="wholebodydynamics">
-        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,
+                                 neck_pitch, neck_roll,neck_yaw,
+                                 l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_prosup,l_wrist_pitch,l_wrist_yaw,
+                                 r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_prosup,r_wrist_pitch,r_wrist_yaw,
+                                 l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,
+                                 r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
         <param name="modelFile">model.urdf</param>
         <param name="fixedFrameGravity">(0,0,-9.81)</param>
         <param name="defaultContactFrames">(l_hand,r_hand,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_upper_leg,r_upper_leg)</param>
@@ -18,6 +23,49 @@
         <param name="useSkinForContacts">false</param>
         <param name="publishNetExternalWrenches">true</param>
         <param name="devicePeriodInSeconds">0.01</param>
+        <param name="estimateJointVelocityAcceleration">true</param>  <!-- if true a kf will estimate the joint velocities and accelerations -->
+        <param name="processNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1)</param>
+        <param name="measurementNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3)</param>
+        <param name="stateCovariance">(5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1)</param>
 
         <group name="GRAVITY_COMPENSATION">
             <param name="enableGravityCompensation">true</param>
diff --git a/devices/wholeBodyDynamics/app/wholebodydynamics-icubheidelberg01-external.xml b/devices/wholeBodyDynamics/app/wholebodydynamics-icubheidelberg01-external.xml
index 4a31942..02c841f 100644
--- a/devices/wholeBodyDynamics/app/wholebodydynamics-icubheidelberg01-external.xml
+++ b/devices/wholeBodyDynamics/app/wholebodydynamics-icubheidelberg01-external.xml
@@ -3,7 +3,9 @@
 
 <devices robot="iCubHeidelberg01" build="1">
     <device name="wholebodydynamics" type="wholebodydynamics">
-        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,
+                                 l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,
+                                 r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
         <param name="modelFile">model.urdf</param>
         <param name="fixedFrameGravity">(0,0,-9.81)</param>
         <param name="defaultContactFrames">(root_link,l_sole,r_sole,l_lower_leg,r_lower_leg)</param>
@@ -13,8 +15,30 @@
         <param name="imuFilterCutoffInHz">3.0</param>
         <param name="forceTorqueFilterCutoffInHz">3.0</param>
         <param name="jointVelFilterCutoffInHz">3.0</param> <!-- used if useJointVelocity is set to true -->
-        <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->        
+        <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
         <param name="devicePeriodInSeconds">0.01</param>
+        <param name="estimateJointVelocityAcceleration">false</param>  <!-- if true a kf will estimate the joint velocities and accelerations -->
+        <param name="processNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1)</param>
+        <param name="measurementNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3)</param>
+        <param name="stateCovariance">(5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1)</param>
 
         <!-- map between iDynTree links (identified by a name)
              and skinDynLib links (identified by their frame name, a BodyPart enum
diff --git a/devices/wholeBodyDynamics/app/wholebodydynamics-icubheidelberg01-robot.xml b/devices/wholeBodyDynamics/app/wholebodydynamics-icubheidelberg01-robot.xml
index 32e5635..0ce985e 100644
--- a/devices/wholeBodyDynamics/app/wholebodydynamics-icubheidelberg01-robot.xml
+++ b/devices/wholeBodyDynamics/app/wholebodydynamics-icubheidelberg01-robot.xml
@@ -3,7 +3,9 @@
 
 <devices robot="iCubHeidelberg01" build="1">
     <device name="wholebodydynamics" type="wholebodydynamics">
-        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,
+                                 l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,
+                                 r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
         <param name="modelFile">model.urdf</param>
         <param name="fixedFrameGravity">(0,0,-9.81)</param>
         <param name="defaultContactFrames">(root_link,l_sole,r_sole,l_lower_leg,r_lower_leg)</param>
@@ -13,8 +15,30 @@
         <param name="imuFilterCutoffInHz">3.0</param>
         <param name="forceTorqueFilterCutoffInHz">3.0</param>
         <param name="jointVelFilterCutoffInHz">3.0</param> <!-- used if useJointVelocity is set to true -->
-        <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->        
+        <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
         <param name="devicePeriodInSeconds">0.01</param>
+        <param name="estimateJointVelocityAcceleration">false</param>  <!-- if true a kf will estimate the joint velocities and accelerations -->
+        <param name="processNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                              1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1,
+                                              1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1, 1.0e+1)</param>
+        <param name="measurementNoiseCovariance">(1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                                  1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3)</param>
+        <param name="stateCovariance">(5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4, 5.0e-4,
+                                       1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3,
+                                       1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1,
+                                       1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1)</param>
 
         <!-- map between iDynTree links (identified by a name)
              and skinDynLib links (identified by their frame name, a BodyPart enum
diff --git a/idl/wholeBodyDynamicsSettings/wholeBodyDynamicsSettings.thrift b/idl/wholeBodyDynamicsSettings/wholeBodyDynamicsSettings.thrift
index ce5f34b..d9fc58f 100644
--- a/idl/wholeBodyDynamicsSettings/wholeBodyDynamicsSettings.thrift
+++ b/idl/wholeBodyDynamicsSettings/wholeBodyDynamicsSettings.thrift
@@ -29,6 +29,7 @@ struct wholeBodyDynamicsSettings {
     11: bool startWithZeroFTSensorOffsets   /** Use zero FT sensor offsets at startup. If this flag is set to false, the device estimates the offsets of FT sensors at startup. Note that this option allows to enable/disable skipping the manual calling of resetOffsets to reset the offsets for FT sensors, most specially during simulations*/
     /** If this flag is set to true, the read from the sensors is skipped at startup */
     12: bool disableSensorReadCheckAtStartup
+    13: bool estimateJointVelocityAcceleration; /** Estimate joint velocity and acceleration if this is true, use measurements otherwise. */
 } (
     yarp.editor = "true"
 )