WIP: accel cal index bug

[dagar]

The accel_T matrix that the program generates is incorrect. It actually generates the transpose of accel_T. Existing calibrations are working because the calibration process only retains the diagonal of the matrix; and regenerate the rotation matrix in another calibration step. This extra step should be unnecessary if the bug is fixed.

In the front documentation block comment, the calibration algorithm is described. I have extended this description to more clearly show the error.

...
* Find accel_T in the equation
*
* 9 unknown constants
* need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
*
* accel_corr_ref[3] = accel_T[3][3] * offset_corr_ref[3]
* where offset_corr_ref = accel_raw_ref[i*2] - accel_offs
*
* This equation becomes trivial to solve if we multiply both sides of the equation
* by the left inverse of accel_T; let's call this inverse matrix mat_A.
* After multiplying both sides of the equation by mat_A, the equation becomes...
*
* mat_A * accel_corr_ref[3] = mat_A * accel_T[3][3] * offset_corr_ref[3]
* which simplifies to...
*
* mat_A * accel_corr_ref[3] = offset_corr_ref[3]
*
* Equation system #1: select the measured results from test 0 (nose up)...
*
* accel_corr_ref[3] if defined to be { 1g, 0, 0}, so mat_A * accel_corr_ref[3] results in the column vector...
* [ mat_A[0][0] * 1g ] [offset_corr_ref[0]]
* [ mat_A[1][0] * 1g ] = [offset_corr_ref[1]]
* [ mat_A[2][0] * 1g ] [offset_corr_ref[2]]
*
* For now, let's use acceleration units of gravities, so each 1g just becomes 1.0.
* And since the test index in the software is the major dimension..
*
* mat_A[0][0] = offset_corr_ref[0][0] ; the offset_corr_ref from test[0], x value
* mat_A[1][0] = offset_corr_ref[0][1] ; the offset_corr_ref from test[0], y value
* mat_A[2][0] = offset_corr_ref[0][2] ; the offset_corr_ref from test[0], z value
*
* Equation system #2: select the measured results from test 2 (left down)...
*
* accel_corr_ref[3] if defined to be { 0, 1g, 0}, so mat_A * accel_corr_ref[3] results in the column vector...
* [ mat_A[0][1] * 1g ] [offset_corr_ref[0]]
* [ mat_A[1][1] * 1g ] = [offset_corr_ref[1]]
* [ mat_A[2][1] * 1g ] [offset_corr_ref[2]]
*
* Making the same gravity simplification...
*
* mat_A[0][1] = offset_corr_ref[2][0]
* mat_A[1][1] = offset_corr_ref[2][1]
* mat_A[2][1] = offset_corr_ref[2][2]
*
* Equation system #3: select the measured results from test 4 (bottom up)...
*
* accel_corr_ref[3] if defined to be { 0, 0, 1g}, so mat_A * accel_corr_ref[3] results in the column vector...
* [ mat_A[0][2] * 1g ] [offset_corr_ref[0]]
* [ mat_A[1][2] * 1g ] = [offset_corr_ref[1]]
* [ mat_A[2][2] * 1g ] [offset_corr_ref[2]]
*
* Making the same gravity simplification...
*
* mat_A[0][2] = offset_corr_ref[4][0]
* mat_A[1][2] = offset_corr_ref[4][1]
* mat_A[2][2] = offset_corr_ref[4][2]
*
* So the entire mat_A matrix is...
* [offset_corr_ref[0][0] offset_corr_ref[2][0] offset_corr_ref[4][0] ]
* [offset_corr_ref[0][1] offset_corr_ref[2][1] offset_corr_ref[4][1] ]
* [offset_corr_ref[0][2] offset_corr_ref[2][2] offset_corr_ref[4][2] ]
*

Note that for the above matrix, test number is the column index and the sensor x/y/z specifier is the row index. This is the opposite from what is coded in the .cpp program, which states...

for (unsigned i = 0; i < 3; i++) {
for (unsigned j = 0; j < 3; j++) {
float a = accel_ref[sensor][i * 2][j] - accel_offs[sensor][j];
mat_A[i][j] = a;
}
}

// It should be...

for (unsigned i = 0; i < 3; i++) {
for (unsigned j = 0; j < 3; j++) {
float a = accel_ref[sensor][i * 2][j] - accel_offs[sensor][j];
mat_A[j][i] = a;
}
}

Continuing the math explanation...

*
* We have solved for mat_A, but mat_A is the inverse of accel_T, for we need to invert mat_T to yield accel_T.
*

Notes:

• The gravities to m/s/s calculation could have been handled in the above by dividing the offset_corr_ref terms by the conversion factor. But dividing a matrix by a constant is equivalent to multiplying its inverse by the same constant, so, the way the existing code handles it is fine.
• The rotation matrix can be extracted from a accel_T by performing a row normalization. Although it is no longer needed for accelerometer rotation (the accel_T already performs this) it is handy for any gyroscope that shares a package with an accelerometer.

Regards,

Bob

* accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
*
* Solve separate system for each row of accel_T:
*
* accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
*
* [I inserted the below to better explain what should be happening.]
* System 1:
* b = accel_T * x
*
* mat_A * b = mat_A * accel_T * x ; but a matrix multiplied by it's inverse is Identity, so...
*
* mat_A * b = x
* where
*
* A * x = b
*
* x = [ accel_T[0][i] ]
* | accel_T[1][i] |
* [ accel_T[2][i] ]
*
* b = [ accel_corr_ref[0][i] ] // One measurement per side is enough
* | accel_corr_ref[2][i] |
* [ accel_corr_ref[4][i] ]
*
* a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
*
* Matrix A is common for all three systems:
* A = [ a[0][0] a[0][1] a[0][2] ]
* | a[2][0] a[2][1] a[2][2] |
* [ a[4][0] a[4][1] a[4][2] ]
*
* x = A^-1 * b
*
* accel_T = A^-1 * g
* g = 9.80665

[LorenzMeier]

Thanks! @bkueng Could you check?

[bkueng]

The way I see it's mostly a matter of how you define the matrix: Bob uses row-major in his explanations.

The implementation could be using column-major since the off-diagonals are not used (and (A^T)^-1 = (A^-1)^T).
However, looking at mat_invert3 it seems to be using row-major as well as it matches the formula in https://ardoris.wordpress.com/2008/07/18/general-formula-for-the-inverse-of-a-3x3-matrix/ if row-major is assumed.
So we should change it and extend the comments.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[LorenzMeier]

Thanks for your earlier contribution. Unfortunately as the project has overall evolved quite a bit, this change doesn't apply any more. Closing stale pull requests like this one is part of us working aggressively to bring down the PR review time so that we will be able to merge or reject PRs in the future in a much more timely fashion.