mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-24 07:09:48 +08:00
update accelerometer calibration to use Levenberg Marquardt least squares fit
- update accel cal to use Levenberg Marquardt least squares fit (same as magnetometer)
- this is more robust to user positioning error as it only requires a nice distribution of points and doesn't actually depend on the user holding the vehicle in each orientation (although the calibration flow is still structured this way)
- use online Welford mean to gather clean readings per orientation
- reset if movement during per side data gathering
- detect invalid rotations (and soon auto correct like mag)
- add `CAL_ACC_SIDES` (analogous to CAL_MAG_SIDES) to limit accel calibration to a subset of orientations (when full set is difficult or impossible)
- accel quick calibration (commander calibrate accel quick) assume vehicle is aligned with gravity and adjust offsets immediately
This commit is contained in:
Daniel Agar
@@ -76,6 +76,7 @@ public:
|
||||
const matrix::Dcmf &rotation() const { return _rotation; }
|
||||
const Rotation &rotation_enum() const { return _rotation_enum; }
|
||||
const matrix::Vector3f &scale() const { return _scale; }
|
||||
const matrix::Vector3f &thermal_offset() const { return _thermal_offset; }
|
||||
|
||||
// apply offsets and scale
|
||||
// rotate corrected measurements from sensor to body frame
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -63,17 +63,16 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, bool lenient_still_position)
|
||||
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub)
|
||||
{
|
||||
static constexpr unsigned ndim = 3;
|
||||
|
||||
float accel_ema[ndim] {}; // exponential moving average of accel
|
||||
float accel_disp[3] {}; // max-hold dispersion of accel
|
||||
static constexpr float ema_len = 0.5f; // EMA time constant in seconds
|
||||
static constexpr float normal_still_thr = 0.25; // normal still threshold
|
||||
float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
|
||||
float still_thr2 = powf(0.25f * 3, 2);
|
||||
static constexpr float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2
|
||||
const hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us
|
||||
static constexpr hrt_abstime still_time = 500000;
|
||||
|
||||
/* set timeout to 90s */
|
||||
static constexpr hrt_abstime timeout = 90_s;
|
||||
@@ -219,7 +218,7 @@ const char *detect_orientation_str(enum detect_orientation_return orientation)
|
||||
|
||||
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
|
||||
bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker,
|
||||
void *worker_data, bool lenient_still_position)
|
||||
void *worker_data)
|
||||
{
|
||||
const hrt_abstime calibration_started = hrt_absolute_time();
|
||||
calibrate_return result = calibrate_return_ok;
|
||||
@@ -268,7 +267,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
|
||||
px4_usleep(20000);
|
||||
calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
|
||||
px4_usleep(20000);
|
||||
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, lenient_still_position);
|
||||
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub);
|
||||
|
||||
if (orient == ORIENTATION_ERROR) {
|
||||
orientation_failures++;
|
||||
|
||||
@@ -54,8 +54,7 @@ static constexpr unsigned detect_orientation_side_count = 6;
|
||||
/// Wait for vehicle to become still and detect it's orientation
|
||||
/// @return Returns detect_orientation_return according to orientation when vehicle
|
||||
/// and ready for measurements
|
||||
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
|
||||
bool lenient_still_detection); ///< true: Use more lenient still position detection
|
||||
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub);
|
||||
|
||||
/// Returns the human readable string representation of the orientation
|
||||
/// @param orientation Orientation to return string for, "error" if buffer is too small
|
||||
@@ -67,17 +66,15 @@ enum calibrate_return {
|
||||
calibrate_return_cancelled
|
||||
};
|
||||
|
||||
typedef calibrate_return(*calibration_from_orientation_worker_t)(detect_orientation_return
|
||||
orientation, ///< Orientation which was detected
|
||||
void *worker_data); ///< Opaque worker data
|
||||
typedef calibrate_return(*calibration_from_orientation_worker_t)(detect_orientation_return orientation,
|
||||
void *worker_data);
|
||||
|
||||
/// Perform calibration sequence which require a rest orientation detection prior to calibration.
|
||||
/// @return OK: Calibration succeeded, ERROR: Calibration failed
|
||||
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
|
||||
bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration
|
||||
calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration
|
||||
void *worker_data, ///< Opaque data passed to worker routine
|
||||
bool lenient_still_detection); ///< true: Use more lenient still position detection
|
||||
void *worker_data); ///< Opaque data passed to worker routine
|
||||
|
||||
/// Used to periodically check for a cancel command
|
||||
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started);
|
||||
|
||||
+141
-133
@@ -41,43 +41,43 @@ struct iteration_result {
|
||||
} 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)
|
||||
static void lm_sphere_fit_iteration(const matrix::Vector3f data[], unsigned samples_collected, sphere_params &p,
|
||||
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++) {
|
||||
for (unsigned k = 0; k < samples_collected; k++) {
|
||||
float x = data[k](0);
|
||||
float y = data[k](1);
|
||||
float z = data[k](2);
|
||||
|
||||
// Calculate Jacobian
|
||||
float A = (p.diag(0) * (x - p.offset(0))) + (p.offdiag(0) * (y - p.offset(1))) + (p.offdiag(1) * (z - p.offset(2)));
|
||||
float B = (p.offdiag(0) * (x - p.offset(0))) + (p.diag(1) * (y - p.offset(1))) + (p.offdiag(2) * (z - p.offset(2)));
|
||||
float C = (p.offdiag(1) * (x - p.offset(0))) + (p.offdiag(2) * (y - p.offset(1))) + (p.diag(2) * (z - p.offset(2)));
|
||||
|
||||
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);
|
||||
|
||||
float sphere_jacob[4];
|
||||
// 0: partial derivative (radius wrt fitness fn) fn operated on sample
|
||||
sphere_jacob[0] = 1.0f;
|
||||
sphere_jacob[0] = 1.f;
|
||||
// 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;
|
||||
sphere_jacob[1] = 1.f * (((p.diag(0) * A) + (p.offdiag(0) * B) + (p.offdiag(1) * C)) / length);
|
||||
sphere_jacob[2] = 1.f * (((p.offdiag(0) * A) + (p.diag(1) * B) + (p.offdiag(2) * C)) / length);
|
||||
sphere_jacob[3] = 1.f * (((p.offdiag(1) * A) + (p.offdiag(2) * B) + (p.diag(2) * C)) / length);
|
||||
|
||||
for (uint8_t i = 0; i < 4; i++) {
|
||||
float residual = p.radius - length;
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
// compute JTJ
|
||||
for (uint8_t j = 0; j < 4; j++) {
|
||||
for (int j = 0; j < 4; j++) {
|
||||
JTJ(i, j) += sphere_jacob[i] * sphere_jacob[j];
|
||||
}
|
||||
|
||||
@@ -87,12 +87,12 @@ void lm_sphere_fit_iteration(const float x[], const float y[], const float z[],
|
||||
|
||||
|
||||
//------------------------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));
|
||||
// refer: https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
|
||||
float fit1_p[4] {p.radius, p.offset(0), p.offset(1), p.offset(2)};
|
||||
float fit2_p[4] {p.radius, p.offset(0), p.offset(1), p.offset(2)};
|
||||
|
||||
JTJ = (JTJ + JTJ.transpose()) * 0.5f; // fix numerical issues
|
||||
matrix::SquareMatrix<float, 4> JTJ2 = JTJ;
|
||||
matrix::SquareMatrix<float, 4> JTJ2{JTJ};
|
||||
|
||||
for (uint8_t i = 0; i < 4; i++) {
|
||||
JTJ(i, i) += result.gradient_damping;
|
||||
@@ -107,42 +107,39 @@ void lm_sphere_fit_iteration(const float x[], const float y[], const float z[],
|
||||
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);
|
||||
for (int row = 0; row < 4; row++) {
|
||||
for (int col = 0; col < 4; col++) {
|
||||
fit1_p[row] -= JTFI[col] * JTJ(row, col);
|
||||
fit2_p[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]));
|
||||
for (unsigned k = 0; k < samples_collected; k++) {
|
||||
float x = data[k](0);
|
||||
float y = data[k](1);
|
||||
float z = data[k](2);
|
||||
|
||||
float A = (p.diag(0) * (x - fit1_p[1])) + (p.offdiag(0) * (y - fit1_p[2])) + (p.offdiag(1) * (z + fit1_p[3]));
|
||||
float B = (p.offdiag(0) * (x - fit1_p[1])) + (p.diag(1) * (y - fit1_p[2])) + (p.offdiag(2) * (z + fit1_p[3]));
|
||||
float C = (p.offdiag(1) * (x - fit1_p[1])) + (p.offdiag(2) * (y - fit1_p[2])) + (p.diag(2) * (z - fit1_p[3]));
|
||||
|
||||
float length = sqrtf(A * A + B * B + C * C);
|
||||
residual = fit1_params[0] - length;
|
||||
float residual = fit1_p[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]));
|
||||
A = (p.diag(0) * (x - fit2_p[1])) + (p.offdiag(0) * (y - fit2_p[2])) + (p.offdiag(1) * (z - fit2_p[3]));
|
||||
B = (p.offdiag(0) * (x - fit2_p[1])) + (p.diag(1) * (y - fit2_p[2])) + (p.offdiag(2) * (z - fit2_p[3]));
|
||||
C = (p.offdiag(1) * (x - fit2_p[1])) + (p.offdiag(2) * (y - fit2_p[2])) + (p.diag(2) * (z - fit2_p[3]));
|
||||
length = sqrtf(A * A + B * B + C * C);
|
||||
residual = fit2_params[0] - length;
|
||||
residual = fit2_p[0] - length;
|
||||
fit2 += residual * residual;
|
||||
}
|
||||
|
||||
float fitness = result.cost;
|
||||
|
||||
fit1 = sqrtf(fit1) / samples_collected;
|
||||
fit2 = sqrtf(fit2) / samples_collected;
|
||||
|
||||
@@ -151,7 +148,7 @@ void lm_sphere_fit_iteration(const float x[], const float y[], const float z[],
|
||||
|
||||
} else if (fit2 < result.cost && fit2 < fit1) {
|
||||
result.gradient_damping /= lma_damping;
|
||||
memcpy(fit1_params, fit2_params, sizeof(fit1_params));
|
||||
memcpy(fit1_p, fit2_p, sizeof(fit1_p));
|
||||
fitness = fit2;
|
||||
|
||||
} else if (fit1 < result.cost) {
|
||||
@@ -159,13 +156,15 @@ void lm_sphere_fit_iteration(const float x[], const float y[], const float z[],
|
||||
}
|
||||
|
||||
//--------------------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];
|
||||
|
||||
p.radius = fit1_p[0];
|
||||
|
||||
p.offset(0) = fit1_p[1];
|
||||
p.offset(1) = fit1_p[2];
|
||||
p.offset(2) = fit1_p[3];
|
||||
|
||||
result.result = iteration_result::STATUS::SUCCESS;
|
||||
|
||||
} else {
|
||||
@@ -173,49 +172,49 @@ void lm_sphere_fit_iteration(const float x[], const float y[], const float z[],
|
||||
}
|
||||
}
|
||||
|
||||
void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[],
|
||||
unsigned int samples_collected, sphere_params ¶ms, iteration_result &result)
|
||||
static void lm_ellipsoid_fit_iteration(const matrix::Vector3f data[], unsigned samples_collected, sphere_params &p,
|
||||
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;
|
||||
const float lma_damping = 10.f;
|
||||
|
||||
matrix::SquareMatrix<float, 9> JTJ;
|
||||
float fit1 = 0.f;
|
||||
float fit2 = 0.f;
|
||||
|
||||
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++) {
|
||||
for (unsigned k = 0; k < samples_collected; k++) {
|
||||
float x = data[k](0);
|
||||
float y = data[k](1);
|
||||
float z = data[k](2);
|
||||
|
||||
// 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 A = (p.diag(0) * (x - p.offset(0))) + (p.offdiag(0) * (y - p.offset(1))) + (p.offdiag(1) * (z - p.offset(2)));
|
||||
float B = (p.offdiag(0) * (x - p.offset(0))) + (p.diag(1) * (y - p.offset(1))) + (p.offdiag(2) * (z - p.offset(2)));
|
||||
float C = (p.offdiag(1) * (x - p.offset(0))) + (p.offdiag(2) * (y - p.offset(1))) + (p.diag(2) * (z - p.offset(2)));
|
||||
float length = sqrtf(A * A + B * B + C * C);
|
||||
residual = params.radius - length;
|
||||
float residual = p.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++) {
|
||||
float ellipsoid_jacob[9];
|
||||
// 0-2: partial derivative (offset wrt fitness fn) fn operated on sample
|
||||
ellipsoid_jacob[0] = 1.f * (((p.diag(0) * A) + (p.offdiag(0) * B) + (p.offdiag(1) * C)) / length);
|
||||
ellipsoid_jacob[1] = 1.f * (((p.offdiag(0) * A) + (p.diag(1) * B) + (p.offdiag(2) * C)) / length);
|
||||
ellipsoid_jacob[2] = 1.f * (((p.offdiag(1) * A) + (p.offdiag(2) * B) + (p.diag(2) * C)) / length);
|
||||
// 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample
|
||||
ellipsoid_jacob[3] = -1.f * ((x - p.offset(0)) * A) / length;
|
||||
ellipsoid_jacob[4] = -1.f * ((y - p.offset(1)) * B) / length;
|
||||
ellipsoid_jacob[5] = -1.f * ((z - p.offset(2)) * C) / length;
|
||||
// 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample
|
||||
ellipsoid_jacob[6] = -1.f * (((y - p.offset(1)) * A) + ((x - p.offset(0)) * B)) / length;
|
||||
ellipsoid_jacob[7] = -1.f * (((z - p.offset(2)) * A) + ((x - p.offset(0)) * C)) / length;
|
||||
ellipsoid_jacob[8] = -1.f * (((z - p.offset(2)) * B) + ((y - p.offset(1)) * C)) / length;
|
||||
|
||||
for (int i = 0; i < 9; i++) {
|
||||
// compute JTJ
|
||||
for (uint8_t j = 0; j < 9; j++) {
|
||||
for (int j = 0; j < 9; j++) {
|
||||
JTJ(i, j) += ellipsoid_jacob[i] * ellipsoid_jacob[j];
|
||||
}
|
||||
|
||||
@@ -226,12 +225,13 @@ void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[
|
||||
|
||||
//------------------------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;
|
||||
float fit1_p[9] {p.offset(0), p.offset(1), p.offset(2), p.diag(0), p.diag(1), p.diag(2), p.offdiag(0), p.offdiag(1), p.offdiag(2)};
|
||||
float fit2_p[9];
|
||||
memcpy(fit2_p, fit1_p, sizeof(fit1_p));
|
||||
|
||||
for (uint8_t i = 0; i < 9; i++) {
|
||||
matrix::SquareMatrix<float, 9> JTJ2{JTJ};
|
||||
|
||||
for (int i = 0; i < 9; i++) {
|
||||
JTJ(i, i) += result.gradient_damping;
|
||||
JTJ2(i, i) += result.gradient_damping / lma_damping;
|
||||
}
|
||||
@@ -247,36 +247,36 @@ void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[
|
||||
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);
|
||||
for (int row = 0; row < 9; row++) {
|
||||
for (int col = 0; col < 9; col++) {
|
||||
fit1_p[row] -= JTFI[col] * JTJ(row, col);
|
||||
fit2_p[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]));
|
||||
for (unsigned k = 0; k < samples_collected; k++) {
|
||||
float x = data[k](0);
|
||||
float y = data[k](1);
|
||||
float z = data[k](2);
|
||||
|
||||
float A = (fit1_p[3] * (x - fit1_p[0])) + (fit1_p[6] * (y - fit1_p[1])) + (fit1_p[7] * (z - fit1_p[2]));
|
||||
float B = (fit1_p[6] * (x - fit1_p[0])) + (fit1_p[4] * (y - fit1_p[1])) + (fit1_p[8] * (z - fit1_p[2]));
|
||||
float C = (fit1_p[7] * (x - fit1_p[0])) + (fit1_p[8] * (y - fit1_p[1])) + (fit1_p[5] * (z - fit1_p[2]));
|
||||
float length = sqrtf(A * A + B * B + C * C);
|
||||
residual = params.radius - length;
|
||||
float residual = p.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]));
|
||||
A = (fit2_p[3] * (x - fit2_p[0])) + (fit2_p[6] * (y - fit2_p[1])) + (fit2_p[7] * (z - fit2_p[2]));
|
||||
B = (fit2_p[6] * (x - fit2_p[0])) + (fit2_p[4] * (y - fit2_p[1])) + (fit2_p[8] * (z - fit2_p[2]));
|
||||
C = (fit2_p[7] * (x - fit2_p[0])) + (fit2_p[8] * (y - fit2_p[1])) + (fit2_p[5] * (z - fit2_p[2]));
|
||||
length = sqrtf(A * A + B * B + C * C);
|
||||
residual = params.radius - length;
|
||||
residual = p.radius - length;
|
||||
fit2 += residual * residual;
|
||||
}
|
||||
|
||||
float fitness = result.cost;
|
||||
|
||||
fit1 = sqrtf(fit1) / samples_collected;
|
||||
fit2 = sqrtf(fit2) / samples_collected;
|
||||
|
||||
@@ -285,7 +285,7 @@ void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[
|
||||
|
||||
} else if (fit2 < result.cost && fit2 < fit1) {
|
||||
result.gradient_damping /= lma_damping;
|
||||
memcpy(fit1_params, fit2_params, sizeof(fit1_params));
|
||||
memcpy(fit1_p, fit2_p, sizeof(fit1_p));
|
||||
fitness = fit2;
|
||||
|
||||
} else if (fit1 < result.cost) {
|
||||
@@ -295,15 +295,17 @@ void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[
|
||||
//--------------------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];
|
||||
|
||||
p.offset(0) = fit1_p[0];
|
||||
p.offset(1) = fit1_p[1];
|
||||
p.offset(2) = fit1_p[2];
|
||||
p.diag(0) = fit1_p[3];
|
||||
p.diag(1) = fit1_p[4];
|
||||
p.diag(2) = fit1_p[5];
|
||||
p.offdiag(0) = fit1_p[6];
|
||||
p.offdiag(1) = fit1_p[7];
|
||||
p.offdiag(2) = fit1_p[8];
|
||||
|
||||
result.result = iteration_result::STATUS::SUCCESS;
|
||||
|
||||
} else {
|
||||
@@ -311,19 +313,13 @@ void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params ¶ms,
|
||||
bool full_ellipsoid)
|
||||
int lm_fit(const matrix::Vector3f data[], unsigned samples_collected, sphere_params ¶ms, bool full_ellipsoid)
|
||||
{
|
||||
static constexpr int min_iterations = 10;
|
||||
static constexpr int max_iterations = 100;
|
||||
|
||||
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;
|
||||
static constexpr float cost_threshold = 0.02f;
|
||||
static constexpr float step_threshold = 0.001f;
|
||||
|
||||
iteration_result iter;
|
||||
iter.cost = 1e30f;
|
||||
@@ -333,15 +329,27 @@ int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int s
|
||||
|
||||
for (int i = 0; i < max_iterations; i++) {
|
||||
if (full_ellipsoid) {
|
||||
lm_ellipsoid_fit_iteration(x, y, z, samples_collected, params, iter);
|
||||
lm_ellipsoid_fit_iteration(data, samples_collected, params, iter);
|
||||
|
||||
PX4_INFO("[%d] O:[%.5f, %.5f, %.5f], S:[%.3f, %.3f, %.3f], Result: %d, Cost: %.6f, Damping: %.6f\n",
|
||||
i,
|
||||
(double)params.offset(0), (double)params.offset(1), (double)params.offset(2),
|
||||
(double)params.diag(0), (double)params.diag(1), (double)params.diag(2),
|
||||
iter.result == iteration_result::STATUS::SUCCESS, (double)iter.cost, (double)iter.gradient_damping);
|
||||
|
||||
} else {
|
||||
lm_sphere_fit_iteration(x, y, z, samples_collected, params, iter);
|
||||
lm_sphere_fit_iteration(data, samples_collected, params, iter);
|
||||
|
||||
PX4_INFO("[%d] O:[%.5f, %.5f, %.5f], Result: %d, Cost: %.6f, Damping: %.6f, Radius: %.4f\n",
|
||||
i,
|
||||
(double)params.offset(0), (double)params.offset(1), (double)params.offset(2),
|
||||
iter.result == iteration_result::STATUS::SUCCESS, (double)iter.cost, (double)iter.gradient_damping,
|
||||
(double)params.radius);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -37,7 +37,9 @@
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
struct sphere_params {
|
||||
matrix::Vector3f offset, diag{1.f, 1.f, 1.f}, offdiag;
|
||||
matrix::Vector3f offset{0.f, 0.f, 0.f};
|
||||
matrix::Vector3f diag{1.f, 1.f, 1.f};
|
||||
matrix::Vector3f offdiag{0.f, 0.f, 0.f};
|
||||
float radius{0.2f};
|
||||
};
|
||||
|
||||
@@ -47,9 +49,7 @@ struct sphere_params {
|
||||
*
|
||||
* Fits a sphere to a set of points on the sphere surface.
|
||||
*
|
||||
* @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 data x,y,z point coordinates
|
||||
* @param samples_collected number of points
|
||||
* @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
|
||||
* @param params the values to be optimized
|
||||
@@ -59,5 +59,4 @@ struct sphere_params {
|
||||
*
|
||||
* @return 0 on success, 1 on failure
|
||||
*/
|
||||
int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params ¶ms,
|
||||
bool full_ellipsoid);
|
||||
int lm_fit(const matrix::Vector3f data[], unsigned samples_collected, sphere_params ¶ms, bool full_ellipsoid);
|
||||
|
||||
@@ -90,9 +90,7 @@ struct mag_worker_data_t {
|
||||
uint64_t calibration_interval_perside_us;
|
||||
unsigned int calibration_counter_total[MAX_MAGS];
|
||||
|
||||
float *x[MAX_MAGS];
|
||||
float *y[MAX_MAGS];
|
||||
float *z[MAX_MAGS];
|
||||
matrix::Vector3f *data[MAX_MAGS] {nullptr, nullptr, nullptr, nullptr};
|
||||
|
||||
float temperature[MAX_MAGS] {NAN, NAN, NAN, NAN};
|
||||
|
||||
@@ -141,19 +139,20 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
|
||||
return result;
|
||||
}
|
||||
|
||||
static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count,
|
||||
static bool reject_sample(Vector3f s, Vector3f data[], unsigned count,
|
||||
unsigned max_count, float mag_sphere_radius)
|
||||
{
|
||||
float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
|
||||
|
||||
for (size_t i = 0; i < count; i++) {
|
||||
float dx = sx - x[i];
|
||||
float dy = sy - y[i];
|
||||
float dz = sz - z[i];
|
||||
float dx = s(0) - data[i](0);
|
||||
float dy = s(1) - data[i](1);
|
||||
float dz = s(2) - data[i](2);
|
||||
float dist = sqrtf(dx * dx + dy * dy + dz * dz);
|
||||
|
||||
if (dist < min_sample_dist) {
|
||||
PX4_DEBUG("rejected X: %.3f Y: %.3f Z: %.3f (%.3f < %.3f) (%u/%u) ", (double)sx, (double)sy, (double)sz, (double)dist,
|
||||
PX4_DEBUG("rejected X: %.3f Y: %.3f Z: %.3f (%.3f < %.3f) (%u/%u) ", (double)s(0), (double)s(1), (double)s(2),
|
||||
(double)dist,
|
||||
(double)min_sample_dist, count, max_count);
|
||||
|
||||
return true;
|
||||
@@ -363,8 +362,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
||||
}
|
||||
|
||||
// Check if this measurement is good to go in
|
||||
bool reject = reject_sample(mag.x, mag.y, mag.z,
|
||||
worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
|
||||
bool reject = reject_sample(Vector3f{mag.x, mag.y, mag.z}, worker_data->data[cur_mag],
|
||||
worker_data->calibration_counter_total[cur_mag],
|
||||
worker_data->calibration_sides * worker_data->calibration_points_perside,
|
||||
mag_sphere_radius);
|
||||
@@ -388,9 +386,9 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
||||
if (!rejected) {
|
||||
for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
|
||||
if (worker_data->calibration[cur_mag].device_id() != 0) {
|
||||
worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](0);
|
||||
worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](1);
|
||||
worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](2);
|
||||
|
||||
worker_data->data[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag];
|
||||
worker_data->calibration_counter_total[cur_mag]++;
|
||||
|
||||
if (!PX4_ISFINITE(worker_data->temperature[cur_mag])) {
|
||||
// set first valid value
|
||||
@@ -399,13 +397,11 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
||||
} else {
|
||||
worker_data->temperature[cur_mag] = 0.5f * (worker_data->temperature[cur_mag] + new_temperature[cur_mag]);
|
||||
}
|
||||
|
||||
worker_data->calibration_counter_total[cur_mag]++;
|
||||
}
|
||||
}
|
||||
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
mag_worker_data_s status;
|
||||
mag_worker_data_s status{};
|
||||
status.timestamp = now;
|
||||
status.timestamp_sample = now;
|
||||
status.done_count = worker_data->done_count;
|
||||
@@ -418,9 +414,9 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
||||
|
||||
if (worker_data->calibration[cur_mag].device_id() != 0) {
|
||||
const unsigned int sample = worker_data->calibration_counter_total[cur_mag] - 1;
|
||||
status.x[cur_mag] = worker_data->x[cur_mag][sample];
|
||||
status.y[cur_mag] = worker_data->y[cur_mag][sample];
|
||||
status.z[cur_mag] = worker_data->z[cur_mag][sample];
|
||||
status.x[cur_mag] = worker_data->data[cur_mag][sample](0);
|
||||
status.y[cur_mag] = worker_data->data[cur_mag][sample](1);
|
||||
status.z[cur_mag] = worker_data->data[cur_mag][sample](2);
|
||||
|
||||
} else {
|
||||
status.x[cur_mag] = 0.f;
|
||||
@@ -527,9 +523,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
|
||||
|
||||
for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
|
||||
// Initialize to no memory allocated
|
||||
worker_data.x[cur_mag] = nullptr;
|
||||
worker_data.y[cur_mag] = nullptr;
|
||||
worker_data.z[cur_mag] = nullptr;
|
||||
worker_data.calibration_counter_total[cur_mag] = 0;
|
||||
}
|
||||
|
||||
@@ -547,11 +540,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
|
||||
worker_data.calibration[cur_mag].set_calibration_index(cur_mag);
|
||||
|
||||
if (worker_data.calibration[cur_mag].device_id() != 0) {
|
||||
worker_data.x[cur_mag] = static_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
|
||||
worker_data.y[cur_mag] = static_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
|
||||
worker_data.z[cur_mag] = static_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
|
||||
|
||||
if (worker_data.x[cur_mag] == nullptr || worker_data.y[cur_mag] == nullptr || worker_data.z[cur_mag] == nullptr) {
|
||||
worker_data.data[cur_mag] = new matrix::Vector3f[calibration_points_maxcount];
|
||||
|
||||
if (worker_data.data[cur_mag] == nullptr) {
|
||||
calibration_log_critical(mavlink_log_pub, "ERROR: out of memory");
|
||||
result = calibrate_return_error;
|
||||
break;
|
||||
@@ -566,8 +558,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
|
||||
result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output
|
||||
worker_data.side_data_collected, // Sides to calibrate
|
||||
mag_calibration_worker, // Calibration worker
|
||||
&worker_data, // Opaque data for calibration worked
|
||||
true); // true: lenient still detection
|
||||
&worker_data); // Opaque data for calibration worked
|
||||
}
|
||||
|
||||
// calibration values for each mag
|
||||
@@ -594,26 +585,26 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
|
||||
// Estimate only the offsets if two-sided calibration is selected, as the problem is not constrained
|
||||
// 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;
|
||||
const bool sphere_fit_only = worker_data.calibration_sides <= 2;
|
||||
|
||||
sphere_params sphere_data;
|
||||
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));
|
||||
sphere_data.offset = sphere[cur_mag];
|
||||
sphere_data.diag = diag[cur_mag];
|
||||
sphere_data.offdiag = offdiag[cur_mag];
|
||||
|
||||
bool sphere_fit_success = false;
|
||||
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);
|
||||
|
||||
int ret = lm_fit(worker_data.data[cur_mag], worker_data.calibration_counter_total[cur_mag], sphere_data, false);
|
||||
|
||||
if (ret == PX4_OK) {
|
||||
sphere_fit_success = true;
|
||||
PX4_INFO("Mag: %" PRIu8 " sphere radius: %.4f", cur_mag, (double)sphere_data.radius);
|
||||
|
||||
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);
|
||||
int ellipsoid_ret = lm_fit(worker_data.data[cur_mag], worker_data.calibration_counter_total[cur_mag], sphere_data,
|
||||
true);
|
||||
|
||||
if (ellipsoid_ret == PX4_OK) {
|
||||
ellipsoid_fit_success = true;
|
||||
@@ -621,13 +612,14 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// TODO: verify sphere radius
|
||||
|
||||
sphere_radius[cur_mag] = sphere_data.radius;
|
||||
|
||||
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);
|
||||
}
|
||||
sphere[cur_mag] = sphere_data.offset;
|
||||
diag[cur_mag] = sphere_data.diag;
|
||||
offdiag[cur_mag] = sphere_data.offdiag;
|
||||
|
||||
if (!sphere_fit_success && !ellipsoid_fit_success) {
|
||||
calibration_log_emergency(mavlink_log_pub, "Retry calibration (unable to fit mag %" PRIu8 ")", cur_mag);
|
||||
@@ -728,7 +720,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
|
||||
|
||||
for (unsigned i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
|
||||
|
||||
const Vector3f sample{worker_data.x[cur_mag][i], worker_data.y[cur_mag][i], worker_data.z[cur_mag][i]};
|
||||
const Vector3f sample{worker_data.data[cur_mag][i]};
|
||||
|
||||
// apply calibration
|
||||
Vector3f m{scale *(sample - offset)};
|
||||
@@ -739,9 +731,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
|
||||
}
|
||||
|
||||
// store back in worker_data
|
||||
worker_data.x[cur_mag][i] = m(0);
|
||||
worker_data.y[cur_mag][i] = m(1);
|
||||
worker_data.z[cur_mag][i] = m(2);
|
||||
worker_data.data[cur_mag][i] = m;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -775,14 +765,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
|
||||
float diff_sum = 0.f;
|
||||
|
||||
for (int i = 0; i < last_sample_index; i++) {
|
||||
float x = worker_data.x[cur_mag][i];
|
||||
float y = worker_data.y[cur_mag][i];
|
||||
float z = worker_data.z[cur_mag][i];
|
||||
rotate_3f((enum Rotation)r, x, y, z);
|
||||
|
||||
Vector3f diff = Vector3f{x, y, z} - Vector3f{worker_data.x[internal_index][i], worker_data.y[internal_index][i], worker_data.z[internal_index][i]};
|
||||
|
||||
diff_sum += diff.norm_squared();
|
||||
const Vector3f &m{worker_data.data[cur_mag][i]};
|
||||
diff_sum += Vector3f((get_rot_matrix((enum Rotation)r) * m) - m).norm_squared();
|
||||
}
|
||||
|
||||
// compute mean squared error
|
||||
@@ -878,9 +862,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
|
||||
|
||||
// Data points are no longer needed
|
||||
for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
|
||||
free(worker_data.x[cur_mag]);
|
||||
free(worker_data.y[cur_mag]);
|
||||
free(worker_data.z[cur_mag]);
|
||||
delete[] worker_data.data[cur_mag];
|
||||
}
|
||||
|
||||
FactoryCalibrationStorage factory_storage;
|
||||
|
||||
@@ -50,38 +50,38 @@ using matrix::Vector3f;
|
||||
class MagCalTest : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
void generate2SidesMagData(float *x, float *y, float *z, unsigned int n_samples, float mag_str);
|
||||
void generate2SidesMagData(matrix::Vector3f mag_data[], unsigned int n_samples, float mag_str);
|
||||
|
||||
/* Generate regularly spaced data on a sphere
|
||||
* Ref.: How to generate equidistributed points on the surface of a sphere, Markus Deserno, 2004
|
||||
*/
|
||||
void generateRegularData(float *x, float *y, float *z, unsigned int n_samples, float mag_str);
|
||||
void generateRegularData(matrix::Vector3f mag_data[], unsigned int n_samples, float mag_str);
|
||||
|
||||
void modifyOffsetScale(float *x, float *y, float *z, unsigned int n_samples, Vector3f offsets, Vector3f scale_factors);
|
||||
void modifyOffsetScale(matrix::Vector3f mag_data[], unsigned int n_samples, Vector3f offsets, Vector3f scale_factors);
|
||||
};
|
||||
|
||||
void MagCalTest::generate2SidesMagData(float *x, float *y, float *z, unsigned int n_samples, float mag_str)
|
||||
void MagCalTest::generate2SidesMagData(matrix::Vector3f data[], unsigned int n_samples, float mag_str)
|
||||
{
|
||||
float psi = 0.f;
|
||||
float theta = 0.f;
|
||||
const float d_angle = 2.f * M_PI_F / float(n_samples / 2);
|
||||
|
||||
for (int i = 0; i < int(n_samples / 2); i++) {
|
||||
x[i] = mag_str * sinf(psi);
|
||||
y[i] = mag_str * cosf(psi);
|
||||
z[i] = 0.f;
|
||||
data[i](0) = mag_str * sinf(psi);
|
||||
data[i](1) = mag_str * cosf(psi);
|
||||
data[i](2) = 0.f;
|
||||
psi += d_angle;
|
||||
}
|
||||
|
||||
for (int i = int(n_samples / 2); i < int(n_samples); i++) {
|
||||
x[i] = mag_str * sinf(theta);
|
||||
y[i] = 0.f;
|
||||
z[i] = mag_str * cosf(theta);
|
||||
data[i](0) = mag_str * sinf(theta);
|
||||
data[i](1) = 0.f;
|
||||
data[i](2) = mag_str * cosf(theta);
|
||||
theta += d_angle;
|
||||
}
|
||||
}
|
||||
|
||||
void MagCalTest::generateRegularData(float *x, float *y, float *z, unsigned int n_samples, float mag_str)
|
||||
void MagCalTest::generateRegularData(matrix::Vector3f mag_data[], unsigned int n_samples, float mag_str)
|
||||
{
|
||||
const float a = 4.f * M_PI_F * mag_str * mag_str / n_samples;
|
||||
const float d = sqrtf(a);
|
||||
@@ -97,9 +97,9 @@ void MagCalTest::generateRegularData(float *x, float *y, float *z, unsigned int
|
||||
|
||||
for (int n = 0; n < m_phi; n++) {
|
||||
const float phi = 2.f * M_PI_F * n / static_cast<float>(m_phi);
|
||||
x[n_count] = mag_str * sinf(theta) * cosf(phi);
|
||||
y[n_count] = mag_str * sinf(theta) * sinf(phi);
|
||||
z[n_count] = mag_str * cosf(theta);
|
||||
mag_data[n_count](0) = mag_str * sinf(theta) * cosf(phi);
|
||||
mag_data[n_count](1) = mag_str * sinf(theta) * sinf(phi);
|
||||
mag_data[n_count](2) = mag_str * cosf(theta);
|
||||
n_count++;
|
||||
}
|
||||
}
|
||||
@@ -111,20 +111,16 @@ void MagCalTest::generateRegularData(float *x, float *y, float *z, unsigned int
|
||||
|
||||
// Padd with constant data
|
||||
while (n_count < n_samples) {
|
||||
x[n_count] = x[n_count - 1];
|
||||
y[n_count] = y[n_count - 1];
|
||||
z[n_count] = z[n_count - 1];
|
||||
mag_data[n_count] = mag_data[n_count - 1];
|
||||
n_count++;
|
||||
}
|
||||
}
|
||||
|
||||
void MagCalTest::modifyOffsetScale(float *x, float *y, float *z, unsigned int n_samples, Vector3f offsets,
|
||||
void MagCalTest::modifyOffsetScale(matrix::Vector3f mag_data[], unsigned int n_samples, Vector3f offsets,
|
||||
Vector3f scale_factors)
|
||||
{
|
||||
for (unsigned int k = 0; k < n_samples; k++) {
|
||||
x[k] = x[k] * scale_factors(0) + offsets(0);
|
||||
y[k] = y[k] * scale_factors(1) + offsets(1);
|
||||
z[k] = z[k] * scale_factors(2) + offsets(2);
|
||||
mag_data[k] = mag_data[k].emult(scale_factors) + offsets;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -138,17 +134,13 @@ TEST_F(MagCalTest, sphere2Sides)
|
||||
const Vector3f offset_true;
|
||||
const Vector3f scale_true = {1.f, 1.f, 1.f};
|
||||
|
||||
float x[N_SAMPLES];
|
||||
float y[N_SAMPLES];
|
||||
float z[N_SAMPLES];
|
||||
|
||||
generate2SidesMagData(x, y, z, N_SAMPLES, mag_str_true);
|
||||
Vector3f mag_data[N_SAMPLES];
|
||||
generate2SidesMagData(mag_data, N_SAMPLES, mag_str_true);
|
||||
|
||||
// WHEN: fitting a sphere with the data and given a wrong initial radius
|
||||
sphere_params sphere;
|
||||
sphere.diag = {1.f, 1.f, 1.f};
|
||||
sphere_params sphere{};
|
||||
sphere.radius = 0.2;
|
||||
int success = lm_mag_fit(x, y, z, N_SAMPLES, sphere, false);
|
||||
int success = lm_fit(mag_data, N_SAMPLES, sphere, false);
|
||||
|
||||
// THEN: the algorithm should converge in a single step
|
||||
EXPECT_EQ(success, PX4_OK);
|
||||
@@ -171,17 +163,15 @@ TEST_F(MagCalTest, sphereRegularlySpaced)
|
||||
const Vector3f offset_true = {-1.07f, 0.35f, -0.78f};
|
||||
const Vector3f scale_true = {1.f, 1.f, 1.f};
|
||||
|
||||
float x[N_SAMPLES];
|
||||
float y[N_SAMPLES];
|
||||
float z[N_SAMPLES];
|
||||
generateRegularData(x, y, z, N_SAMPLES, mag_str_true);
|
||||
modifyOffsetScale(x, y, z, N_SAMPLES, offset_true, scale_true);
|
||||
Vector3f mag_data[N_SAMPLES];
|
||||
generateRegularData(mag_data, N_SAMPLES, mag_str_true);
|
||||
modifyOffsetScale(mag_data, N_SAMPLES, offset_true, scale_true);
|
||||
|
||||
// WHEN: fitting a sphere to the data
|
||||
sphere_params sphere;
|
||||
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);
|
||||
int success = lm_fit(mag_data, N_SAMPLES, sphere, false);
|
||||
|
||||
// THEN: the algorithm should converge in a few iterations and
|
||||
// find the correct parameters
|
||||
@@ -200,7 +190,7 @@ TEST_F(MagCalTest, replayTestData)
|
||||
// GIVEN: a real test dataset with large offsets
|
||||
// and where the two first iterations of the LM algorithm
|
||||
// produces a negative radius and a constant fitness value
|
||||
constexpr unsigned int N_SAMPLES = 231;
|
||||
constexpr unsigned N_SAMPLES = 231;
|
||||
|
||||
const float mag_str_true = 0.4f;
|
||||
const Vector3f offset_true = {-0.18f, 0.05f, -0.58f};
|
||||
@@ -209,7 +199,16 @@ TEST_F(MagCalTest, replayTestData)
|
||||
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);
|
||||
|
||||
matrix::Vector3f mag_data1[231];
|
||||
|
||||
for (unsigned i = 0; i < N_SAMPLES; i++) {
|
||||
mag_data1[i](0) = mag_data1_x[i];
|
||||
mag_data1[i](1) = mag_data1_y[i];
|
||||
mag_data1[i](2) = mag_data1_z[i];
|
||||
}
|
||||
|
||||
int sphere_success = lm_fit(mag_data1, N_SAMPLES, sphere, false);
|
||||
|
||||
// THEN: the algorithm should converge and find the correct parameters
|
||||
EXPECT_EQ(sphere_success, PX4_OK);
|
||||
@@ -222,8 +221,8 @@ TEST_F(MagCalTest, replayTestData)
|
||||
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);
|
||||
int ellipsoid_step_1_success = lm_fit(mag_data1, N_SAMPLES, ellipsoid, false);
|
||||
int ellipsoid_success = lm_fit(mag_data1, N_SAMPLES, ellipsoid, true);
|
||||
const Vector3f scale_true = {1.f, 1.06f, 0.94f};
|
||||
|
||||
EXPECT_EQ(ellipsoid_step_1_success, PX4_OK);
|
||||
|
||||
@@ -0,0 +1,66 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Bitfield selecting mag sides for calibration
|
||||
*
|
||||
* If set to two side calibration, only the offsets are estimated, the scale
|
||||
* calibration is left unchanged. Thus an initial six side calibration is
|
||||
* recommended.
|
||||
*
|
||||
* Bits:
|
||||
* ORIENTATION_TAIL_DOWN = 1
|
||||
* ORIENTATION_NOSE_DOWN = 2
|
||||
* ORIENTATION_LEFT = 4
|
||||
* ORIENTATION_RIGHT = 8
|
||||
* ORIENTATION_UPSIDE_DOWN = 16
|
||||
* ORIENTATION_RIGHTSIDE_UP = 32
|
||||
*
|
||||
* @min 34
|
||||
* @max 63
|
||||
* @value 34 Two side calibration
|
||||
* @value 38 Three side calibration
|
||||
* @value 63 Six side calibration
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CAL_ACC_SIDES, 63);
|
||||
|
||||
/**
|
||||
* Automatically set external rotations.
|
||||
*
|
||||
* During calibration attempt to automatically determine the rotation of external magnetometers.
|
||||
*
|
||||
* @boolean
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CAL_CAL_ROT_AUTO, 1);
|
||||
Reference in New Issue
Block a user