mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 14:24:21 +08:00
Refactor LM mag calibration routines
This commit is contained in:
Julian Kent
committed by
Lorenz Meier
Lorenz Meier
parent
a9fc104387
commit
e96e028327
@@ -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)
|
||||
|
||||
@@ -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<float, 4> 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<float, 4> 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<float, 9> 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<float, 9> 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;
|
||||
}
|
||||
|
||||
@@ -34,9 +34,14 @@
|
||||
#pragma once
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
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<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++) {
|
||||
|
||||
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);
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user