diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index ab92265bef..90ddc90b9a 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -237,13 +237,22 @@ int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[ unsigned int size, unsigned int max_iterations, float delta, 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) { - float _fitness = 1.0e30f, _sphere_lambda = 1.0f; + float _fitness = 1.0e30f, _sphere_lambda = 1.0f, _ellipsoid_lambda = 1.0f; for (int i = 0; i < max_iterations; i++) { //printf("%d, offset: %.6f %.6f %.6f %.6f fitness: %.6f\n", i, (double)*offset_x, (double)*offset_y, (double)*offset_z, (double)*sphere_radius, (double)_fitness); 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); + + } + + _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; @@ -263,9 +272,9 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & float JTJ2[16]; float JTFI[4]; float residual = 0.0f; - memset(&JTJ, 0, sizeof(JTJ)); - memset(&JTJ2, 0, sizeof(JTJ2)); - memset(&JTFI, 0, sizeof(JTFI)); + memset(JTJ, 0, sizeof(JTJ)); + memset(JTJ2, 0, sizeof(JTJ2)); + memset(JTFI, 0, sizeof(JTFI)); // Gauss Newton Part common for all kind of extensions including LM for (uint16_t k = 0; k < _samples_collected; k++) { @@ -376,6 +385,364 @@ 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, + 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; + float fitness = _fitness; + float fit1 = 0.0f, fit2 = 0.0f; + + float JTJ[81]; + float JTJ2[81]; + float JTFI[9]; + float residual = 0.0f; + memset(JTJ, 0, sizeof(JTJ)); + memset(JTJ2, 0, sizeof(JTJ2)); + memset(JTFI, 0, sizeof(JTFI)); + float ellipsoid_jacob[9]; + + // Gauss Newton Part common for all kind of extensions including LM + for (uint16_t k = 0; k < _samples_collected; k++) { + + //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)); + float length = sqrtf(A * A + B * B + C * C); + residual = *sphere_radius - length; + fit1 += residual * residual; + // 0-2: partial derivative (offset wrt fitness fn) fn operated on sample + ellipsoid_jacob[0] = -1.0f * (((*diag_x * A) + (*offdiag_x * B) + (*offdiag_y * C)) / length); + ellipsoid_jacob[1] = -1.0f * (((*offdiag_x * A) + (*diag_y * B) + (*offdiag_z * C)) / length); + ellipsoid_jacob[2] = -1.0f * (((*offdiag_y * A) + (*offdiag_z * B) + (*diag_z * C)) / length); + // 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample + ellipsoid_jacob[3] = -1.0f * ((x[k] + *offset_x) * A) / length; + ellipsoid_jacob[4] = -1.0f * ((y[k] + *offset_y) * B) / length; + ellipsoid_jacob[5] = -1.0f * ((z[k] + *offset_z) * C) / length; + // 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample + ellipsoid_jacob[6] = -1.0f * (((y[k] + *offset_y) * A) + ((x[k] + *offset_x) * B)) / length; + ellipsoid_jacob[7] = -1.0f * (((z[k] + *offset_z) * A) + ((x[k] + *offset_x) * C)) / length; + ellipsoid_jacob[8] = -1.0f * (((z[k] + *offset_z) * B) + ((y[k] + *offset_y) * C)) / length; + + for (uint8_t i = 0; i < 9; i++) { + // 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 + } + + JTFI[i] += ellipsoid_jacob[i] * residual; + } + } + + + //------------------------Levenberg-Marquardt-part-starts-here---------------------------------// + //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)); + + for (uint8_t i = 0; i < 9; i++) { + JTJ[i * 9 + i] += _sphere_lambda; + JTJ2[i * 9 + i] += _sphere_lambda / lma_damping; + } + + + if (!mat_inverse(JTJ, JTJ, 9)) { + return -1; + } + + if (!mat_inverse(JTJ2, JTJ2, 9)) { + return -1; + } + + + + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t col = 0; col < 9; col++) { + fit1_params[row] -= JTFI[col] * JTJ[row * 9 + col]; + fit2_params[row] -= JTFI[col] * JTJ2[row * 9 + col]; + } + } + + //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] * + (z[k] + fit1_params[2])); + float C = (fit1_params[7] * (x[k] + fit1_params[0])) + (fit1_params[8] * (y[k] + fit1_params[1])) + (fit1_params[5] * + (z[k] + fit1_params[2])); + float length = sqrtf(A * A + B * B + C * C); + residual = *sphere_radius - length; + fit1 += residual * residual; + + A = (fit2_params[3] * (x[k] + fit2_params[0])) + (fit2_params[6] * (y[k] + fit2_params[1])) + (fit2_params[7] * + (z[k] + fit2_params[2])); + B = (fit2_params[6] * (x[k] + fit2_params[0])) + (fit2_params[4] * (y[k] + fit2_params[1])) + (fit2_params[8] * + (z[k] + fit2_params[2])); + C = (fit2_params[7] * (x[k] + fit2_params[0])) + (fit2_params[8] * (y[k] + fit2_params[1])) + (fit2_params[5] * + (z[k] + fit2_params[2])); + length = sqrtf(A * A + B * B + C * C); + residual = *sphere_radius - length; + fit2 += residual * residual; + } + + fit1 = sqrtf(fit1) / _samples_collected; + fit2 = sqrtf(fit2) / _samples_collected; + + if (fit1 > _fitness && fit2 > _fitness) { + _sphere_lambda *= lma_damping; + + } else if (fit2 < _fitness && fit2 < fit1) { + _sphere_lambda /= lma_damping; + memcpy(fit1_params, fit2_params, sizeof(fit1_params)); + fitness = fit2; + + } else if (fit1 < _fitness) { + fitness = fit1; + } + + //--------------------Levenberg-Marquardt-part-ends-here--------------------------------// + if (!isnan(fitness) && fitness < _fitness) { + _fitness = fitness; + *offset_x = fit1_params[0]; + *offset_y = fit1_params[1]; + *offset_z = fit1_params[2]; + *diag_x = fit1_params[3]; + *diag_y = fit1_params[4]; + *diag_z = fit1_params[5]; + *offdiag_x = fit1_params[6]; + *offdiag_y = fit1_params[7]; + *offdiag_z = fit1_params[8]; + return 0; + + } else { + return -1; + } +} + + +//TODO: use higher precision datatypes to achieve more accuracy for matrix algebra operations + +/* + * Does matrix multiplication of two regular/square matrices + * + * @param A, Matrix A + * @param B, Matrix B + * @param n, dimemsion of square matrices + * @returns multiplied matrix i.e. A*B + */ + +static float *mat_mul(float *A, float *B, uint8_t n) +{ + float *ret = new float[n * n]; + memset(ret, 0.0f, n * n * sizeof(float)); + + for (uint8_t i = 0; i < n; i++) { + for (uint8_t j = 0; j < n; j++) { + for (uint8_t k = 0; k < n; k++) { + ret[i * n + j] += A[i * n + k] * B[k * n + j]; + } + } + } + + return ret; +} + +static inline void swap(float &a, float &b) +{ + float c; + c = a; + a = b; + b = c; +} + +/* + * calculates pivot matrix such that all the larger elements in the row are on diagonal + * + * @param A, input matrix matrix + * @param pivot + * @param n, dimenstion of square matrix + * @returns false = matrix is Singular or non positive definite, true = matrix inversion successful + */ + +static void mat_pivot(float *A, float *pivot, uint8_t n) +{ + for (uint8_t i = 0; i < n; i++) { + for (uint8_t j = 0; j < n; j++) { + pivot[i * n + j] = (i == j); + } + } + + for (uint8_t i = 0; i < n; i++) { + uint8_t max_j = i; + + for (uint8_t j = i; j < n; j++) { + if (fabsf(A[j * n + i]) > fabsf(A[max_j * n + i])) { + max_j = j; + } + } + + if (max_j != i) { + for (uint8_t k = 0; k < n; k++) { + swap(pivot[i * n + k], pivot[max_j * n + k]); + } + } + } +} + +/* + * calculates matrix inverse of Lower trangular matrix using forward substitution + * + * @param L, lower triangular matrix + * @param out, Output inverted lower triangular matrix + * @param n, dimension of matrix + */ + +static void mat_forward_sub(float *L, float *out, uint8_t n) +{ + // Forward substitution solve LY = I + for (int i = 0; i < n; i++) { + out[i * n + i] = 1 / L[i * n + i]; + + for (int j = i + 1; j < n; j++) { + for (int k = i; k < j; k++) { + out[j * n + i] -= L[j * n + k] * out[k * n + i]; + } + + out[j * n + i] /= L[j * n + j]; + } + } +} + +/* + * calculates matrix inverse of Upper trangular matrix using backward substitution + * + * @param U, upper triangular matrix + * @param out, Output inverted upper triangular matrix + * @param n, dimension of matrix + */ + +static void mat_back_sub(float *U, float *out, uint8_t n) +{ + // Backward Substitution solve UY = I + for (int i = n - 1; i >= 0; i--) { + out[i * n + i] = 1 / U[i * n + i]; + + for (int j = i - 1; j >= 0; j--) { + for (int k = i; k > j; k--) { + out[j * n + i] -= U[j * n + k] * out[k * n + i]; + } + + out[j * n + i] /= U[j * n + j]; + } + } +} + +/* + * Decomposes square matrix into Lower and Upper triangular matrices such that + * A*P = L*U, where P is the pivot matrix + * ref: http://rosettacode.org/wiki/LU_decomposition + * @param U, upper triangular matrix + * @param out, Output inverted upper triangular matrix + * @param n, dimension of matrix + */ + +static void mat_LU_decompose(float *A, float *L, float *U, float *P, uint8_t n) +{ + memset(L, 0, n * n * sizeof(float)); + memset(U, 0, n * n * sizeof(float)); + memset(P, 0, n * n * sizeof(float)); + mat_pivot(A, P, n); + + float *APrime = mat_mul(P, A, n); + + for (uint8_t i = 0; i < n; i++) { + L[i * n + i] = 1; + } + + for (uint8_t i = 0; i < n; i++) { + for (uint8_t j = 0; j < n; j++) { + if (j <= i) { + U[j * n + i] = APrime[j * n + i]; + + for (uint8_t k = 0; k < j; k++) { + U[j * n + i] -= L[j * n + k] * U[k * n + i]; + } + } + + if (j >= i) { + L[j * n + i] = APrime[j * n + i]; + + for (uint8_t k = 0; k < i; k++) { + L[j * n + i] -= L[j * n + k] * U[k * n + i]; + } + + L[j * n + i] /= U[i * n + i]; + } + } + } + + delete[] APrime; +} + +/* + * matrix inverse code for any square matrix using LU decomposition + * inv = inv(U)*inv(L)*P, where L and U are triagular matrices and P the pivot matrix + * ref: http://www.cl.cam.ac.uk/teaching/1314/NumMethods/supporting/mcmaster-kiruba-ludecomp.pdf + * @param m, input 4x4 matrix + * @param inv, Output inverted 4x4 matrix + * @param n, dimension of square matrix + * @returns false = matrix is Singular, true = matrix inversion successful + */ +bool mat_inverse(float *A, float *inv, uint8_t n) +{ + float *L, *U, *P; + bool ret = true; + L = new float[n * n]; + U = new float[n * n]; + P = new float[n * n]; + mat_LU_decompose(A, L, U, P, n); + + float *L_inv = new float[n * n]; + float *U_inv = new float[n * n]; + + memset(L_inv, 0, n * n * sizeof(float)); + mat_forward_sub(L, L_inv, n); + + memset(U_inv, 0, n * n * sizeof(float)); + mat_back_sub(U, U_inv, n); + + // decomposed matrices no longer required + delete[] L; + delete[] U; + + float *inv_unpivoted = mat_mul(U_inv, L_inv, n); + float *inv_pivoted = mat_mul(inv_unpivoted, P, n); + + //check sanity of results + for (uint8_t i = 0; i < n; i++) { + for (uint8_t j = 0; j < n; j++) { + if (isnan(inv_pivoted[i * n + j]) || isinf(inv_pivoted[i * n + j])) { + ret = false; + } + } + } + + memcpy(inv, inv_pivoted, n * n * sizeof(float)); + + //free memory + delete[] inv_pivoted; + delete[] inv_unpivoted; + delete[] P; + delete[] U_inv; + delete[] L_inv; + return ret; +} bool inverse4x4(float m[], float invOut[]) { diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 916eed196b..298b13e0d0 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -63,7 +63,13 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float & 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); + // FIXME: Change the name static const unsigned max_accel_sens = 3; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index b41473d8eb..e79e6e98c4 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -635,6 +635,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external"); calibration_log_info(mavlink_log_pub, "Offsets: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)sphere_x[cur_mag], (double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag); + calibration_log_info(mavlink_log_pub, "Scale: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)diag_x[cur_mag], + (double)diag_y[cur_mag], (double)diag_z[cur_mag], cur_mag); + calibration_log_info(mavlink_log_pub, "offdiag: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)offdiag_x[cur_mag], + (double)offdiag_y[cur_mag], (double)offdiag_z[cur_mag], cur_mag); result = calibrate_return_ok; } } @@ -727,6 +731,9 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) mscale.x_offset = sphere_x[cur_mag]; mscale.y_offset = sphere_y[cur_mag]; mscale.z_offset = sphere_z[cur_mag]; + mscale.x_scale = diag_x[cur_mag]; + mscale.y_scale = diag_y[cur_mag]; + mscale.z_scale = diag_z[cur_mag]; #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)