mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-06 16:49:51 +08:00
commander: add ellipsoid 9 param fit for magnetometer corrections
This commit is contained in:
Siddharth Bharat Purohit
committed by
Lorenz Meier
Lorenz Meier
parent
c9ac15f0dd
commit
78b8deda15
@@ -237,13 +237,22 @@ 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;
|
||||
float _fitness = 1.0e30f, _sphere_lambda = 1.0f, _ellipsoid_lambda = 1.0f;
|
||||
|
||||
for (int i = 0; i < max_iterations; i++) {
|
||||
//printf("%d, offset: %.6f %.6f %.6f %.6f fitness: %.6f\n", i, (double)*offset_x, (double)*offset_y, (double)*offset_z, (double)*sphere_radius, (double)_fitness);
|
||||
run_lm_sphere_fit(x, y, z, _fitness, _sphere_lambda,
|
||||
size, offset_x, offset_y, offset_z,
|
||||
sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z);
|
||||
|
||||
}
|
||||
|
||||
_fitness = 1.0e30f;
|
||||
|
||||
for (int i = 0; i < max_iterations; i++) {
|
||||
run_lm_ellipsoid_fit(x, y, z, _fitness, _ellipsoid_lambda,
|
||||
size, offset_x, offset_y, offset_z,
|
||||
sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z);
|
||||
}
|
||||
|
||||
return 0;
|
||||
@@ -263,9 +272,9 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
|
||||
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));
|
||||
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++) {
|
||||
@@ -376,6 +385,364 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
|
||||
}
|
||||
}
|
||||
|
||||
int run_lm_ellipsoid_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[81];
|
||||
float JTJ2[81];
|
||||
float JTFI[9];
|
||||
float residual = 0.0f;
|
||||
memset(JTJ, 0, sizeof(JTJ));
|
||||
memset(JTJ2, 0, sizeof(JTJ2));
|
||||
memset(JTFI, 0, sizeof(JTFI));
|
||||
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 (!isnan(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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//TODO: use higher precision datatypes to achieve more accuracy for matrix algebra operations
|
||||
|
||||
/*
|
||||
* Does matrix multiplication of two regular/square matrices
|
||||
*
|
||||
* @param A, Matrix A
|
||||
* @param B, Matrix B
|
||||
* @param n, dimemsion of square matrices
|
||||
* @returns multiplied matrix i.e. A*B
|
||||
*/
|
||||
|
||||
static float *mat_mul(float *A, float *B, uint8_t n)
|
||||
{
|
||||
float *ret = new float[n * n];
|
||||
memset(ret, 0.0f, n * n * sizeof(float));
|
||||
|
||||
for (uint8_t i = 0; i < n; i++) {
|
||||
for (uint8_t j = 0; j < n; j++) {
|
||||
for (uint8_t k = 0; k < n; k++) {
|
||||
ret[i * n + j] += A[i * n + k] * B[k * n + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline void swap(float &a, float &b)
|
||||
{
|
||||
float c;
|
||||
c = a;
|
||||
a = b;
|
||||
b = c;
|
||||
}
|
||||
|
||||
/*
|
||||
* calculates pivot matrix such that all the larger elements in the row are on diagonal
|
||||
*
|
||||
* @param A, input matrix matrix
|
||||
* @param pivot
|
||||
* @param n, dimenstion of square matrix
|
||||
* @returns false = matrix is Singular or non positive definite, true = matrix inversion successful
|
||||
*/
|
||||
|
||||
static void mat_pivot(float *A, float *pivot, uint8_t n)
|
||||
{
|
||||
for (uint8_t i = 0; i < n; i++) {
|
||||
for (uint8_t j = 0; j < n; j++) {
|
||||
pivot[i * n + j] = (i == j);
|
||||
}
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < n; i++) {
|
||||
uint8_t max_j = i;
|
||||
|
||||
for (uint8_t j = i; j < n; j++) {
|
||||
if (fabsf(A[j * n + i]) > fabsf(A[max_j * n + i])) {
|
||||
max_j = j;
|
||||
}
|
||||
}
|
||||
|
||||
if (max_j != i) {
|
||||
for (uint8_t k = 0; k < n; k++) {
|
||||
swap(pivot[i * n + k], pivot[max_j * n + k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* calculates matrix inverse of Lower trangular matrix using forward substitution
|
||||
*
|
||||
* @param L, lower triangular matrix
|
||||
* @param out, Output inverted lower triangular matrix
|
||||
* @param n, dimension of matrix
|
||||
*/
|
||||
|
||||
static void mat_forward_sub(float *L, float *out, uint8_t n)
|
||||
{
|
||||
// Forward substitution solve LY = I
|
||||
for (int i = 0; i < n; i++) {
|
||||
out[i * n + i] = 1 / L[i * n + i];
|
||||
|
||||
for (int j = i + 1; j < n; j++) {
|
||||
for (int k = i; k < j; k++) {
|
||||
out[j * n + i] -= L[j * n + k] * out[k * n + i];
|
||||
}
|
||||
|
||||
out[j * n + i] /= L[j * n + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* calculates matrix inverse of Upper trangular matrix using backward substitution
|
||||
*
|
||||
* @param U, upper triangular matrix
|
||||
* @param out, Output inverted upper triangular matrix
|
||||
* @param n, dimension of matrix
|
||||
*/
|
||||
|
||||
static void mat_back_sub(float *U, float *out, uint8_t n)
|
||||
{
|
||||
// Backward Substitution solve UY = I
|
||||
for (int i = n - 1; i >= 0; i--) {
|
||||
out[i * n + i] = 1 / U[i * n + i];
|
||||
|
||||
for (int j = i - 1; j >= 0; j--) {
|
||||
for (int k = i; k > j; k--) {
|
||||
out[j * n + i] -= U[j * n + k] * out[k * n + i];
|
||||
}
|
||||
|
||||
out[j * n + i] /= U[j * n + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Decomposes square matrix into Lower and Upper triangular matrices such that
|
||||
* A*P = L*U, where P is the pivot matrix
|
||||
* ref: http://rosettacode.org/wiki/LU_decomposition
|
||||
* @param U, upper triangular matrix
|
||||
* @param out, Output inverted upper triangular matrix
|
||||
* @param n, dimension of matrix
|
||||
*/
|
||||
|
||||
static void mat_LU_decompose(float *A, float *L, float *U, float *P, uint8_t n)
|
||||
{
|
||||
memset(L, 0, n * n * sizeof(float));
|
||||
memset(U, 0, n * n * sizeof(float));
|
||||
memset(P, 0, n * n * sizeof(float));
|
||||
mat_pivot(A, P, n);
|
||||
|
||||
float *APrime = mat_mul(P, A, n);
|
||||
|
||||
for (uint8_t i = 0; i < n; i++) {
|
||||
L[i * n + i] = 1;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < n; i++) {
|
||||
for (uint8_t j = 0; j < n; j++) {
|
||||
if (j <= i) {
|
||||
U[j * n + i] = APrime[j * n + i];
|
||||
|
||||
for (uint8_t k = 0; k < j; k++) {
|
||||
U[j * n + i] -= L[j * n + k] * U[k * n + i];
|
||||
}
|
||||
}
|
||||
|
||||
if (j >= i) {
|
||||
L[j * n + i] = APrime[j * n + i];
|
||||
|
||||
for (uint8_t k = 0; k < i; k++) {
|
||||
L[j * n + i] -= L[j * n + k] * U[k * n + i];
|
||||
}
|
||||
|
||||
L[j * n + i] /= U[i * n + i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
delete[] APrime;
|
||||
}
|
||||
|
||||
/*
|
||||
* matrix inverse code for any square matrix using LU decomposition
|
||||
* inv = inv(U)*inv(L)*P, where L and U are triagular matrices and P the pivot matrix
|
||||
* ref: http://www.cl.cam.ac.uk/teaching/1314/NumMethods/supporting/mcmaster-kiruba-ludecomp.pdf
|
||||
* @param m, input 4x4 matrix
|
||||
* @param inv, Output inverted 4x4 matrix
|
||||
* @param n, dimension of square matrix
|
||||
* @returns false = matrix is Singular, true = matrix inversion successful
|
||||
*/
|
||||
bool mat_inverse(float *A, float *inv, uint8_t n)
|
||||
{
|
||||
float *L, *U, *P;
|
||||
bool ret = true;
|
||||
L = new float[n * n];
|
||||
U = new float[n * n];
|
||||
P = new float[n * n];
|
||||
mat_LU_decompose(A, L, U, P, n);
|
||||
|
||||
float *L_inv = new float[n * n];
|
||||
float *U_inv = new float[n * n];
|
||||
|
||||
memset(L_inv, 0, n * n * sizeof(float));
|
||||
mat_forward_sub(L, L_inv, n);
|
||||
|
||||
memset(U_inv, 0, n * n * sizeof(float));
|
||||
mat_back_sub(U, U_inv, n);
|
||||
|
||||
// decomposed matrices no longer required
|
||||
delete[] L;
|
||||
delete[] U;
|
||||
|
||||
float *inv_unpivoted = mat_mul(U_inv, L_inv, n);
|
||||
float *inv_pivoted = mat_mul(inv_unpivoted, P, n);
|
||||
|
||||
//check sanity of results
|
||||
for (uint8_t i = 0; i < n; i++) {
|
||||
for (uint8_t j = 0; j < n; j++) {
|
||||
if (isnan(inv_pivoted[i * n + j]) || isinf(inv_pivoted[i * n + j])) {
|
||||
ret = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
memcpy(inv, inv_pivoted, n * n * sizeof(float));
|
||||
|
||||
//free memory
|
||||
delete[] inv_pivoted;
|
||||
delete[] inv_unpivoted;
|
||||
delete[] P;
|
||||
delete[] U_inv;
|
||||
delete[] L_inv;
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool inverse4x4(float m[], float invOut[])
|
||||
{
|
||||
|
||||
@@ -63,7 +63,13 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
|
||||
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);
|
||||
int run_lm_ellipsoid_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[]);
|
||||
bool mat_inverse(float* A, float* inv, uint8_t n);
|
||||
|
||||
// FIXME: Change the name
|
||||
static const unsigned max_accel_sens = 3;
|
||||
|
||||
|
||||
@@ -635,6 +635,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
||||
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
|
||||
calibration_log_info(mavlink_log_pub, "Offsets: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)sphere_x[cur_mag],
|
||||
(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
|
||||
calibration_log_info(mavlink_log_pub, "Scale: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)diag_x[cur_mag],
|
||||
(double)diag_y[cur_mag], (double)diag_z[cur_mag], cur_mag);
|
||||
calibration_log_info(mavlink_log_pub, "offdiag: x: %8.4f, y: %8.4f, z: %8.4f, #%u", (double)offdiag_x[cur_mag],
|
||||
(double)offdiag_y[cur_mag], (double)offdiag_z[cur_mag], cur_mag);
|
||||
result = calibrate_return_ok;
|
||||
}
|
||||
}
|
||||
@@ -727,6 +731,9 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
||||
mscale.x_offset = sphere_x[cur_mag];
|
||||
mscale.y_offset = sphere_y[cur_mag];
|
||||
mscale.z_offset = sphere_z[cur_mag];
|
||||
mscale.x_scale = diag_x[cur_mag];
|
||||
mscale.y_scale = diag_y[cur_mag];
|
||||
mscale.z_scale = diag_z[cur_mag];
|
||||
|
||||
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user