Improve velocity estimation

[andreadelprete]

Currently the velocities are estimated using a Savitsky-Golay filter, which tries to fit (in the least-squares sense) a 2nd-order polynomial to the last N joint position measurements, and then uses the derivative of the polynomial in the center of the sliding window as velocity estimate. This method is actually equivalent to a 1st-order low-pass filter. We may get better results by using a 2nd-order Butterworth filter, but this remains to be verified on the robot.

[andreadelprete]

I found this post online that describes an alternative to Savitsky-Golay filters for computing numerical derivatives. It could interesting to give it a try.

[andreadelprete]

I just found a python implementation of the method described in the post linked above.

[andreadelprete]

And here it is a Matlab implementation, which contains also the case of one-side filter (the one we are interested in).

[andreadelprete]

I found the slides of a presentation given by Mamadou Mboup. He presents a good review of the state of the art for derivative estimation, and he discusses a technique I didn't know (Jacobi derivative estimators). It may be interesting to take a look at this as well.

[andreadelprete]

Another common approach to velocity estimation is to do finite differencing followed by low-pass filtering. A common choice are low-pass Butterworth filters because they are minimum-phase filters (hence they provide little delay) and they have a smooth frequency response. On this page they provide a C code to compute the filter coefficients given the cut-off frequency and the filter order: http://www.exstrom.com/journal/sigproc/

[andreadelprete]

Here you can find an interesting comparison between different kinds of low-pass filters: https://inst.eecs.berkeley.edu/~ee247/fa10/files07/lectures/L2_2_f10n.pdf

[andreadelprete]

Rohan and I did some tests on HRP-2 last week to try and figure out which velocity estimation worked better among the different filters that Rohan designed offline. We used the inverse dynamics controller to follow a challenging joint trajectory, in which the knee reaches its velocity limit. In this test we focused mainly on the current tracking, which uses the velocity estimation to compensate for the back-EMF. Here you can see the current tracking when using a low-pass filter with large delay (about 100 ms) for velocity estimation:

Instead with a filter with a small delay (about 7 ms) you got this:

Using just finite differences of the encoders we got this:

Overall it seems that low delay and large noise is better than large delay and small noise. However, we should consider that during this test the robot was in the air. When we put it on the ground it may not tolerate this level of noise in the joint velocity estimation, and may become unstable. More tests are needed to determine the best velocity estimation for our needs.

[nmansard]

It is interesting to see that the result with small delay and with finite diff are quite the same (very similar shape) with just the high-frequency noise due to the finite diff. We may then deduce that removing this high-freq noise is an indicator that the system "works".

[proyan]

In further tests last week, @andreadelprete and @thomasfla found that savitzky-golay filter was providing better (or rather, more suitable) base estimation profile than the high noise-low delay (HNLD) filters that we used earlier. The previous HNLD velocity filters were used for mostly friction compensation, while the tests that @andreadelprete did was for base estimation with robot on the ground.

To improve the base estimation profile, we got the spectral density of experimental data with robot on the ground. The sample profile for the rhp joint after finite differencing(in blue) compared with savitzky golay (in green) is as follows:

The top subplot is the spectral density, while the bottom subplot is the velocity profile of fd vs savitzky-golay.

Similar plots were obtained for other leg joints as well. Based on the spectral density, it appears that 4 distinct regions of frequency behaviours are present. For the rhp joint, with the signal data given by experiment 20171016_152901_balance_ctrl_for_vel_est and sampling frequency 666Hz, these regions are:
0-7Hz: Information about the velocity profile
7-30 Hz: Resonance on the unit steps of encoder.
30-70Hz: Relatively low amplitude noise+ resonance on unit steps of encoder
70-333Hz: Noise

The behaviour of Savitzky-golay is such that the net power in the high noise region is really small, while the first ripple of the filter occurs around the 2nd frequency zone. So to replicate the performance, we decided to use two separate filters, one which suppresses between 7-30Hz frequency but may lead to low noise suppression in high frequencies, and another which has high noise suppression in high frequencies, but has a much higher transition frequency. Both these filters would result in low delay performance. After some trial and error, the following filters were chosen:

b1,a1 = cheby2(2, 20,0.05);
b2,a2= cheby1(4,0.05,0.08)

The spectral performance of these filters with savitzky golay is as follows: (savitzky golay is blue, and the chebyshev series filters are in green)

The frequency performance of the filter wrt to savitzky golay is as follows:

The delay is much smaller than that of savitzky golay, and the performance of the filter is better in both regions of interest. While the low delay of the velocity filters we used for friction compensation is appealing, the same performance is difficult to replicate in the base-estimators. Perhaps we should be using different filters based on the different performance characteristics, and see how the overall effect tuns out to be.

The same filter behaves better than savitzky golay in other joints as well.