mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 03:20:00 +03:00
Improved Magnetometer / Compass Bias Estimator (#12998)
* 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 <mark@numloq.nl> * Update src/main/sensors/compass.h Co-authored-by: Mark Haslinghuis <mark@numloq.nl> * Update src/main/sensors/compass.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Changes according to PR comments --------- Co-authored-by: Mark Haslinghuis <mark@numloq.nl> Co-authored-by: Petr Ledvina <ledvinap@gmail.com>
This commit is contained in:
pichim
GitHub
parent
9618dc8451
commit
84a04db8b9
4 changed files with 182 additions and 25 deletions
|
|
@ -116,4 +116,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"DSHOT_TELEMETRY_COUNTS",
|
||||
"RPM_LIMIT",
|
||||
"RC_STATS",
|
||||
"MAG_CALIB",
|
||||
};
|
||||
|
|
|
|||
|
|
@ -116,6 +116,7 @@ typedef enum {
|
|||
DEBUG_DSHOT_TELEMETRY_COUNTS,
|
||||
DEBUG_RPM_LIMIT,
|
||||
DEBUG_RC_STATS,
|
||||
DEBUG_MAG_CALIB,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
||||
|
|
@ -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;
|
||||
|
|
@ -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)
|
||||
{
|
||||
|
|
@ -339,7 +355,6 @@ bool compassInit(void)
|
|||
LED1_ON;
|
||||
magDev.init(&magDev);
|
||||
LED1_OFF;
|
||||
magInit = 1;
|
||||
|
||||
magDev.magAlignment = alignment;
|
||||
|
||||
|
|
@ -349,6 +364,8 @@ bool compassInit(void)
|
|||
|
||||
buildRotationMatrixFromAlignment(&compassConfig()->mag_customAlignment, &magDev.rotationMatrix);
|
||||
|
||||
compassBiasEstimatorInit(&compassBiasEstimator, LAMBDA_MIN, P0);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
@ -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();
|
||||
}
|
||||
|
||||
saveConfigAndNotify();
|
||||
}
|
||||
}
|
||||
|
||||
// remove bias
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
mag.magADC[axis] -= magZero->raw[axis];
|
||||
}
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue