fjandrad commented 6 years ago

Since there seems to be a bug in the accelerations (and maybe velocites) coming from the data streamed by the robot. Consider doing numerical differentiation on the data befor entering estimate wrenches. Compare to see if the errors from measured to estimate are reduced somehow.

fjandrad commented 6 years ago

The changes to do the differentiation were done. Currently the acceleration measurements are not reliable. Now we need to evaluate if it is useful and what is its impact?

evaluation of using numerical differentiation

Questions to answer

How different is the estimated data if I use numerical differentiation?
How different is if just estimate the acc from the state ext velocities?
Are the velocities from state ext different from differentiating joint positions?
How different is if I estimate acc and vel from state ext joint positions?
How much does it change if I filter joint positions?
How much does it affect the external force estimation?
With the change in the estimated data should I use non filter ft data in the fitting?
Hypothesis

If I compare the measured forces with the estimated forces ( taking out the mean difference from the measurements), the combinations with the lower MSE should be a good match between the options

Options to check

on the acceleration
\dot{dq} : differentiate velocity positions from state ext
\ddot{q} : differentiate twice the joint positions from state ext
ddq : use directly the state ext accelerations
on the joint positions
q : use joint positions directly
qf : filter the joint positions
on the raw measurements
ft : use the raw data as is
ff : filter the raw data

In total this means we have 12 combinations to test.

fjandrad commented 6 years ago

checking error between numerical differentiated variables and state ext values

tests performed on the dataset 2018_09_10_Grid taken from the iCubGenova04

velocities

A comparison in the first joints :

accelerations

Using second numerical derivation of joint positions:

Using filtered joint positions( sgolay N=2,F=41) and doing second numerical derivation we get= Given this results we will filter the qj values and select the amount of value to use in the filter based on which of them gives the lower mse with respect to the measured data

fjandrad commented 6 years ago

It seems filtering the joint positions is really needed. Another option is to filter the joint velocities to get the Acc without the need to double differentiate. In that case it is required to see how it affects the estimation of the forces and which values of sgolay to use.

fjandrad commented 6 years ago

Example on the right leg dataset 2018_09_10 Grid

No acceleration in the estimation of the forces

Applying numerical differentiation

Applying sgolay on a 11 size window on the qj and then differentiating

fjandrad commented 6 years ago

A proposed solution is to apply sgolay only on the dqj instead of starting form qj. It is also worth exploring getting ddqj directly from the sgolay filter instead of applying filter and then numerical derivation.

traversaro commented 6 years ago

Which windows size/order are you using for the filtering?

traversaro commented 6 years ago

@nunoguedelha wrote:

I have a script that allows to play dynamically with the window size and polynomial order, monitoring the filtering result online, while changing these parameters https://github.com/robotology-playground/sensors-calib-inertial/blob/4514bd12ff6ee3d4959a347575b325752efc863c/src/%2BMathUtils/sgolayFilterParamsTuning.m

nunoguedelha commented 6 years ago

@traversaro @fjandrad I've re-implemented the derivative computation ($\dot{q}$,$\ddot{q}$) using the Sgolay filter instead of computing the numerical basic derivative from the filtered $q$. Here is the link to the script: https://github.com/loc2/link-angular-acceleration-estimator/blob/master/src/%2BMathUtils/sgolayFilterAndDerivate.m

nunoguedelha commented 6 years ago

Here's an example of use:

    polynomOrder = 19;
    frameLen = 251;
    derivOrder = 2;
    xRaw = <line vector q>;
    dt = 1e-2;
    [xFiltered, dxFiltered] = MathUtils.sgolayFilterAndDerivate(polynomOrder,frameLen,derivOrder,xRaw,dt);
    qFiltered = xFiltered;
    dqFiltered = dxFiltered(:,1);
    d2qFiltered = dxFiltered(:,2);

unfortunately, this assumes that the time step "dt" is constant.

prashanthr05 commented 6 years ago

@nunoguedelha @fjandrad @traversaro after obtaining the filtered results, it is better to trim away the first (windowSize+1)/ 2 samples and the last (windowSize+1)/ 2 samples from the results. Since the underlying convolution is done using the parameter 'shape' = 'same', the convolution is performed only for the central part of the input vector but the size of the input vector is retained. This deems the samples at the extremum meaningless.

See this for documentation on convolution.

nunoguedelha commented 6 years ago

