Add kalman filter joint velocity and acceleration estimation

[isorrentino]

This PR implements the joint velocity and acceleration estimation using a steady-state Kalman filter with a null jerk model.

The state vector is

$$ x_k = \begin{bmatrix} q_k \\ \dot q_k \\ \ddot q_k \end{bmatrix} $$

The state-space model is:

$$ x_{k+1} = \begin{bmatrix} 1 & T & T^2 / 2 \\ 0 & 1 & T \\ 0 & 0 & 1 \end{bmatrix} x_k + v $$

$$ y_k = \begin{bmatrix} 1& 0 & 0 \end{bmatrix} x_k + w $$

The filter can be enabled or not from configuration files. The parameter estimateJointVelocityAcceleration has been added.

The process, measurement, and initial state covariances can be set from configuration file. The covariance matrices are meant to be diagonal. It is possible to set a specific covariance per each joint.

The filter has been already tested on the robot. I streamed on a yarp port the quantities coming from the low level and the quantities estimated by the kf (here I was controlling the right hip pitch in torque):

cc @GiulioRomualdi @HosameldinMohamed @traversaro @DanielePucci

[traversaro]

I asked for a review to @HosameldinMohamed and @prashanthr05 . As a preliminary comment, it would be great to document the new options in https://github.com/robotology/whole-body-estimators/blob/master/devices/wholeBodyDynamics/README.md .

[isorrentino]

Something I wanted to discuss is the measurement equation. I used as measurements both position and velocity even though I know that I should use only the position. The choice is coming from the need to reduce the delay of the smoothed signal. Actually, that was only a test and in the end, I left the implementation like that. But maybe we should use the correct form as the only real measurement we have is the encoder one and so the joint position. I would change the implementation if you (@traversaro @prashanthr05 @GiulioRomualdi @HosameldinMohamed) agree on this point.

[traversaro]

Something I wanted to discuss is the measurement equation. I used as measurements both position and velocity even though I know that I should use only the position. The choice is coming from the need to reduce the delay of the smoothed signal. Actually, that was only a test and in the end, I left the implementation like that. But maybe we should use the correct form as the only real measurement we have is the encoder one and so the joint position. I would change the implementation if you (@traversaro @prashanthr05 @GiulioRomualdi @HosameldinMohamed) agree on this point.

Can you make a plot comparison the results with the two variants? Otherwise I have no specific preference between the two variants. Just to understand, if you use also the velocity as a measurement you are using the velocity estimated via kalman filter as well?

[isorrentino]

Can you make a plot comparison the results with the two variants? Otherwise I have no specific preference between the two variants. Just to understand, if you use also the velocity as a measurement you are using the velocity estimated via kalman filter as well?

Here the comparison using the same covariances

On the left the joint velocity is used, on the right, it is not. Anyway, playing a bit more with the covariances I'm able to reduce the delay even though the acceleration will be much more noisy, but it is noisy also using the joint velocities.

So, in conclusion, I would go for not using the joint velocity in the kf measurements. cc @traversaro

[traversaro]

Cool, ok!

[isorrentino]

So, in conclusion, I would go for not using the joint velocity in the kf measurements.

Code changed here https://github.com/isorrentino/whole-body-estimators/commit/4fae39a0a410ebacc592275db5e4d767ce1f1aa9

[isorrentino]

The second concern is not related to this PR but in general the growing size of WBD code. Just wanted to start a discussion on it, since with additions coming in, soon it might become really difficult to debug WBD.

@prashanthr05 actually, I agree with you. But maybe this can be discussed and handled in a separate PR.

[isorrentino]

Hi @prashanthr05, I applied your suggestions.

[prashanthr05]

Thanks, @isorrentino. Could you also update the CHANGELOG? @HosameldinMohamed if it's ok with you, we may proceed with the merge.

[HosameldinMohamed]

Hi @isorrentino, I am trying to run the code from your branch. I am testing it with iRonCub. When I set the parameter estimateJointVelocityAcceleration to true I got this error

[ERROR]  wholeBodyDynamics : size of covariance vector does not match covariance matrix size. Expected  78  . Read  96
[ERROR]  wholeBodyDynamics : failed to set process noise covariance matrix. 
[ERROR] wholeBodyDynamics: unable to init kalman filter.

Which makes sense of course because the number of joints in iRonCub is different. Also, it shows that checks are well implemented.

I wanted to modify the size of the matrices to be suitable for iRonCub, so I have a question, how are the matrices are ordered? I assume it's the same way as in yarpmotorgui for example. but maybe it's useful to explain it.

[isorrentino]

Hi @HosameldinMohamed, I'll update the documentation. I also noticed that there is a bug in some config files as I copied pasted the same parameters I used for iCub3 in simulation, but instead, some files contain a shorter list of joints. I'm going to fix this. Thanks for your comment.

[HosameldinMohamed]

Hi @HosameldinMohamed, I'll update the documentation. I also noticed that there is a bug in some config files as I copied pasted the same parameters I used for iCub3 in simulation, but instead, some files contain a shorter list of joints. I'm going to fix this. Thanks for your comment.

If I understood correctly, I guess you are following the same order as in the axesNames parameter. Then as you mentioned in the first comment you take only one value for each state/measurement then you fill the main diagonal of the matrices.

I think if you just mention that in the documentation it's enough for a new user. thanks!

[isorrentino]

@HosameldinMohamed modifications pushed.