forked from Archive/PX4-Autopilot
commander: mag_calibration move to Magnetometer calibration helper
This commit is contained in:
Daniel Agar
parent
1867e540b0
commit
ad148af2fd
|
|
@ -62,225 +62,23 @@
|
|||
|
||||
using namespace time_literals;
|
||||
|
||||
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
|
||||
float *sphere_radius)
|
||||
{
|
||||
|
||||
float x_sumplain = 0.0f;
|
||||
float x_sumsq = 0.0f;
|
||||
float x_sumcube = 0.0f;
|
||||
|
||||
float y_sumplain = 0.0f;
|
||||
float y_sumsq = 0.0f;
|
||||
float y_sumcube = 0.0f;
|
||||
|
||||
float z_sumplain = 0.0f;
|
||||
float z_sumsq = 0.0f;
|
||||
float z_sumcube = 0.0f;
|
||||
|
||||
float xy_sum = 0.0f;
|
||||
float xz_sum = 0.0f;
|
||||
float yz_sum = 0.0f;
|
||||
|
||||
float x2y_sum = 0.0f;
|
||||
float x2z_sum = 0.0f;
|
||||
float y2x_sum = 0.0f;
|
||||
float y2z_sum = 0.0f;
|
||||
float z2x_sum = 0.0f;
|
||||
float z2y_sum = 0.0f;
|
||||
|
||||
for (unsigned int i = 0; i < size; i++) {
|
||||
|
||||
float x2 = x[i] * x[i];
|
||||
float y2 = y[i] * y[i];
|
||||
float z2 = z[i] * z[i];
|
||||
|
||||
x_sumplain += x[i];
|
||||
x_sumsq += x2;
|
||||
x_sumcube += x2 * x[i];
|
||||
|
||||
y_sumplain += y[i];
|
||||
y_sumsq += y2;
|
||||
y_sumcube += y2 * y[i];
|
||||
|
||||
z_sumplain += z[i];
|
||||
z_sumsq += z2;
|
||||
z_sumcube += z2 * z[i];
|
||||
|
||||
xy_sum += x[i] * y[i];
|
||||
xz_sum += x[i] * z[i];
|
||||
yz_sum += y[i] * z[i];
|
||||
|
||||
x2y_sum += x2 * y[i];
|
||||
x2z_sum += x2 * z[i];
|
||||
|
||||
y2x_sum += y2 * x[i];
|
||||
y2z_sum += y2 * z[i];
|
||||
|
||||
z2x_sum += z2 * x[i];
|
||||
z2y_sum += z2 * y[i];
|
||||
}
|
||||
|
||||
//
|
||||
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
|
||||
//
|
||||
// P is a structure that has been computed with the data earlier.
|
||||
// P.npoints is the number of elements; the length of X,Y,Z are identical.
|
||||
// P's members are logically named.
|
||||
//
|
||||
// X[n] is the x component of point n
|
||||
// Y[n] is the y component of point n
|
||||
// Z[n] is the z component of point n
|
||||
//
|
||||
// A is the x coordiante of the sphere
|
||||
// B is the y coordiante of the sphere
|
||||
// C is the z coordiante of the sphere
|
||||
// Rsq is the radius squared of the sphere.
|
||||
//
|
||||
//This method should converge; maybe 5-100 iterations or more.
|
||||
//
|
||||
float x_sum = x_sumplain / size; //sum( X[n] )
|
||||
float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
|
||||
float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
|
||||
float y_sum = y_sumplain / size; //sum( Y[n] )
|
||||
float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
|
||||
float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
|
||||
float z_sum = z_sumplain / size; //sum( Z[n] )
|
||||
float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
|
||||
float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
|
||||
|
||||
float XY = xy_sum / size; //sum( X[n] * Y[n] )
|
||||
float XZ = xz_sum / size; //sum( X[n] * Z[n] )
|
||||
float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
|
||||
float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
|
||||
float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
|
||||
float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
|
||||
float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
|
||||
float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
|
||||
float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
|
||||
|
||||
//Reduction of multiplications
|
||||
float F0 = x_sum2 + y_sum2 + z_sum2;
|
||||
float F1 = 0.5f * F0;
|
||||
float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
|
||||
float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
|
||||
float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
|
||||
|
||||
//Set initial conditions:
|
||||
float A = x_sum;
|
||||
float B = y_sum;
|
||||
float C = z_sum;
|
||||
|
||||
//First iteration computation:
|
||||
float A2 = A * A;
|
||||
float B2 = B * B;
|
||||
float C2 = C * C;
|
||||
float QS = A2 + B2 + C2;
|
||||
float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
|
||||
|
||||
//Set initial conditions:
|
||||
float Rsq = F0 + QB + QS;
|
||||
|
||||
//First iteration computation:
|
||||
float Q0 = 0.5f * (QS - Rsq);
|
||||
float Q1 = F1 + Q0;
|
||||
float Q2 = 8.0f * (QS - Rsq + QB + F0);
|
||||
float aA, aB, aC, nA, nB, nC, dA, dB, dC;
|
||||
|
||||
//Iterate N times, ignore stop condition.
|
||||
unsigned int n = 0;
|
||||
|
||||
while (n < max_iterations) {
|
||||
n++;
|
||||
|
||||
//Compute denominator:
|
||||
aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
|
||||
aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
|
||||
aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
|
||||
aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
|
||||
aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
|
||||
aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;
|
||||
|
||||
//Compute next iteration
|
||||
nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
|
||||
nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
|
||||
nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
|
||||
|
||||
//Check for stop condition
|
||||
dA = (nA - A);
|
||||
dB = (nB - B);
|
||||
dC = (nC - C);
|
||||
|
||||
if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
|
||||
|
||||
//Compute next iteration's values
|
||||
A = nA;
|
||||
B = nB;
|
||||
C = nC;
|
||||
A2 = A * A;
|
||||
B2 = B * B;
|
||||
C2 = C * C;
|
||||
QS = A2 + B2 + C2;
|
||||
QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
|
||||
Rsq = F0 + QB + QS;
|
||||
Q0 = 0.5f * (QS - Rsq);
|
||||
Q1 = F1 + Q0;
|
||||
Q2 = 8.0f * (QS - Rsq + QB + F0);
|
||||
}
|
||||
|
||||
*sphere_x = A;
|
||||
*sphere_y = B;
|
||||
*sphere_z = C;
|
||||
*sphere_radius = sqrtf(Rsq);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, int max_iterations, float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z,
|
||||
float *offdiag_x, float *offdiag_y, float *offdiag_z, bool sphere_fit_only)
|
||||
{
|
||||
float _fitness = 1.0e30f, _sphere_lambda = 1.0f, _ellipsoid_lambda = 1.0f;
|
||||
|
||||
for (int i = 0; i < max_iterations; i++) {
|
||||
run_lm_sphere_fit(x, y, z, _fitness, _sphere_lambda,
|
||||
size, offset_x, offset_y, offset_z,
|
||||
sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z);
|
||||
|
||||
}
|
||||
|
||||
if (!sphere_fit_only) {
|
||||
_fitness = 1.0e30f;
|
||||
|
||||
for (int i = 0; i < max_iterations; i++) {
|
||||
run_lm_ellipsoid_fit(x, y, z, _fitness, _ellipsoid_lambda,
|
||||
size, offset_x, offset_y, offset_z,
|
||||
sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
|
||||
unsigned int size, float *offset_x, float *offset_y, float *offset_z,
|
||||
unsigned int samples_collected, float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z)
|
||||
{
|
||||
//Run Sphere Fit using Levenberg Marquardt LSq Fit
|
||||
const float lma_damping = 10.0f;
|
||||
float _samples_collected = size;
|
||||
// Run Sphere Fit using Levenberg Marquardt LSq Fit
|
||||
const float lma_damping = 10.f;
|
||||
float fitness = _fitness;
|
||||
float fit1 = 0.0f, fit2 = 0.0f;
|
||||
float fit1 = 0.f;
|
||||
float fit2 = 0.f;
|
||||
|
||||
matrix::SquareMatrix<float, 4> JTJ;
|
||||
matrix::SquareMatrix<float, 4> JTJ2;
|
||||
float JTFI[4] = {};
|
||||
matrix::SquareMatrix<float, 4> JTJ{};
|
||||
matrix::SquareMatrix<float, 4> JTJ2{};
|
||||
float JTFI[4] {};
|
||||
float residual = 0.0f;
|
||||
|
||||
// Gauss Newton Part common for all kind of extensions including LM
|
||||
for (uint16_t k = 0; k < _samples_collected; k++) {
|
||||
for (uint16_t k = 0; k < samples_collected; k++) {
|
||||
|
||||
float sphere_jacob[4];
|
||||
//Calculate Jacobian
|
||||
|
|
@ -310,7 +108,7 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
|
|||
|
||||
|
||||
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
|
||||
//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
|
||||
// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
|
||||
float fit1_params[4] = {*sphere_radius, *offset_x, *offset_y, *offset_z};
|
||||
float fit2_params[4];
|
||||
memcpy(fit2_params, fit1_params, sizeof(fit1_params));
|
||||
|
|
@ -335,8 +133,8 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
|
|||
}
|
||||
}
|
||||
|
||||
//Calculate mean squared residuals
|
||||
for (uint16_t k = 0; k < _samples_collected; k++) {
|
||||
// Calculate mean squared residuals
|
||||
for (uint16_t k = 0; k < samples_collected; k++) {
|
||||
float A = (*diag_x * (x[k] - fit1_params[1])) + (*offdiag_x * (y[k] - fit1_params[2])) + (*offdiag_y *
|
||||
(z[k] + fit1_params[3]));
|
||||
float B = (*offdiag_x * (x[k] - fit1_params[1])) + (*diag_y * (y[k] - fit1_params[2])) + (*offdiag_z *
|
||||
|
|
@ -358,8 +156,8 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
|
|||
fit2 += residual * residual;
|
||||
}
|
||||
|
||||
fit1 = sqrtf(fit1) / _samples_collected;
|
||||
fit2 = sqrtf(fit2) / _samples_collected;
|
||||
fit1 = sqrtf(fit1) / samples_collected;
|
||||
fit2 = sqrtf(fit2) / samples_collected;
|
||||
|
||||
if (fit1 > _fitness && fit2 > _fitness) {
|
||||
_sphere_lambda *= lma_damping;
|
||||
|
|
@ -389,26 +187,25 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
|
|||
}
|
||||
|
||||
int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
|
||||
unsigned int size, float *offset_x, float *offset_y, float *offset_z,
|
||||
unsigned int samples_collected, float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z)
|
||||
{
|
||||
//Run Sphere Fit using Levenberg Marquardt LSq Fit
|
||||
// Run Sphere Fit using Levenberg Marquardt LSq Fit
|
||||
const float lma_damping = 10.0f;
|
||||
float _samples_collected = size;
|
||||
float fitness = _fitness;
|
||||
float fit1 = 0.0f;
|
||||
float fit2 = 0.0f;
|
||||
|
||||
float JTJ[81] = {};
|
||||
float JTJ2[81] = {};
|
||||
float JTFI[9] = {};
|
||||
float JTJ[81] {};
|
||||
float JTJ2[81] {};
|
||||
float JTFI[9] {};
|
||||
float residual = 0.0f;
|
||||
float ellipsoid_jacob[9];
|
||||
|
||||
// Gauss Newton Part common for all kind of extensions including LM
|
||||
for (uint16_t k = 0; k < _samples_collected; k++) {
|
||||
for (uint16_t k = 0; k < samples_collected; k++) {
|
||||
|
||||
//Calculate Jacobian
|
||||
// Calculate Jacobian
|
||||
float A = (*diag_x * (x[k] - *offset_x)) + (*offdiag_x * (y[k] - *offset_y)) + (*offdiag_y * (z[k] - *offset_z));
|
||||
float B = (*offdiag_x * (x[k] - *offset_x)) + (*diag_y * (y[k] - *offset_y)) + (*offdiag_z * (z[k] - *offset_z));
|
||||
float C = (*offdiag_y * (x[k] - *offset_x)) + (*offdiag_z * (y[k] - *offset_y)) + (*diag_z * (z[k] - *offset_z));
|
||||
|
|
@ -432,7 +229,7 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
|
|||
// compute JTJ
|
||||
for (uint8_t j = 0; j < 9; j++) {
|
||||
JTJ[i * 9 + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
|
||||
JTJ2[i * 9 + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j]; //a backup JTJ for LM
|
||||
JTJ2[i * 9 + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j]; // a backup JTJ for LM
|
||||
}
|
||||
|
||||
JTFI[i] += ellipsoid_jacob[i] * residual;
|
||||
|
|
@ -441,7 +238,7 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
|
|||
|
||||
|
||||
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
|
||||
//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
|
||||
// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
|
||||
float fit1_params[9] = {*offset_x, *offset_y, *offset_z, *diag_x, *diag_y, *diag_z, *offdiag_x, *offdiag_y, *offdiag_z};
|
||||
float fit2_params[9];
|
||||
memcpy(fit2_params, fit1_params, sizeof(fit1_params));
|
||||
|
|
@ -467,8 +264,8 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
|
|||
}
|
||||
}
|
||||
|
||||
//Calculate mean squared residuals
|
||||
for (uint16_t k = 0; k < _samples_collected; k++) {
|
||||
// Calculate mean squared residuals
|
||||
for (uint16_t k = 0; k < samples_collected; k++) {
|
||||
float A = (fit1_params[3] * (x[k] - fit1_params[0])) + (fit1_params[6] * (y[k] - fit1_params[1])) + (fit1_params[7] *
|
||||
(z[k] - fit1_params[2]));
|
||||
float B = (fit1_params[6] * (x[k] - fit1_params[0])) + (fit1_params[4] * (y[k] - fit1_params[1])) + (fit1_params[8] *
|
||||
|
|
@ -490,8 +287,8 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
|
|||
fit2 += residual * residual;
|
||||
}
|
||||
|
||||
fit1 = sqrtf(fit1) / _samples_collected;
|
||||
fit2 = sqrtf(fit2) / _samples_collected;
|
||||
fit1 = sqrtf(fit1) / samples_collected;
|
||||
fit2 = sqrtf(fit2) / samples_collected;
|
||||
|
||||
if (fit1 > _fitness && fit2 > _fitness) {
|
||||
_sphere_lambda *= lma_damping;
|
||||
|
|
|
|||
|
|
@ -48,7 +48,6 @@
|
|||
* @param z point coordinates on the Z axis
|
||||
* @param size number of points
|
||||
* @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
|
||||
* @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
|
||||
* @param sphere_x coordinate of the sphere center on the X axis
|
||||
* @param sphere_y coordinate of the sphere center on the Y axis
|
||||
* @param sphere_z coordinate of the sphere center on the Z axis
|
||||
|
|
@ -56,23 +55,19 @@
|
|||
*
|
||||
* @return 0 on success, 1 on failure
|
||||
*/
|
||||
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
|
||||
float *sphere_radius);
|
||||
int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, int max_iterations, float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
|
||||
float *offdiag_z, bool sphere_fit_only);
|
||||
int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
|
||||
unsigned int size, float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
|
||||
float *offdiag_z);
|
||||
int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
|
||||
unsigned int size, float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
|
||||
float *offdiag_z);
|
||||
bool inverse4x4(float m[], float invOut[]);
|
||||
bool mat_inverse(float *A, float *inv, uint8_t n);
|
||||
int run_lm_sphere_fit(const float x[], const float y[], const float z[],
|
||||
float &fitness, float &sphere_lambda, unsigned int size,
|
||||
float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius,
|
||||
float *diag_x, float *diag_y, float *diag_z,
|
||||
float *offdiag_x, float *offdiag_y, float *offdiag_z);
|
||||
|
||||
int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[],
|
||||
float &fitness, float &sphere_lambda, unsigned int size,
|
||||
float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius,
|
||||
float *diag_x, float *diag_y, float *diag_z,
|
||||
float *offdiag_x, float *offdiag_y, float *offdiag_z);
|
||||
|
||||
// The order of these cannot change since the calibration calculations depend on them in this order
|
||||
enum detect_orientation_return {
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue