naveenoid / jointoffsetcalibinertial Goto Github PK

I am using fminunc optimisation function just for validating the scripts, the cost function and the overall data processing. The final offset results are too large w.r.t. the expected joint offsets. We were expecting:

Dq < 10 deg ~ 0.05*pi rad

For the standard deviation of the results, we are expecting a value below 0.1 deg :

Sigma(Dq) < 0.1 deg ~ 0.0005*pi

The mean of the cost function value is the squared mean distance between the measured and the estimated gravity, and should be < 1/10*g.

Optimisation results:

• offsets of joints J0 and J3 as expected, but the other ones are way too large, around 30 deg !
• exit flags are also normal
• the mean of the cost function ~ 10 m.s^{-2} which is the same order of magnitude as the gravity, so we have an issue here.
• the standard deviations are correct.

This leads us to think that the issue comes probably from the estimated gravity vectors, which the following analysis will confirm.

We summarise below the optimisation results and the standard deviation for our first data set :

calibrated part  : left_leg (6 DoF)
Data reference : iCubGenova02_#1
number of samples (after sgolay filtering) : ~ 3000 (1000)
random subsets size : 100 samples

Final optimization results. Each column stands for a random init of the data subset.
Optimal offsets Dq (in radians):

optimalDq =

J0:   -0.4491   -0.4492   -0.4491   -0.4493   -0.4491
J1:    0.0726    0.0726    0.0726    0.0726    0.0726
J2:   -0.4646   -0.4647   -0.4646   -0.4647   -0.4645
J3:   -0.1137   -0.1137   -0.1137   -0.1137   -0.1137
J4:    0.2148    0.2149    0.2149    0.2149    0.2149
J5:    0.0249    0.0250    0.0250    0.0250    0.0249

Mean cost ( in (m.s^{-2})^2 ):

ans =

  105.4144  105.4148  105.4145  105.4152  105.4145

optimization function exit flag:

exitflag =

     2     2     2     2     2

other optimization info:

output = 

1x5 struct array with fields:
    iterations
    funcCount
    stepsize
    firstorderopt
    algorithm
    message

Standard deviation for each joint offset:

std_optDq =

   1.0e-04 *

    0.8117
    0.0612
    0.8064
    0.0425
    0.0117
    0.1088

I'm reporting here issues that impact this feature but are already identified as being originated in other components like for instance in iDynTree:

• getSensorIndex from sensors class returns index shifted by 1. robotology/idyntree#138
• Accelerometer 11B6 sending invalid data.
  robotology/icub-tech-support#189
• issues on arm accelerometers on iCubGenova05.
  robotology/icub-tech-support#246

measured2estimatedGravNormIssue.pdf
@naveenoid , I did some unit testing on the cost function and found a discrepancy between the measurements and the predictions. I plotted the norm of both quantities for a given data subset of 100 samples. I'm expecting to have a constant value around 9.8 and 10 m.s-2 with very small oscillation (<1/10). I get a flat line from the prediction (positive proper acceleration) but a 10m.s-2 oscillation in the measurements (refer to attached figure).
I didn't have time to look thourouly, but it looks like 1 measurement is missing / is null for every timestamp. I suspect an index issue. It should be easy to debug tomorrow.

I suggest we set here some aspects of the dev process, like for instance some naming convention for the branches we will be pushing to this repo. I'm listing in this first message the topics of further discussions (list to be populated). We can check the boxes once we finished the discussion on a topic and agreed :

• branch naming conventions
• data import folder (and source)
• ...