diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 90ddc90b9a..dfeef3ec85 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -240,7 +241,6 @@ int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[ 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); @@ -525,357 +525,6 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa } } - -//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[]) -{ - float inv[16], det; - uint8_t i; - - inv[0] = m[5] * m[10] * m[15] - - m[5] * m[11] * m[14] - - m[9] * m[6] * m[15] + - m[9] * m[7] * m[14] + - m[13] * m[6] * m[11] - - m[13] * m[7] * m[10]; - - inv[4] = -m[4] * m[10] * m[15] + - m[4] * m[11] * m[14] + - m[8] * m[6] * m[15] - - m[8] * m[7] * m[14] - - m[12] * m[6] * m[11] + - m[12] * m[7] * m[10]; - - inv[8] = m[4] * m[9] * m[15] - - m[4] * m[11] * m[13] - - m[8] * m[5] * m[15] + - m[8] * m[7] * m[13] + - m[12] * m[5] * m[11] - - m[12] * m[7] * m[9]; - - inv[12] = -m[4] * m[9] * m[14] + - m[4] * m[10] * m[13] + - m[8] * m[5] * m[14] - - m[8] * m[6] * m[13] - - m[12] * m[5] * m[10] + - m[12] * m[6] * m[9]; - - inv[1] = -m[1] * m[10] * m[15] + - m[1] * m[11] * m[14] + - m[9] * m[2] * m[15] - - m[9] * m[3] * m[14] - - m[13] * m[2] * m[11] + - m[13] * m[3] * m[10]; - - inv[5] = m[0] * m[10] * m[15] - - m[0] * m[11] * m[14] - - m[8] * m[2] * m[15] + - m[8] * m[3] * m[14] + - m[12] * m[2] * m[11] - - m[12] * m[3] * m[10]; - - inv[9] = -m[0] * m[9] * m[15] + - m[0] * m[11] * m[13] + - m[8] * m[1] * m[15] - - m[8] * m[3] * m[13] - - m[12] * m[1] * m[11] + - m[12] * m[3] * m[9]; - - inv[13] = m[0] * m[9] * m[14] - - m[0] * m[10] * m[13] - - m[8] * m[1] * m[14] + - m[8] * m[2] * m[13] + - m[12] * m[1] * m[10] - - m[12] * m[2] * m[9]; - - inv[2] = m[1] * m[6] * m[15] - - m[1] * m[7] * m[14] - - m[5] * m[2] * m[15] + - m[5] * m[3] * m[14] + - m[13] * m[2] * m[7] - - m[13] * m[3] * m[6]; - - inv[6] = -m[0] * m[6] * m[15] + - m[0] * m[7] * m[14] + - m[4] * m[2] * m[15] - - m[4] * m[3] * m[14] - - m[12] * m[2] * m[7] + - m[12] * m[3] * m[6]; - - inv[10] = m[0] * m[5] * m[15] - - m[0] * m[7] * m[13] - - m[4] * m[1] * m[15] + - m[4] * m[3] * m[13] + - m[12] * m[1] * m[7] - - m[12] * m[3] * m[5]; - - inv[14] = -m[0] * m[5] * m[14] + - m[0] * m[6] * m[13] + - m[4] * m[1] * m[14] - - m[4] * m[2] * m[13] - - m[12] * m[1] * m[6] + - m[12] * m[2] * m[5]; - - inv[3] = -m[1] * m[6] * m[11] + - m[1] * m[7] * m[10] + - m[5] * m[2] * m[11] - - m[5] * m[3] * m[10] - - m[9] * m[2] * m[7] + - m[9] * m[3] * m[6]; - - inv[7] = m[0] * m[6] * m[11] - - m[0] * m[7] * m[10] - - m[4] * m[2] * m[11] + - m[4] * m[3] * m[10] + - m[8] * m[2] * m[7] - - m[8] * m[3] * m[6]; - - inv[11] = -m[0] * m[5] * m[11] + - m[0] * m[7] * m[9] + - m[4] * m[1] * m[11] - - m[4] * m[3] * m[9] - - m[8] * m[1] * m[7] + - m[8] * m[3] * m[5]; - - inv[15] = m[0] * m[5] * m[10] - - m[0] * m[6] * m[9] - - m[4] * m[1] * m[10] + - m[4] * m[2] * m[9] + - m[8] * m[1] * m[6] - - m[8] * m[2] * m[5]; - - det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; - - if (fabsf(det) < 1.1755e-38f) { - return false; - } - - det = 1.0f / det; - - for (i = 0; i < 16; i++) { - invOut[i] = inv[i] * det; - } - - return true; -} - enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub, bool lenient_still_position) {