Skip to content

Commit

Permalink
Improved Magnetometer / Compass Bias Estimator (betaflight#12998)
Browse files Browse the repository at this point in the history 
* Rebased on master and clean-up (untested), this is still the initial method

* Included math.h in compass.c, so checks don't fail

* Change comments, compass is still only running at 167 Hz, not worth investing more

* Alternative method, using only mag data

* Removed unnecessary scaling and fine tuning

* Corrected comment

* Removed no longer needed debug DEBUG_MAG_TASK_RATE, comments and corrected some typos

* Improved readability and update of comment

* Rebased on master and resolved conflicts

* Included math.h and maths.h so checks don't fail

* Update src/main/sensors/compass.c

Co-authored-by: Mark Haslinghuis <[email protected]>

* Update src/main/sensors/compass.h

Co-authored-by: Mark Haslinghuis <[email protected]>

* Update src/main/sensors/compass.c

Co-authored-by: Petr Ledvina <[email protected]>

* Changes according to PR comments

---------

Co-authored-by: Mark Haslinghuis <[email protected]>
Co-authored-by: Petr Ledvina <[email protected]>
  • Loading branch information
@pichim @haslinghuis @ledvinap
3 people authored Nov 15, 2023
1 parent 9618dc8 commit 84a04db
Show file tree
Hide file tree
Showing 4 changed files with 181 additions and 24 deletions.
1 change: 1 addition & 0 deletions src/main/build/debug.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,4 +116,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"DSHOT_TELEMETRY_COUNTS",
"RPM_LIMIT",
"RC_STATS",
"MAG_CALIB",
};
1 change: 1 addition & 0 deletions src/main/build/debug.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,7 @@ typedef enum {
DEBUG_DSHOT_TELEMETRY_COUNTS,
DEBUG_RPM_LIMIT,
DEBUG_RC_STATS,
DEBUG_MAG_CALIB,
DEBUG_COUNT
} debugType_e;

Expand Down
191 changes: 168 additions & 23 deletions src/main/sensors/compass.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,16 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>

#include "platform.h"

#if defined(USE_MAG)

#include "build/debug.h"

#include "common/axis.h"
#include "common/maths.h"

#include "config/config.h"

Expand Down Expand Up @@ -62,9 +66,22 @@

#include "compass.h"

#define LAMBDA_MIN 0.95f // minimal adaptive forgetting factor, range: [0.90, 0.99], currently tuned for 200 Hz
// (TASK_COMPASS_RATE_HZ) and update rate of compassBiasEstimatorApply(), not the mag readout
// rate.
// offline evaluations showed that 10 Hz is ok, 50 Hz is better and above 100 Hz is good.
// TBC with online tests.
#define P0 1.0e2f // value to initialize P(0) = diag([P0, P0, P0, P0]), typically in range: (1, 1000)

#define CALIBRATION_WAIT_US (15 * 1000 * 1000) // wait for movement to start and trigger the calibration routine in us
#define GYRO_NORM_SQUARED_MIN sq(DEGREES_TO_RADIANS(450.0f)) // minimal value that has to be reached so that the calibration routine starts in (rad/sec)^2,
// a relatively high value so that the calibration routine is not triggered too early
#define CALIBRATION_TIME_US (30 * 1000 * 1000) // duration of the calibration phase in us

static timeUs_t tCal = 0;
static flightDynamicsTrims_t magZeroTempMin;
static flightDynamicsTrims_t magZeroTempMax;
static bool didMovementStart = false;

static compassBiasEstimator_t compassBiasEstimator;

magDev_t magDev;
mag_t mag;
Expand Down Expand Up @@ -120,7 +137,6 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig)

static int16_t magADCRaw[XYZ_AXIS_COUNT];
static int16_t magADCRawPrevious[XYZ_AXIS_COUNT];
static uint8_t magInit = 0;

void compassPreInit(void)
{
Expand Down Expand Up @@ -339,7 +355,6 @@ bool compassInit(void)
LED1_ON;
magDev.init(&magDev);
LED1_OFF;
magInit = 1;

magDev.magAlignment = alignment;

Expand All @@ -349,6 +364,8 @@ bool compassInit(void)

buildRotationMatrixFromAlignment(&compassConfig()->mag_customAlignment, &magDev.rotationMatrix);

compassBiasEstimatorInit(&compassBiasEstimator, LAMBDA_MIN, P0);

return true;
}