@prashanthr05 yes it was intended. For applying the convolution to the entire set of samples, we need to use the "transition" filters also returned bu the core S-Golay function. In my script, if I'm not mistaken, I'm only using the central column filter. Basically the core S-Golay function returns a matrix of convolutional filters (1 filter per column). The central column holds the main filter for the whole range where the window fits. All the columns to the left are for the starting transition and the ones to the right for the ending transition (or the other way around, don't remember).

nunoguedelha commented 6 years ago

@prashanthr05 Anyway you're right, considering what I said, we should have 'shape' = 'valid', but it reduces the final amount of output samples. I think that's why I kept the 'same'. Sorry should have documented that : /

prashanthr05 commented 6 years ago

I do something like this,

function [out] = trimDatasets(in, winSize)
    out = in((winSize+1)/2:(end - (winSize+1)/2));
end

traversaro commented 6 years ago

Yes, the tricky part is that typically you need to do the trimming for all the data that you have in the datasets, not only position/velocity/acceleration. To be clear, same is ok as long as the dataset starts and ends with numer of constant position samples.

prashanthr05 commented 6 years ago

Yes, the tricky part is that typically you need to do the trimming for all the data that you have in the datasets, not only position/velocity/acceleration.

True.

To be clear, same is ok as long as the dataset starts and ends with numer of constant position samples.

That's a cool workaround.

fjandrad commented 6 years ago

I have a function called applyMask that allows me to trim a struct variable anyway I want as long as I input a logical vector, that is how I get rid of the initial and ending part after sgolay

fjandrad commented 6 years ago

Which windows size/order are you using for the filtering?

@traversaro I'm still unsure which values to use in the sgolay filter, definitively I will consider less or equal samples than the filter I was using for the FT, although to be honest I think those values were also a bit random. Do you have any suggestion on the selection of values? Another option I was thinking is taking the one that looks more or less close in shape to the values in velocity of the state ext that do not look super crazy and for the same values see what it looks like in the accelerations.

fjandrad commented 6 years ago

MSE between ftdata and estimated data

No filter was applied to the wrench only on the joint positions.

Estimation types in a data set		MSE			MSE
polynomial order	windows size	F_x	F_y	F_z	$\tau_{x}$	$\tau_{y}$	$\tau_{z}$
no filter	0	44.1277	91.7951	5.0516	0.0327	0.1072	0.0104
2	5	50.4879	98.6095	5.0556	0.5695	0.6552	0.0109
2	15	44.0961	91.8285	5.0516	0.0276	0.0975	0.0103
4	15	44.3528	92.1109	5.0523	0.0538	0.1200	0.0104
6	15	45.8732	93.8119	5.0534	0.1894	0.2532	0.0105
4	25	44.0817	91.8171	5.0516	0.0274	0.0966	0.0103
6	25	44.1226	91.8880	5.0519	0.0354	0.1007	0.0104
8	25	44.4674	92.2597	5.0524	0.0661	0.1309	0.0104
10	25	45.3750	93.2273	5.0533	0.1436	0.2079	0.0105
4	35	44.1847	91.8616	5.0515	0.0283	0.1039	0.0103
8	35	44.0720	91.8388	5.0518	0.0312	0.0966	0.0104
10	35	44.2061	91.9900	5.0521	0.0442	0.1083	0.0104
12	35	44.5363	92.3309	5.0524	0.0720	0.1360	0.0104
8	45	44.0860	91.8138	5.0515	0.0274	0.0970	0.0103
10	45	44.0615	91.8234	5.0518	0.0296	0.0955	0.0103
12	45	44.1174	91.8975	5.0519	0.0365	0.1008	0.0104

MSE of filtered wrench data

Estimation types in a data set		MSE			MSE
polynomial order	windows size	F_x	F_y	F_z	$\tau_{x}$	$\tau_{y}$	$\tau_{z}$
no filter	0	41.7736	90.9960	4.3843	0.0069	0.0556	0.0079
2	5	41.8984	91.1609	4.3845	0.0072	0.0569	0.0079
2	15	41.8967	91.1648	4.3843	0.0070	0.0567	0.0079
4	15	41.8967	91.1675	4.3844	0.0070	0.0568	0.0079
6	15	41.8968	91.1671	4.3844	0.0071	0.0568	0.0079
4	25	41.8971	91.1641	4.3843	0.0070	0.0568	0.0079
6	25	41.8960	91.1657	4.3843	0.0070	0.0567	0.0079
8	25	41.8963	91.1691	4.3844	0.0070	0.0568	0.0079
10	25	41.8975	91.1683	4.3844	0.0071	0.0568	0.0079
4	35	41.8971	91.1656	4.3843	0.0070	0.0568	0.0079
8	35	41.8967	91.1646	4.3843	0.0070	0.0567	0.0079
10	35	41.8956	91.1669	4.3843	0.0070	0.0567	0.0079
12	35	41.8957	91.1685	4.3843	0.0070	0.0568	0.0079
8	45	41.8974	91.1638	4.3843	0.0070	0.0568	0.0079
10	45	41.8972	91.1633	4.3843	0.0070	0.0568	0.0079
12	45	41.8962	91.1649	4.3843	0.0070	0.0567	0.0079

Conclusion

Although it look promising at the beginning, looking at the MSE as a performance index is clear that there is no clear advantage of filtering the joint positions and is just as good and simpler to use zero values for the acceleration. At least when looking at the grid. I'll double check with a yoga experiment.

traversaro commented 6 years ago

Another option I was thinking is taking the one that looks more or less close in shape to the values in velocity of the state ext that do not look super crazy and for the same values see what it looks like in the accelerations.

I typically do that.

fjandrad commented 6 years ago

Comparison on a yoga dataset

No acceleration

With acceleration best results: polynomial 4 window 35

MSE values

Estimation types in a data set		MSE			MSE
polynomial order	windows size	F_x	F_y	F_z	$\tau_{x}$	$\tau_{y}$	$\tau_{z}$
no filter	0	6.5313	32.9165	4.2546	0.0675	0.0432	0.0106
2	5	46.2334	67.4949	24.5470	2.4205	2.4109	0.0429
2	15	7.1465	33.8454	4.4272	0.0708	0.0583	0.0108
4	15	9.9128	36.0406	6.1807	0.2218	0.2194	0.0130
6	15	21.3098	46.4798	13.7325	0.9417	0.8946	0.0210
4	25	7.1589	33.8492	4.4372	0.0714	0.0586	0.0108
6	25	8.2175	34.6105	5.0813	0.1250	0.1203	0.0117
8	25	11.1480	37.0600	6.9595	0.2909	0.2930	0.0141
10	25	17.5567	43.2357	11.5518	0.7172	0.6698	0.0183
4	35	6.9254	33.6907	4.2773	0.0599	0.0454	0.0106
8	35	7.7693	34.2732	4.8121	0.1012	0.0942	0.0114
10	35	9.0795	35.2843	5.6303	0.1714	0.1711	0.0124
12	35	11.7038	37.4924	7.3032	0.3216	0.3251	0.0144
8	45	7.1679	33.8526	4.4272	0.0714	0.0589	0.0107
10	45	7.5890	34.1328	4.7012	0.0913	0.0831	0.0111
12	45	8.3610	34.7066	5.1889	0.1307	0.1290	0.0119

MSE filtered

Estimation types in a data set		MSE			MSE
polynomial order	windows size	F_x	F_y	F_z	$\tau_{x}$	$\tau_{y}$	$\tau_{z}$
no filter	0	5.7745	29.6649	2.8175	0.0165	0.0152	0.0066
2	5	6.1349	30.5286	2.8207	0.0100	0.0157	0.0068
2	15	6.1186	30.5157	2.8117	0.0091	0.0148	0.0067
4	15	6.1222	30.5177	2.8139	0.0092	0.0150	0.0068
6	15	6.1292	30.5240	2.8185	0.0097	0.0154	0.0068
4	25	6.1189	30.5156	2.8118	0.0091	0.0148	0.0067
6	25	6.1207	30.5163	2.8129	0.0092	0.0149	0.0067
8	25	6.1237	30.5187	2.8148	0.0093	0.0151	0.0068
10	25	6.1278	30.5228	2.8177	0.0096	0.0153	0.0068
4	35	6.1182	30.5155	2.8113	0.0090	0.0148	0.0067
8	35	6.1201	30.5161	2.8126	0.0091	0.0149	0.0067
10	35	6.1219	30.5170	2.8137	0.0092	0.0150	0.0068
12	35	6.1244	30.5191	2.8152	0.0094	0.0151	0.0068
8	45	6.1191	30.5157	2.8118	0.0091	0.0148	0.0067
10	45	6.1199	30.5161	2.8124	0.0091	0.0149	0.0067
12	45	6.1210	30.5165	2.8132	0.0092	0.0149	0.0067

Conclusion

It seems that polynomial 4 with window size 35 is a good approximation. It might be also possible that low polynomial with larger window size could reduce more the MSE, but we could over filter. It would be nice to have another criteria for selection of polynomial and window size that is less subjective. I like the fact that the profile of the resulting forces has more of the non smooth behaviors that we see in the ft even when filtered. This makes me think that is the actual effect of the acceleration that we are neglecting to some extent, so the reference values used in the calibration might be slightly closer to real values. The question is, is it worth implementing into the normal calibration workflow? It will probably do not decrease the error seen on the robot related to external forces since at the moment we disable the acceleration measurements. cc @traversaro

traversaro commented 6 years ago

One interesting point is that if we plug support for estimating acceleration, then it is already there when we use more challenging datasets. However, this yoga is the slow one? On the "fast" one, I guess there should be at least a small effect due to the acceleration, while the curves in https://github.com/robotology-playground/insitu-ft-analysis/issues/43#issuecomment-435214486 seems to be exactly the same (but it would be easier to evaluate if we plotted the two forces in the same plot).

fjandrad commented 6 years ago

this yoga is medium slow. The forces we should be looking at are the second set of 3 (Fx2 ... etc ). they are very similar with small differences. So I think I will put it but leave it disabled by default. Ideally if we put support for estimating acceleration , I think it should be on robot side and not here in an offline procedure for calibration. So I expect just to read the acceleration from somewhere.

nunoguedelha commented 6 years ago

@fjandrad I was going through this interesting analysis and there are a couple of things I'm not sure I understand:

in general, what do Fx2,Fy2,Fz2 VS Fx,Fy,Fz stand for? Yoga VS pole data set?
in https://github.com/robotology-playground/insitu-ft-analysis/issues/43#issuecomment-435214486, I assume that "MSE filtered" are computed using filtered FT data, as in the former comments, right?
No filter was applied to the wrench only on the joint positions. So which method did you apply for estimating the acceleration? filtered q, then second numerical derivative? or direct derivative estimated with SG?
the plot "With acceleration best results: polynomial 4 window 35" in https://github.com/robotology-playground/insitu-ft-analysis/issues/43#issuecomment-435214486 was obtained with filtered FT data or non filtered?
for each curve, how do you obtain several points for each timestamp? several trials of yoga demo?

fjandrad commented 6 years ago

@nunoguedelha In general the graphs compare the measured and the estimated wrenches. The first plots were a comparison using the grid dataset; the other ones are on a Yoga dataset. Measured refers to the values coming from the sensor, estimated are the values coming from iDynTree.

The plot shows the not filtered measured data in the first three lines and the estimated filtered data in the second 3 lines. The estimated filtered data can be considered doubled filtered first on the joint positions and then after the estimation of the wrenches was obtained. I had some graphs before without the second filter but I did not save them. I can redo them if you consider them relevant.
MSE filtered is comparing with the filtered measured data and double filtered estimated data.

for each curve, how do you obtain several points for each timestamp? several trials of yoga demo?

In this case it is always the same data. I obtained different points by applying different combinations of polynomial order and window size to the joint positions, and then estimate using iDyntree.

fjandrad commented 6 years ago

Comparison between no filter at joint position, filter pol 4 win 35, and measured data (all of them filtered at wrench level )

The order is the one F= no filter, F2= filter pol 4 win 35, F3= measured I put some green arrows to highlight the behavior change that I consider interesting. It is interesting because the acceleration creates a behavior that is in the over all shape (not the actual value) similar to the one we see on the actual sensor measurements. cc @traversaro this is the small non smooth behaviors I was referring to.

traversaro commented 6 years ago

Yes, this is definitely interesting, and probably worth including in the estimation if possible.

fjandrad commented 5 years ago

This was stopped mainly due to a problem in the filtering, clipping of dataset order. But I actually thought it through and its enough to just decouple the filtering of joints from the filtering of forces. The reasoning is the clipping filtering issue happens at the forces due to undesired external forces while the joint positions do not have this issue. So we can filter the joints before clipping for continuity and avoid inducing delays int he estimated wrench computation we replace only the acc and velocities

fjandrad commented 5 years ago

Done in 978048e04a980174ebb277e9eb9b798858d4325b and 0499b45c74d91c884a832d3bff9266845f3ca356

robotology-playground / insitu-ft-analysis

Do numerical differentiation for velocity and acceleration #43

evaluation of using numerical differentiation

Questions to answer

Hypothesis

Options to check

on the acceleration

on the joint positions

on the raw measurements

checking error between numerical differentiated variables and state ext values

velocities

accelerations

No acceleration in the estimation of the forces

Applying numerical differentiation

Applying sgolay on a 11 size window on the qj and then differentiating

MSE between ftdata and estimated data

MSE of filtered wrench data

Conclusion

Comparison on a yoga dataset

MSE values

MSE filtered

Conclusion

Comparison between no filter at joint position, filter pol 4 win 35, and measured data (all of them filtered at wrench level )