diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 980bd8281e..1273dadd8a 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -48,6 +48,7 @@ px4_add_module( factory_calibration_storage.cpp gyro_calibration.cpp level_calibration.cpp + lm_fit.cpp mag_calibration.cpp rc_calibration.cpp state_machine_helper.cpp @@ -68,4 +69,4 @@ if(PX4_TESTING) add_subdirectory(commander_tests) endif() -px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS mathlib) +px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander) diff --git a/src/modules/commander/lm_fit.cpp b/src/modules/commander/lm_fit.cpp new file mode 100644 index 0000000000..39eb571a79 --- /dev/null +++ b/src/modules/commander/lm_fit.cpp @@ -0,0 +1,356 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "lm_fit.hpp" + +struct iteration_result { + float gradient_damping; + float cost; + enum class STATUS { + SUCCESS, FAILURE + } result = STATUS::SUCCESS; +}; + +void lm_sphere_fit_iteration(const float x[], const float y[], const float z[], + unsigned int samples_collected, sphere_params ¶ms, iteration_result &result) +{ + // Run Sphere Fit using Levenberg Marquardt LSq Fit + const float lma_damping = 10.f; + float fitness = result.cost; + float fit1 = 0.f; + float fit2 = 0.f; + + matrix::SquareMatrix JTJ; + 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++) { + + float sphere_jacob[4]; + //Calculate Jacobian + float A = (params.diag(0) * (x[k] - params.offset(0))) + (params.offdiag(0) * (y[k] - params.offset(1))) + + (params.offdiag(1) * (z[k] - params.offset(2))); + float B = (params.offdiag(0) * (x[k] - params.offset(0))) + (params.diag(1) * (y[k] - params.offset(1))) + + (params.offdiag(2) * (z[k] - params.offset(2))); + float C = (params.offdiag(1) * (x[k] - params.offset(0))) + (params.offdiag(2) * (y[k] - params.offset(1))) + + (params.diag(2) * (z[k] - params.offset(2))); + float length = sqrtf(A * A + B * B + C * C); + + // 0: partial derivative (radius wrt fitness fn) fn operated on sample + sphere_jacob[0] = 1.0f; + // 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample + sphere_jacob[1] = 1.0f * (((params.diag(0) * A) + (params.offdiag(0) * B) + (params.offdiag(1) * C)) / length); + sphere_jacob[2] = 1.0f * (((params.offdiag(0) * A) + (params.diag(1) * B) + (params.offdiag(2) * C)) / length); + sphere_jacob[3] = 1.0f * (((params.offdiag(1) * A) + (params.offdiag(2) * B) + (params.diag(2) * C)) / length); + residual = params.radius - length; + + for (uint8_t i = 0; i < 4; i++) { + // compute JTJ + for (uint8_t j = 0; j < 4; j++) { + JTJ(i, j) += sphere_jacob[i] * sphere_jacob[j]; + } + + JTFI[i] += sphere_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[4] = {params.radius, params.offset(0), params.offset(1), params.offset(2)}; + float fit2_params[4]; + memcpy(fit2_params, fit1_params, sizeof(fit1_params)); + JTJ = (JTJ + JTJ.transpose()) * 0.5f; // fix numerical issues + matrix::SquareMatrix JTJ2 = JTJ; + + for (uint8_t i = 0; i < 4; i++) { + JTJ(i, i) += result.gradient_damping; + JTJ2(i, i) += result.gradient_damping / lma_damping; + } + + if (!JTJ.I(JTJ)) { + result.result = iteration_result::STATUS::FAILURE; + return; + } + + if (!JTJ2.I(JTJ2)) { + result.result = iteration_result::STATUS::FAILURE; + return; + + } + + for (uint8_t row = 0; row < 4; row++) { + for (uint8_t col = 0; col < 4; col++) { + fit1_params[row] -= JTFI[col] * JTJ(row, col); + fit2_params[row] -= JTFI[col] * JTJ2(row, col); + } + } + + // Calculate mean squared residuals + for (uint16_t k = 0; k < samples_collected; k++) { + float A = (params.diag(0) * (x[k] - fit1_params[1])) + (params.offdiag(0) * (y[k] - fit1_params[2])) + + (params.offdiag(1) * + (z[k] + fit1_params[3])); + float B = (params.offdiag(0) * (x[k] - fit1_params[1])) + (params.diag(1) * (y[k] - fit1_params[2])) + + (params.offdiag(2) * + (z[k] + fit1_params[3])); + float C = (params.offdiag(1) * (x[k] - fit1_params[1])) + (params.offdiag(2) * (y[k] - fit1_params[2])) + (params.diag( + 2) * + (z[k] - fit1_params[3])); + float length = sqrtf(A * A + B * B + C * C); + residual = fit1_params[0] - length; + fit1 += residual * residual; + + A = (params.diag(0) * (x[k] - fit2_params[1])) + (params.offdiag(0) * (y[k] - fit2_params[2])) + (params.offdiag(1) * + (z[k] - fit2_params[3])); + B = (params.offdiag(0) * (x[k] - fit2_params[1])) + (params.diag(1) * (y[k] - fit2_params[2])) + (params.offdiag(2) * + (z[k] - fit2_params[3])); + C = (params.offdiag(1) * (x[k] - fit2_params[1])) + (params.offdiag(2) * (y[k] - fit2_params[2])) + (params.diag(2) * + (z[k] - fit2_params[3])); + length = sqrtf(A * A + B * B + C * C); + residual = fit2_params[0] - length; + fit2 += residual * residual; + } + + fit1 = sqrtf(fit1) / samples_collected; + fit2 = sqrtf(fit2) / samples_collected; + + if (fit1 > result.cost && fit2 > result.cost) { + result.gradient_damping *= lma_damping; + + } else if (fit2 < result.cost && fit2 < fit1) { + result.gradient_damping /= lma_damping; + memcpy(fit1_params, fit2_params, sizeof(fit1_params)); + fitness = fit2; + + } else if (fit1 < result.cost) { + fitness = fit1; + } + + //--------------------Levenberg-Marquardt-part-ends-here--------------------------------// + + if (PX4_ISFINITE(fitness) && fitness <= result.cost) { + result.cost = fitness; + params.radius = fit1_params[0]; + params.offset(0) = fit1_params[1]; + params.offset(1) = fit1_params[2]; + params.offset(2) = fit1_params[3]; + result.result = iteration_result::STATUS::SUCCESS; + + } else { + result.result = iteration_result::STATUS::FAILURE; + } +} + +void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[], + unsigned int samples_collected, sphere_params ¶ms, iteration_result &result) +{ + // Run Sphere Fit using Levenberg Marquardt LSq Fit + const float lma_damping = 10.0f; + float fitness = result.cost; + float fit1 = 0.0f; + float fit2 = 0.0f; + + matrix::SquareMatrix JTJ; + 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++) { + + // Calculate Jacobian + float A = (params.diag(0) * (x[k] - params.offset(0))) + (params.offdiag(0) * (y[k] - params.offset(1))) + + (params.offdiag(1) * (z[k] - params.offset(2))); + float B = (params.offdiag(0) * (x[k] - params.offset(0))) + (params.diag(1) * (y[k] - params.offset(1))) + + (params.offdiag(2) * (z[k] - params.offset(2))); + float C = (params.offdiag(1) * (x[k] - params.offset(0))) + (params.offdiag(2) * (y[k] - params.offset(1))) + + (params.diag(2) * (z[k] - params.offset(2))); + float length = sqrtf(A * A + B * B + C * C); + residual = params.radius - length; + fit1 += residual * residual; + // 0-2: partial derivative (offset wrt fitness fn) fn operated on sample + ellipsoid_jacob[0] = 1.0f * (((params.diag(0) * A) + (params.offdiag(0) * B) + (params.offdiag(1) * C)) / length); + ellipsoid_jacob[1] = 1.0f * (((params.offdiag(0) * A) + (params.diag(1) * B) + (params.offdiag(2) * C)) / length); + ellipsoid_jacob[2] = 1.0f * (((params.offdiag(1) * A) + (params.offdiag(2) * B) + (params.diag(2) * C)) / length); + // 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample + ellipsoid_jacob[3] = -1.0f * ((x[k] - params.offset(0)) * A) / length; + ellipsoid_jacob[4] = -1.0f * ((y[k] - params.offset(1)) * B) / length; + ellipsoid_jacob[5] = -1.0f * ((z[k] - params.offset(2)) * C) / length; + // 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample + ellipsoid_jacob[6] = -1.0f * (((y[k] - params.offset(1)) * A) + ((x[k] - params.offset(0)) * B)) / length; + ellipsoid_jacob[7] = -1.0f * (((z[k] - params.offset(2)) * A) + ((x[k] - params.offset(0)) * C)) / length; + ellipsoid_jacob[8] = -1.0f * (((z[k] - params.offset(2)) * B) + ((y[k] - params.offset(1)) * C)) / length; + + for (uint8_t i = 0; i < 9; i++) { + // compute JTJ + for (uint8_t j = 0; j < 9; j++) { + JTJ(i, j) += ellipsoid_jacob[i] * ellipsoid_jacob[j]; + } + + 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] = {params.offset(0), params.offset(1), params.offset(2), params.diag(0), params.diag(1), params.diag(2), params.offdiag(0), params.offdiag(1), params.offdiag(2)}; + float fit2_params[9]; + memcpy(fit2_params, fit1_params, sizeof(fit1_params)); + matrix::SquareMatrix JTJ2 = JTJ; + + for (uint8_t i = 0; i < 9; i++) { + JTJ(i, i) += result.gradient_damping; + JTJ2(i, i) += result.gradient_damping / lma_damping; + } + + + if (!JTJ.I(JTJ)) { + result.result = iteration_result::STATUS::FAILURE; + return; + } + + if (!JTJ2.I(JTJ2)) { + result.result = iteration_result::STATUS::FAILURE; + return; + } + + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t col = 0; col < 9; col++) { + fit1_params[row] -= JTFI[col] * JTJ(row, col); + fit2_params[row] -= JTFI[col] * JTJ2(row, 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 = params.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 = params.radius - length; + fit2 += residual * residual; + } + + fit1 = sqrtf(fit1) / samples_collected; + fit2 = sqrtf(fit2) / samples_collected; + + if (fit1 > result.cost && fit2 > result.cost) { + result.gradient_damping *= lma_damping; + + } else if (fit2 < result.cost && fit2 < fit1) { + result.gradient_damping /= lma_damping; + memcpy(fit1_params, fit2_params, sizeof(fit1_params)); + fitness = fit2; + + } else if (fit1 < result.cost) { + fitness = fit1; + } + + //--------------------Levenberg-Marquardt-part-ends-here--------------------------------// + if (PX4_ISFINITE(fitness) && fitness <= result.cost) { + result.cost = fitness; + params.offset(0) = fit1_params[0]; + params.offset(1) = fit1_params[1]; + params.offset(2) = fit1_params[2]; + params.diag(0) = fit1_params[3]; + params.diag(1) = fit1_params[4]; + params.diag(2) = fit1_params[5]; + params.offdiag(0) = fit1_params[6]; + params.offdiag(1) = fit1_params[7]; + params.offdiag(2) = fit1_params[8]; + result.result = iteration_result::STATUS::SUCCESS; + + } else { + result.result = iteration_result::STATUS::FAILURE; + } +} + + + +int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params ¶ms, + bool full_ellipsoid) +{ + + const int max_iterations = 100; + const int min_iterations = 10; + const float cost_threshold = 0.01; + const float step_threshold = 0.001; + + const float min_radius = 0.2; + const float max_radius = 0.7; + + iteration_result iter; + iter.cost = 1e30f; + iter.gradient_damping = 1; + + bool success = false; + + for (int i = 0; i < max_iterations; i++) { + if (full_ellipsoid) { + lm_ellipsoid_fit_iteration(x, y, z, samples_collected, params, iter); + + } else { + lm_sphere_fit_iteration(x, y, z, samples_collected, params, iter); + } + + if (iter.result == iteration_result::STATUS::SUCCESS + && min_radius < params.radius && params.radius < max_radius + && i > min_iterations && (iter.cost < cost_threshold || iter.gradient_damping < step_threshold)) { + success = true; + break; + } + } + + if (success) { + return PX4_OK; + } + + return 1; +} + diff --git a/src/modules/commander/lm_fit.hpp b/src/modules/commander/lm_fit.hpp index 09ee20b880..52fa674d02 100644 --- a/src/modules/commander/lm_fit.hpp +++ b/src/modules/commander/lm_fit.hpp @@ -34,9 +34,14 @@ #pragma once #include -#include #include +struct sphere_params { + matrix::Vector3f offset, diag{1.f, 1.f, 1.f}, offdiag; + float radius{0.2f}; +}; + + /** * Least-squares fit of a sphere to a set of points. * @@ -45,270 +50,14 @@ * @param x point coordinates on the X axis * @param y point coordinates on the Y axis * @param z point coordinates on the Z axis - * @param size number of points + * @param samples_collected number of points * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100. - * @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 - * @param sphere_radius sphere radius + * @param params the values to be optimized + * @param full_ellipsoid whether to just optimize a sphere, or do an ellipsoid optimization + * + * NB!! If you optimize the full ellipsoid, you must have already optimized without the full ellipsoid * * @return 0 on success, 1 on failure */ -inline int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, - 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.f; - float fitness = _fitness; - float fit1 = 0.f; - float fit2 = 0.f; - - matrix::SquareMatrix JTJ{}; - matrix::SquareMatrix 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++) { - - float sphere_jacob[4]; - //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); - - // 0: partial derivative (radius wrt fitness fn) fn operated on sample - sphere_jacob[0] = 1.0f; - // 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample - sphere_jacob[1] = 1.0f * (((*diag_x * A) + (*offdiag_x * B) + (*offdiag_y * C)) / length); - sphere_jacob[2] = 1.0f * (((*offdiag_x * A) + (*diag_y * B) + (*offdiag_z * C)) / length); - sphere_jacob[3] = 1.0f * (((*offdiag_y * A) + (*offdiag_z * B) + (*diag_z * C)) / length); - residual = *sphere_radius - length; - - for (uint8_t i = 0; i < 4; i++) { - // compute JTJ - for (uint8_t j = 0; j < 4; j++) { - JTJ(i, j) += sphere_jacob[i] * sphere_jacob[j]; - JTJ2(i, j) += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM - } - - JTFI[i] += sphere_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[4] = {*sphere_radius, *offset_x, *offset_y, *offset_z}; - float fit2_params[4]; - memcpy(fit2_params, fit1_params, sizeof(fit1_params)); - - for (uint8_t i = 0; i < 4; i++) { - JTJ(i, i) += _sphere_lambda; - JTJ2(i, i) += _sphere_lambda / lma_damping; - } - - if (!JTJ.I(JTJ)) { - return -1; - } - - if (!JTJ2.I(JTJ2)) { - return -1; - } - - for (uint8_t row = 0; row < 4; row++) { - for (uint8_t col = 0; col < 4; col++) { - fit1_params[row] -= JTFI[col] * JTJ(row, col); - fit2_params[row] -= JTFI[col] * JTJ2(row, col); - } - } - - // 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 * - (z[k] + fit1_params[3])); - float C = (*offdiag_y * (x[k] - fit1_params[1])) + (*offdiag_z * (y[k] - fit1_params[2])) + (*diag_z * - (z[k] - fit1_params[3])); - float length = sqrtf(A * A + B * B + C * C); - residual = fit1_params[0] - length; - fit1 += residual * residual; - - A = (*diag_x * (x[k] - fit2_params[1])) + (*offdiag_x * (y[k] - fit2_params[2])) + (*offdiag_y * - (z[k] - fit2_params[3])); - B = (*offdiag_x * (x[k] - fit2_params[1])) + (*diag_y * (y[k] - fit2_params[2])) + (*offdiag_z * - (z[k] - fit2_params[3])); - C = (*offdiag_y * (x[k] - fit2_params[1])) + (*offdiag_z * (y[k] - fit2_params[2])) + (*diag_z * - (z[k] - fit2_params[3])); - length = sqrtf(A * A + B * B + C * C); - residual = fit2_params[0] - 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 (PX4_ISFINITE(fitness) && fitness < _fitness) { - _fitness = fitness; - *sphere_radius = fit1_params[0]; - *offset_x = fit1_params[1]; - *offset_y = fit1_params[2]; - *offset_z = fit1_params[3]; - return 0; - - } else { - return -1; - } -} - -inline int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &_fitness, - float &_sphere_lambda, 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 fitness = _fitness; - float fit1 = 0.0f; - float fit2 = 0.0f; - - 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++) { - - // 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 (PX4_ISFINITE(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; - } -} +int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params ¶ms, + bool full_ellipsoid); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 033413ccf8..dac72a7269 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -570,74 +570,38 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma // enough to reliably estimate both scales and offsets with 2 sides only (even if the existing calibration // is already close) bool sphere_fit_only = worker_data.calibration_sides <= 2; + + sphere_params sphere_data; + sphere_data.radius = sphere_radius[cur_mag]; + sphere_data.offset = matrix::Vector3f(sphere[cur_mag](0), sphere[cur_mag](1), sphere[cur_mag](2)); + sphere_data.diag = matrix::Vector3f(diag[cur_mag](0), diag[cur_mag](1), diag[cur_mag](2)); + sphere_data.offdiag = matrix::Vector3f(offdiag[cur_mag](0), offdiag[cur_mag](1), offdiag[cur_mag](2)); + bool sphere_fit_success = false; - { - float fitness = 1.0e30f; - float sphere_lambda = 1.0f; - static constexpr int max_iterations = 100; + bool ellipsoid_fit_success = false; + int ret = lm_mag_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], + worker_data.calibration_counter_total[cur_mag], sphere_data, false); - for (int i = 0; i < max_iterations; i++) { - int ret = run_lm_sphere_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], - fitness, sphere_lambda, worker_data.calibration_counter_total[cur_mag], - &sphere[cur_mag](0), &sphere[cur_mag](1), &sphere[cur_mag](2), - &sphere_radius[cur_mag], - &diag[cur_mag](0), &diag[cur_mag](1), &diag[cur_mag](2), - &offdiag[cur_mag](0), &offdiag[cur_mag](1), &offdiag[cur_mag](2)); + if (ret == PX4_OK) { + sphere_fit_success = true; + PX4_INFO("Mag: %d sphere radius: %.4f", cur_mag, (double)sphere_data.radius); - if (ret == PX4_OK) { - sphere_fit_success = true; + if (!sphere_fit_only) { + int ellipsoid_ret = lm_mag_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], + worker_data.calibration_counter_total[cur_mag], sphere_data, true); - } else { - // stop here if fit has previously succeeded an if it is good enough - // TODO: extract those conditions for the unit test - if (sphere_fit_success - && (i > 10) - && (fitness < 0.01f) - && (sphere_radius[cur_mag] >= 0.2f) - && (sphere_radius[cur_mag] <= 0.7f)) { - break; - } + if (ellipsoid_ret == PX4_OK) { + ellipsoid_fit_success = true; } - - PX4_DEBUG("Mag: %d (%d/%d) sphere fit ret=%d, fitness: %.5f, lambda: %.5f, radius: %.3f, offset: [%.3f, %.3f %.3f]", - cur_mag, i, max_iterations, ret, (double)fitness, (double)sphere_lambda, (double)sphere_radius[cur_mag], - (double)sphere[cur_mag](0), (double)sphere[cur_mag](1), (double)sphere[cur_mag](2)); } - - PX4_INFO("Mag: %d sphere fitness: %.5f radius: %.4f", cur_mag, (double)fitness, (double)sphere_radius[cur_mag]); } - bool ellipsoid_fit_success = false; + sphere_radius[cur_mag] = sphere_data.radius; - if (!sphere_fit_only) { - float fitness = 1.0e30f; - float ellipsoid_lambda = 1.0f; - static constexpr int max_iterations = 100; - - for (int i = 0; i < max_iterations; i++) { - int ret = run_lm_ellipsoid_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], - fitness, ellipsoid_lambda, worker_data.calibration_counter_total[cur_mag], - &sphere[cur_mag](0), &sphere[cur_mag](1), &sphere[cur_mag](2), - &sphere_radius[cur_mag], - &diag[cur_mag](0), &diag[cur_mag](1), &diag[cur_mag](2), - &offdiag[cur_mag](0), &offdiag[cur_mag](1), &offdiag[cur_mag](2)); - - if (ret == PX4_OK) { - ellipsoid_fit_success = true; - - } else { - // stop here if fit has previously succeeded - if (ellipsoid_fit_success) { - break; - } - } - - PX4_DEBUG("Mag: %d (%d/%d) ellipsoid fit ret=%d, fitness: %.5f, lambda: %.5f, radius: %.3f, offset: [%.3f, %.3f %.3f]", - cur_mag, i, max_iterations, ret, (double)fitness, (double)ellipsoid_lambda, (double)sphere_radius[cur_mag], - (double)sphere[cur_mag](0), (double)sphere[cur_mag](1), (double)sphere[cur_mag](2)); - } - - PX4_INFO("Mag: %d ellipsoid fitness: %.5f", cur_mag, (double)fitness); + for (int i = 0; i < 3; i++) { + sphere[cur_mag](i) = sphere_data.offset(i); + diag[cur_mag](i) = sphere_data.diag(i); + offdiag[cur_mag](i) = sphere_data.offdiag(i); } if (!sphere_fit_success && !ellipsoid_fit_success) { diff --git a/src/modules/commander/mag_calibration_test.cpp b/src/modules/commander/mag_calibration_test.cpp index d2cd29ba98..3343a28cd3 100644 --- a/src/modules/commander/mag_calibration_test.cpp +++ b/src/modules/commander/mag_calibration_test.cpp @@ -144,31 +144,21 @@ TEST_F(MagCalTest, sphere2Sides) generate2SidesMagData(x, y, z, N_SAMPLES, mag_str_true); - float fitness = 1.0e30f; - float sphere_lambda = 1.f; - float sphere_radius = 0.2f; - Vector3f offset; - Vector3f diag = {1.f, 1.f, 1.f}; - Vector3f offdiag; - // WHEN: fitting a sphere with the data and given a wrong initial radius - int ret = run_lm_sphere_fit(x, y, z, - fitness, sphere_lambda, N_SAMPLES, - &offset(0), &offset(1), &offset(2), - &sphere_radius, - &diag(0), &diag(1), &diag(2), - &offdiag(0), &offdiag(1), &offdiag(2)); + sphere_params sphere; + sphere.diag = {1.f, 1.f, 1.f}; + sphere.radius = 0.2; + int success = lm_mag_fit(x, y, z, N_SAMPLES, sphere, false); // THEN: the algorithm should converge in a single step - EXPECT_EQ(ret, 0); - EXPECT_LT(fitness, 1e-5f); - EXPECT_NEAR(sphere_radius, mag_str_true, 0.001f) << "radius: " << sphere_radius; - EXPECT_NEAR(offset(0), offset_true(0), 0.001f) << "offset X: " << offset(0); - EXPECT_NEAR(offset(1), offset_true(1), 0.001f) << "offset Y: " << offset(1); - EXPECT_NEAR(offset(2), offset_true(2), 0.001f) << "offset Z: " << offset(2); - EXPECT_NEAR(diag(0), scale_true(0), 0.001f) << "scale X: " << scale_true(0); - EXPECT_NEAR(diag(1), scale_true(1), 0.001f) << "scale Y: " << scale_true(1); - EXPECT_NEAR(diag(2), scale_true(2), 0.001f) << "scale Z: " << scale_true(2); + EXPECT_EQ(success, PX4_OK); + EXPECT_NEAR(sphere.radius, mag_str_true, 0.001f) << "radius: " << sphere.radius; + EXPECT_NEAR(sphere.offset(0), offset_true(0), 0.001f) << "offset X: " << sphere.offset(0); + EXPECT_NEAR(sphere.offset(1), offset_true(1), 0.001f) << "offset Y: " << sphere.offset(1); + EXPECT_NEAR(sphere.offset(2), offset_true(2), 0.001f) << "offset Z: " << sphere.offset(2); + EXPECT_NEAR(sphere.diag(0), scale_true(0), 0.001f) << "scale X: " << sphere.diag(0); + EXPECT_NEAR(sphere.diag(1), scale_true(1), 0.001f) << "scale Y: " << sphere.diag(1); + EXPECT_NEAR(sphere.diag(2), scale_true(2), 0.001f) << "scale Z: " << sphere.diag(2); } TEST_F(MagCalTest, sphereRegularlySpaced) @@ -187,43 +177,22 @@ TEST_F(MagCalTest, sphereRegularlySpaced) generateRegularData(x, y, z, N_SAMPLES, mag_str_true); modifyOffsetScale(x, y, z, N_SAMPLES, offset_true, scale_true); - float fitness = 1.0e30f; - float sphere_lambda = 1.f; - float sphere_radius = 0.2f; - Vector3f offset; - Vector3f diag = {1.f, 1.f, 1.f}; - Vector3f offdiag; - - bool sphere_fit_success = false; - // WHEN: fitting a sphere to the data - for (int i = 0; i < 8; i++) { - const bool ret = run_lm_sphere_fit(x, y, z, - fitness, sphere_lambda, N_SAMPLES, - &offset(0), &offset(1), &offset(2), - &sphere_radius, - &diag(0), &diag(1), &diag(2), - &offdiag(0), &offdiag(1), &offdiag(2)); - - if (ret == 0) { - sphere_fit_success = true; - - } else if (sphere_fit_success) { - break; - } - } + sphere_params sphere; + sphere.diag = {1.f, 1.f, 1.f}; + sphere.radius = 0.2; + int success = lm_mag_fit(x, y, z, N_SAMPLES, sphere, false); // THEN: the algorithm should converge in a few iterations and // find the correct parameters - EXPECT_TRUE(sphere_fit_success); - EXPECT_LT(fitness, 1e-6f); - EXPECT_NEAR(sphere_radius, mag_str_true, 0.001f) << "radius: " << sphere_radius; - EXPECT_NEAR(offset(0), offset_true(0), 0.001f) << "offset X: " << offset(0); - EXPECT_NEAR(offset(1), offset_true(1), 0.001f) << "offset Y: " << offset(1); - EXPECT_NEAR(offset(2), offset_true(2), 0.001f) << "offset Z: " << offset(2); - EXPECT_NEAR(diag(0), scale_true(0), 0.001f) << "scale X: " << scale_true(0); - EXPECT_NEAR(diag(1), scale_true(1), 0.001f) << "scale Y: " << scale_true(1); - EXPECT_NEAR(diag(2), scale_true(2), 0.001f) << "scale Z: " << scale_true(2); + EXPECT_EQ(success, PX4_OK); + EXPECT_NEAR(sphere.radius, mag_str_true, 0.001f) << "radius: " << sphere.radius; + EXPECT_NEAR(sphere.offset(0), offset_true(0), 0.001f) << "offset X: " << sphere.offset(0); + EXPECT_NEAR(sphere.offset(1), offset_true(1), 0.001f) << "offset Y: " << sphere.offset(1); + EXPECT_NEAR(sphere.offset(2), offset_true(2), 0.001f) << "offset Z: " << sphere.offset(2); + EXPECT_NEAR(sphere.diag(0), scale_true(0), 0.001f) << "scale X: " << scale_true(0); + EXPECT_NEAR(sphere.diag(1), scale_true(1), 0.001f) << "scale Y: " << scale_true(1); + EXPECT_NEAR(sphere.diag(2), scale_true(2), 0.001f) << "scale Z: " << scale_true(2); } TEST_F(MagCalTest, replayTestData) @@ -235,76 +204,35 @@ TEST_F(MagCalTest, replayTestData) const float mag_str_true = 0.4f; const Vector3f offset_true = {-0.18f, 0.05f, -0.58f}; - const Vector3f scale_true = {1.f, 1.06f, 0.94f}; - - float fitness = 1.0e30f; - float sphere_lambda = 1.f; - float sphere_radius = 0.2f; - Vector3f offset; - Vector3f diag = {1.f, 1.f, 1.f}; - Vector3f offdiag; - - bool sphere_fit_success = false; // WHEN: fitting a sphere to the data - for (int i = 0; i < 100; i++) { - const bool ret = run_lm_sphere_fit(mag_data1_x, mag_data1_y, mag_data1_z, - fitness, sphere_lambda, N_SAMPLES, - &offset(0), &offset(1), &offset(2), - &sphere_radius, - &diag(0), &diag(1), &diag(2), - &offdiag(0), &offdiag(1), &offdiag(2)); - - printf("fitness: %.6f\t sphere_lambda: %.3f\t radius: %.3f\n", - (double)fitness, (double)sphere_lambda, (double)sphere_radius); - - // This is fragile because it is a copy of the code and not a - // test of the code itself. TODO: move the check in a function that - // can be tested here - if (ret == 0) { - - sphere_fit_success = true; - - } else if (sphere_fit_success - && (i > 10) - && (fitness < 0.01f) - && (sphere_radius >= 0.2f) - && (sphere_radius <= 0.7f)) { - break; - } - } - - printf("Ellipsoid fit\n"); - bool ellipsoid_fit_success = false; - - for (int i = 0; i < 100; i++) { - const bool ret = run_lm_ellipsoid_fit(mag_data1_x, mag_data1_y, mag_data1_z, - fitness, sphere_lambda, N_SAMPLES, - &offset(0), &offset(1), &offset(2), - &sphere_radius, - &diag(0), &diag(1), &diag(2), - &offdiag(0), &offdiag(1), &offdiag(2)); - - printf("fitness: %.6f\t sphere_lambda: %.3f\t radius: %.3f\n", - (double)fitness, (double)sphere_lambda, (double)sphere_radius); - - if (ret == 0) { - ellipsoid_fit_success = true; - - } else if (ellipsoid_fit_success) { - break; - } - } + sphere_params sphere; + sphere.diag = {1.f, 1.f, 1.f}; + sphere.radius = 0.2; + int sphere_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, sphere, false); // THEN: the algorithm should converge and find the correct parameters - EXPECT_TRUE(sphere_fit_success); - EXPECT_TRUE(ellipsoid_fit_success); - EXPECT_LT(fitness, 1e-3f); - EXPECT_NEAR(sphere_radius, mag_str_true, 0.1f) << "radius: " << sphere_radius; - EXPECT_NEAR(offset(0), offset_true(0), 0.01f) << "offset X: " << offset(0); - EXPECT_NEAR(offset(1), offset_true(1), 0.01f) << "offset Y: " << offset(1); - EXPECT_NEAR(offset(2), offset_true(2), 0.01f) << "offset Z: " << offset(2); - EXPECT_NEAR(diag(0), scale_true(0), 0.01f) << "scale X: " << diag(0); - EXPECT_NEAR(diag(1), scale_true(1), 0.01f) << "scale Y: " << diag(1); - EXPECT_NEAR(diag(2), scale_true(2), 0.01f) << "scale Z: " << diag(2); + EXPECT_EQ(sphere_success, PX4_OK); + EXPECT_NEAR(sphere.radius, mag_str_true, 0.1f) << "radius: " << sphere.radius; + EXPECT_NEAR(sphere.offset(0), offset_true(0), 0.01f) << "offset X: " << sphere.offset(0); + EXPECT_NEAR(sphere.offset(1), offset_true(1), 0.01f) << "offset Y: " << sphere.offset(1); + EXPECT_NEAR(sphere.offset(2), offset_true(2), 0.01f) << "offset Z: " << sphere.offset(2); + + printf("Ellipsoid fit\n"); + sphere_params ellipsoid; + ellipsoid.diag = {1.f, 1.f, 1.f}; + ellipsoid.radius = 0.2; + int ellipsoid_step_1_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, ellipsoid, false); + int ellipsoid_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, ellipsoid, true); + const Vector3f scale_true = {1.f, 1.06f, 0.94f}; + + EXPECT_EQ(ellipsoid_step_1_success, PX4_OK); + EXPECT_EQ(ellipsoid_success, PX4_OK); + EXPECT_NEAR(ellipsoid.radius, mag_str_true, 0.1f) << "radius: " << sphere.radius; + EXPECT_NEAR(ellipsoid.offset(0), offset_true(0), 0.01f) << "offset X: " << ellipsoid.offset(0); + EXPECT_NEAR(ellipsoid.offset(1), offset_true(1), 0.01f) << "offset Y: " << ellipsoid.offset(1); + EXPECT_NEAR(ellipsoid.offset(2), offset_true(2), 0.01f) << "offset Z: " << ellipsoid.offset(2); + EXPECT_NEAR(ellipsoid.diag(0), scale_true(0), 0.01f) << "scale X: " << ellipsoid.diag(0); + EXPECT_NEAR(ellipsoid.diag(1), scale_true(1), 0.01f) << "scale Y: " << ellipsoid.diag(1); + EXPECT_NEAR(ellipsoid.diag(2), scale_true(2), 0.01f) << "scale Z: " << ellipsoid.diag(2); }