Expand All @@ -359,13 +376,16 @@ bool compassIsHealthy(void)

void compassStartCalibration(void)
{
tCal = micros();
// starting now, the user has CALIBRATION_WAIT_US to start moving the quad and trigger the actual calibration routine
tCal = micros() + CALIBRATION_WAIT_US;
flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero;
for (int axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
magZero->raw[axis] = 0;
magZeroTempMin.raw[axis] = mag.magADC[axis];
magZeroTempMax.raw[axis] = mag.magADC[axis];
}
didMovementStart = false;

// reset / update the compass bias estimator for faster convergence
compassBiasEstimatorUpdate(&compassBiasEstimator, LAMBDA_MIN, P0);
}

bool compassIsCalibrationComplete(void)
Expand Down Expand Up @@ -402,36 +422,161 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
}

flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero;
if (magInit) { // we apply offset only once mag calibration is done
mag.magADC[X] -= magZero->raw[X];
mag.magADC[Y] -= magZero->raw[Y];
mag.magADC[Z] -= magZero->raw[Z];
}

if (tCal != 0) {
if ((currentTimeUs - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
if (cmpTimeUs(tCal, currentTimeUs) > 0) {
LED0_TOGGLE;
for (int axis = 0; axis < 3; axis++) {
if (mag.magADC[axis] < magZeroTempMin.raw[axis])
magZeroTempMin.raw[axis] = mag.magADC[axis];
if (mag.magADC[axis] > magZeroTempMax.raw[axis])
magZeroTempMax.raw[axis] = mag.magADC[axis];

// it is assumed that the user has started to move the quad if squared norm of rotational speed vector is greater than GYRO_NORM_SQUARED_MIN
float gyroNormSquared = 0.0f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroNormSquared += sq(DEGREES_TO_RADIANS(gyroGetFilteredDownsampled(axis)));
}

// check if movement has started
if (!didMovementStart && gyroNormSquared > GYRO_NORM_SQUARED_MIN) {
didMovementStart = true;
// starting now, the user has CALIBRATION_TIME_US to move the quad in a figure of eight in all directions
tCal = micros() + CALIBRATION_TIME_US;
}

// only start calibration after the user has started to move the quad
if (didMovementStart) {
compassBiasEstimatorApply(&compassBiasEstimator, mag.magADC);
}
} else {
tCal = 0;
for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets
// only copy the estimated bias and save it to the config if the user has actually triggered the calibration routine
if (didMovementStart) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
magZero->raw[axis] = lrintf(compassBiasEstimator.b[axis]);
}
saveConfigAndNotify();
}
}
}

// remove bias
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
mag.magADC[axis] -= magZero->raw[axis];
}

saveConfigAndNotify();
if (debugMode == DEBUG_MAG_CALIB) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// DEBUG 0-2: magADC[X], magADC[Y], magADC[Z]
DEBUG_SET(DEBUG_MAG_CALIB, axis, lrintf(mag.magADC[axis]));
// DEBUG 4-6: estimated magnetometer bias
DEBUG_SET(DEBUG_MAG_CALIB, axis + 4, lrintf(compassBiasEstimator.b[axis]));
}
// DEBUG 3: norm / length of magADC, ideally the norm stays constant independent of the orientation of the quad
DEBUG_SET(DEBUG_MAG_CALIB, 3, lrintf(sqrtf(sq(mag.magADC[X]) + sq(mag.magADC[Y]) + sq(mag.magADC[Z]))));
// map adaptive forgetting factor lambda from (lambda_min, 1.0f) -> (0, 2000)
const float mapLambdaGain = 1.0f / (1.0f - compassBiasEstimator.lambda_min + 1.0e-6f) * 2.0e3f;
// DEBUG 7: adaptive forgetting factor lambda, after the transient phase it should converge to 2000
DEBUG_SET(DEBUG_MAG_CALIB, 7, lrintf((compassBiasEstimator.lambda - compassBiasEstimator.lambda_min) * mapLambdaGain));
}

nextPeriod = TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ) - COMPASS_READ_US * busyCount;

// Reset the busy count
busyCount = 0;

return nextPeriod;
}

