mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-06 16:49:51 +08:00
commander: add new math for sphere fit for compass calibration
This commit is contained in:
Siddharth Bharat Purohit
committed by
Lorenz Meier
Lorenz Meier
parent
f746141afe
commit
f811777789
@@ -233,6 +233,265 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, unsigned int max_iterations, float delta, float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z)
|
||||
{
|
||||
float _fitness = 1.0e30f, _sphere_lambda = 1.0f;
|
||||
for(int i=0; i < max_iterations; i++) {
|
||||
//printf("%d, offset: %.6f %.6f %.6f %.6f fitness: %.6f\n", i, (double)*offset_x, (double)*offset_y, (double)*offset_z, (double)*sphere_radius, (double)_fitness);
|
||||
run_lm_sphere_fit(x, y, z, _fitness, _sphere_lambda,
|
||||
size, offset_x, offset_y, offset_z,
|
||||
sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
|
||||
unsigned int size, float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z)
|
||||
{
|
||||
//Run Sphere Fit using Levenberg Marquardt LSq Fit
|
||||
const float lma_damping = 10.0f;
|
||||
float _samples_collected = size;
|
||||
float fitness = _fitness;
|
||||
float fit1 = 0.0f, fit2 = 0.0f;
|
||||
|
||||
float JTJ[16];
|
||||
float JTJ2[16];
|
||||
float JTFI[4];
|
||||
float residual = 0.0f;
|
||||
memset(&JTJ,0,sizeof(JTJ));
|
||||
memset(&JTJ2,0,sizeof(JTJ2));
|
||||
memset(&JTFI,0,sizeof(JTFI));
|
||||
// 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*4 + j] += sphere_jacob[i] * sphere_jacob[j];
|
||||
JTJ2[i*4 + 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*4 + i] += _sphere_lambda;
|
||||
JTJ2[i*4 + i] += _sphere_lambda/lma_damping;
|
||||
}
|
||||
|
||||
if(!inverse4x4(JTJ, JTJ)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if(!inverse4x4(JTJ2, 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*4 + col];
|
||||
fit2_params[row] -= JTFI[col] * JTJ2[row*4 + 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(!isnan(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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool inverse4x4(float m[],float invOut[])
|
||||
{
|
||||
float inv[16], det;
|
||||
uint8_t i;
|
||||
|
||||
inv[0] = m[5] * m[10] * m[15] -
|
||||
m[5] * m[11] * m[14] -
|
||||
m[9] * m[6] * m[15] +
|
||||
m[9] * m[7] * m[14] +
|
||||
m[13] * m[6] * m[11] -
|
||||
m[13] * m[7] * m[10];
|
||||
|
||||
inv[4] = -m[4] * m[10] * m[15] +
|
||||
m[4] * m[11] * m[14] +
|
||||
m[8] * m[6] * m[15] -
|
||||
m[8] * m[7] * m[14] -
|
||||
m[12] * m[6] * m[11] +
|
||||
m[12] * m[7] * m[10];
|
||||
|
||||
inv[8] = m[4] * m[9] * m[15] -
|
||||
m[4] * m[11] * m[13] -
|
||||
m[8] * m[5] * m[15] +
|
||||
m[8] * m[7] * m[13] +
|
||||
m[12] * m[5] * m[11] -
|
||||
m[12] * m[7] * m[9];
|
||||
|
||||
inv[12] = -m[4] * m[9] * m[14] +
|
||||
m[4] * m[10] * m[13] +
|
||||
m[8] * m[5] * m[14] -
|
||||
m[8] * m[6] * m[13] -
|
||||
m[12] * m[5] * m[10] +
|
||||
m[12] * m[6] * m[9];
|
||||
|
||||
inv[1] = -m[1] * m[10] * m[15] +
|
||||
m[1] * m[11] * m[14] +
|
||||
m[9] * m[2] * m[15] -
|
||||
m[9] * m[3] * m[14] -
|
||||
m[13] * m[2] * m[11] +
|
||||
m[13] * m[3] * m[10];
|
||||
|
||||
inv[5] = m[0] * m[10] * m[15] -
|
||||
m[0] * m[11] * m[14] -
|
||||
m[8] * m[2] * m[15] +
|
||||
m[8] * m[3] * m[14] +
|
||||
m[12] * m[2] * m[11] -
|
||||
m[12] * m[3] * m[10];
|
||||
|
||||
inv[9] = -m[0] * m[9] * m[15] +
|
||||
m[0] * m[11] * m[13] +
|
||||
m[8] * m[1] * m[15] -
|
||||
m[8] * m[3] * m[13] -
|
||||
m[12] * m[1] * m[11] +
|
||||
m[12] * m[3] * m[9];
|
||||
|
||||
inv[13] = m[0] * m[9] * m[14] -
|
||||
m[0] * m[10] * m[13] -
|
||||
m[8] * m[1] * m[14] +
|
||||
m[8] * m[2] * m[13] +
|
||||
m[12] * m[1] * m[10] -
|
||||
m[12] * m[2] * m[9];
|
||||
|
||||
inv[2] = m[1] * m[6] * m[15] -
|
||||
m[1] * m[7] * m[14] -
|
||||
m[5] * m[2] * m[15] +
|
||||
m[5] * m[3] * m[14] +
|
||||
m[13] * m[2] * m[7] -
|
||||
m[13] * m[3] * m[6];
|
||||
|
||||
inv[6] = -m[0] * m[6] * m[15] +
|
||||
m[0] * m[7] * m[14] +
|
||||
m[4] * m[2] * m[15] -
|
||||
m[4] * m[3] * m[14] -
|
||||
m[12] * m[2] * m[7] +
|
||||
m[12] * m[3] * m[6];
|
||||
|
||||
inv[10] = m[0] * m[5] * m[15] -
|
||||
m[0] * m[7] * m[13] -
|
||||
m[4] * m[1] * m[15] +
|
||||
m[4] * m[3] * m[13] +
|
||||
m[12] * m[1] * m[7] -
|
||||
m[12] * m[3] * m[5];
|
||||
|
||||
inv[14] = -m[0] * m[5] * m[14] +
|
||||
m[0] * m[6] * m[13] +
|
||||
m[4] * m[1] * m[14] -
|
||||
m[4] * m[2] * m[13] -
|
||||
m[12] * m[1] * m[6] +
|
||||
m[12] * m[2] * m[5];
|
||||
|
||||
inv[3] = -m[1] * m[6] * m[11] +
|
||||
m[1] * m[7] * m[10] +
|
||||
m[5] * m[2] * m[11] -
|
||||
m[5] * m[3] * m[10] -
|
||||
m[9] * m[2] * m[7] +
|
||||
m[9] * m[3] * m[6];
|
||||
|
||||
inv[7] = m[0] * m[6] * m[11] -
|
||||
m[0] * m[7] * m[10] -
|
||||
m[4] * m[2] * m[11] +
|
||||
m[4] * m[3] * m[10] +
|
||||
m[8] * m[2] * m[7] -
|
||||
m[8] * m[3] * m[6];
|
||||
|
||||
inv[11] = -m[0] * m[5] * m[11] +
|
||||
m[0] * m[7] * m[9] +
|
||||
m[4] * m[1] * m[11] -
|
||||
m[4] * m[3] * m[9] -
|
||||
m[8] * m[1] * m[7] +
|
||||
m[8] * m[3] * m[5];
|
||||
|
||||
inv[15] = m[0] * m[5] * m[10] -
|
||||
m[0] * m[6] * m[9] -
|
||||
m[4] * m[1] * m[10] +
|
||||
m[4] * m[2] * m[9] +
|
||||
m[8] * m[1] * m[6] -
|
||||
m[8] * m[2] * m[5];
|
||||
|
||||
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
|
||||
|
||||
if(fabsf(det) < 1.1755e-38f){
|
||||
return false;
|
||||
}
|
||||
|
||||
det = 1.0f / det;
|
||||
|
||||
for (i = 0; i < 16; i++)
|
||||
invOut[i] = inv[i] * det;
|
||||
return true;
|
||||
}
|
||||
|
||||
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub, bool lenient_still_position)
|
||||
{
|
||||
const unsigned ndim = 3;
|
||||
|
||||
@@ -55,7 +55,13 @@
|
||||
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
|
||||
float *sphere_radius);
|
||||
|
||||
int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[],
|
||||
unsigned int size, unsigned int max_iterations, float delta, float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z);
|
||||
int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
|
||||
unsigned int size, float *offset_x, float *offset_y, float *offset_z,
|
||||
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z);
|
||||
bool inverse4x4(float m[],float invOut[]);
|
||||
// FIXME: Change the name
|
||||
static const unsigned max_accel_sens = 3;
|
||||
|
||||
|
||||
@@ -568,10 +568,16 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
||||
// Calculate calibration values for each mag
|
||||
|
||||
|
||||
float sphere_x[max_mags];
|
||||
float sphere_y[max_mags];
|
||||
float sphere_z[max_mags];
|
||||
float sphere_radius[max_mags];
|
||||
float sphere_x[max_mags] = {0.0f};
|
||||
float sphere_y[max_mags] = {0.0f};
|
||||
float sphere_z[max_mags] = {0.0f};
|
||||
float sphere_radius[max_mags] = {0.2f};
|
||||
float diag_x[max_mags] = {1.0f};
|
||||
float diag_y[max_mags] = {1.0f};
|
||||
float diag_z[max_mags] = {1.0f};
|
||||
float offdiag_x[max_mags] = {0.0f};
|
||||
float offdiag_y[max_mags] = {0.0f};
|
||||
float offdiag_z[max_mags] = {0.0f};
|
||||
|
||||
// Sphere fit the data to get calibration values
|
||||
if (result == calibrate_return_ok) {
|
||||
@@ -579,11 +585,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
||||
if (device_ids[cur_mag] != 0) {
|
||||
// Mag in this slot is available and we should have values for it to calibrate
|
||||
|
||||
sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
|
||||
ellipsoid_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
|
||||
worker_data.calibration_counter_total[cur_mag],
|
||||
100, 0.0f,
|
||||
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
|
||||
&sphere_radius[cur_mag]);
|
||||
&sphere_radius[cur_mag], &diag_x[cur_mag], &diag_y[cur_mag], &diag_z[cur_mag], &offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]);
|
||||
|
||||
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
|
||||
calibration_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
|
||||
|
||||
Reference in New Issue
Block a user