// initialize the compass bias estimator
void compassBiasEstimatorInit(compassBiasEstimator_t *cBE, const float lambda_min, const float p0)
{
memset(cBE, 0, sizeof(*cBE)); // zero contained IEEE754 floats
// create identity matrix
for (unsigned i = 0; i < 4; i++) {
cBE->U[i][i] = 1.0f;
}

compassBiasEstimatorUpdate(cBE, lambda_min, p0);

cBE->lambda = lambda_min;
}

// reset / update the compass bias estimator, this can be used after the compass bias estimator did
// already run to achieve faster convergence for the next run
void compassBiasEstimatorUpdate(compassBiasEstimator_t *cBE, const float lambda_min, const float p0)
{
cBE->lambda_min = lambda_min;
// update diagonal entries for faster convergence
for (unsigned i = 0; i < 4; i++) {
cBE->D[i] = p0;
}
}

// apply one estimation step of the compass bias estimator
void compassBiasEstimatorApply(compassBiasEstimator_t *cBE, float *mag)
{
// update phi
float phi[4];
phi[0] = sq(mag[0]) + sq(mag[1]) + sq(mag[2]);
for (unsigned i = 0; i < 3; i++) {
phi[i + 1] = mag[i];
}

// update e
float e = 1.0f;
for (unsigned i = 0; i < 4; i++) {
e -= phi[i] * cBE->theta[i];
}

// U D U^T
float f[4];
float v[4];
for (unsigned i = 0; i < 4; i++) {
f[i] = 0.0f;
for (unsigned j = 0; j <= i; j++) {
f[i] += cBE->U[j][i] * phi[j];
}
v[i] = cBE->D[i] * f[i];
}

// first iteration
float alpha[4];
float k[4] = {0};
alpha[0] = cBE->lambda + v[0] * f[0];
cBE->D[0] /= alpha[0];
k[0] = v[0];

// rest of the iterations
for (unsigned i = 1; i < 4; i++) {
alpha[i] = alpha[i - 1] + v[i] * f[i];
cBE->D[i] *= alpha[i - 1] / (alpha[i] * cBE->lambda);
for (unsigned j = 0; j < i; j++) {
float dU = -(f[i] / alpha[i - 1]) * k[j];
k[j] += v[i] * cBE->U[j][i];
cBE->U[j][i] += dU;
}
k[i] += v[i];
}

// parameter-update
for (unsigned i = 0; i < 4; i++) {
cBE->theta[i] += (k[i] / alpha[3]) * e;
}

// bias update
for (unsigned i = 0; i < 3; i++) {
cBE->b[i] = -0.5f * cBE->theta[i + 1] / cBE->theta[0];
}

// compute zn
float U_v;
float phiTrans_U_v = 0.0f;
for (unsigned i = 0; i < 4; i++) {
U_v = 0.0f;
for (unsigned j = i; j < 4; j++) {
U_v += cBE->U[i][j] * v[j];
}
phiTrans_U_v += phi[i] * U_v;
}
float zn = cBE->lambda / (cBE->lambda + phiTrans_U_v);

// update lambda
cBE->lambda = cBE->lambda_min + (1.0f - cBE->lambda_min) * sq(zn);
}
#endif // USE_MAG
12 changes: 11 additions & 1 deletion src/main/sensors/compass.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,14 @@ typedef struct compassConfig_s {
sensorAlignment_t mag_customAlignment;
} compassConfig_t;

typedef struct compassBiasEstimator_s {
float lambda_min, lambda;
float b[3];
float theta[4];
float U[4][4];
float D[4];
} compassBiasEstimator_t;

PG_DECLARE(compassConfig_t, compassConfig);

bool compassIsHealthy(void);
Expand All @@ -70,4 +78,6 @@ bool compassInit(void);
void compassPreInit(void);
void compassStartCalibration(void);
bool compassIsCalibrationComplete(void);

void compassBiasEstimatorInit(compassBiasEstimator_t *compassBiasEstimator, const float lambda_min, const float p0);
void compassBiasEstimatorUpdate(compassBiasEstimator_t *compassBiasEstimator, const float lambda_min, const float p0);
void compassBiasEstimatorApply(compassBiasEstimator_t *cBE, float *mag);

0 comments on commit 84a04db

Please sign in to comment.