Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Fast Kalman Gyro Filter: Implementation and parameter groups only

Browse source 
Signed-off-by: AJ Christensen <aj@junglistheavy.industries>
This commit is contained in:
Kalyn Doerr 2018-01-07 11:37:24 +13:00 • committed by AJ Christensen
parent 172c1e370b
commit 6e6aafe6d5
6 changed files with 89 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

30
src/main/common/filter.c
View file

@ -302,3 +302,33 @@ float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input)
return filter->movingSum / ++filter->filledCount + 1;
}
}
// Fast two-state Kalman
void fastKalmanInit(fastKalman_t *filter, float q, float r, float p)
{
filter->q = q * 0.000001f; // add multiplier to make tuning easier
filter->r = r * 0.001f; // add multiplier to make tuning easier
filter->p = p * 0.001f; // add multiplier to make tuning easier
filter->x = 0.0f; // set initial value, can be zero if unknown
filter->lastX = 0.0f; // set initial value, can be zero if unknown
filter->k = 0.0f; // kalman gain
}
FAST_CODE float fastKalmanUpdate(fastKalman_t *filter, float input)
{
// project the state ahead using acceleration
filter->x += (filter->x - filter->lastX);
// update last state
filter->lastX = filter->x;
// prediction update
filter->p = filter->p + filter->q;
// measurement update
filter->k = filter->p / (filter->p + filter->r);
filter->x += filter->k * (input - filter->x);
filter->p = (1.0f - filter->k) * filter->p;
return filter->x;
}

12
src/main/common/filter.h
View file

@ -53,6 +53,15 @@ typedef struct firFilterDenoise_s {
float state[MAX_FIR_DENOISE_WINDOW_SIZE];
} firFilterDenoise_t;
typedef struct fastKalman_s {
float q; // process noise covariance
float r; // measurement noise covariance
float p; // estimation error covariance matrix
float k; // kalman gain
float x; // state
float lastX; // previous state
} fastKalman_t;
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
@ -87,6 +96,9 @@ float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input);
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
void fastKalmanInit(fastKalman_t *filter, float q, float r, float p);
float fastKalmanUpdate(fastKalman_t *filter, float input);
// not exactly correct, but very very close and much much faster
#define filterGetNotchQApprox(centerFreq, cutoff) ((float)(cutoff * centerFreq) / ((float)(centerFreq - cutoff) * (float)(centerFreq + cutoff)))

5
src/main/interface/settings.c
View file

@ -356,6 +356,11 @@ const clivalue_t valueTable[] = {
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) },
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
#if defined(USE_GYRO_FAST_KALMAN)
{ "gyro_kalman_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_kalman_q) },
{ "gyro_kalman_r", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_kalman_r) },
{ "gyro_kalman_p", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_kalman_p) },
#endif
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
#ifdef USE_GYRO_OVERFLOW_CHECK
{ "gyro_overflow_detect", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_OVERFLOW_CHECK }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) },

39
src/main/sensors/gyro.c
View file

@ -118,6 +118,11 @@ typedef struct gyroSensor_s {
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
timeUs_t overflowTimeUs;
bool overflowDetected;
#if defined(USE_GYRO_FAST_KALMAN)
// gyro kalman filter
filterApplyFnPtr fastKalmanApplyFn;
fastKalman_t fastKalman[XYZ_AXIS_COUNT];
#endif
} gyroSensor_t;
STATIC_UNIT_TESTED FAST_RAM gyroSensor_t gyroSensor1;
@ -126,6 +131,9 @@ STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyroSensor1;
STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
#endif
#if defined(USE_GYRO_FAST_KALMAN)
static void gyroInitFilterKalman(gyroSensor_t *gyroSensor, uint16_t gyro_kalman_q, uint16_t gyro_kalman_r, uint16_t gyro_kalman_p);
#endif
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
#define DEBUG_GYRO_CALIBRATION 3
@ -155,7 +163,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_soft_notch_cutoff_1 = 300,
.gyro_soft_notch_hz_2 = 200,
.gyro_soft_notch_cutoff_2 = 100,
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
.gyro_kalman_q = 0,
.gyro_kalman_r = 0,
.gyro_kalman_p = 0,
);
@ -542,10 +553,28 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
}
#endif
#if defined(USE_GYRO_FAST_KALMAN)
static void gyroInitFilterKalman(gyroSensor_t *gyroSensor, uint16_t gyro_kalman_q, uint16_t gyro_kalman_r, uint16_t gyro_kalman_p)
{
gyroSensor->fastKalmanApplyFn = nullFilterApply;
// If Kalman Filter noise covariances for Process and Measurement are non-zero, we treat as enabled
if (gyro_kalman_q != 0 && gyro_kalman_r != 0) {
gyroSensor->fastKalmanApplyFn = (filterApplyFnPtr)fastKalmanUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fastKalmanInit(&gyroSensor->fastKalman[axis], gyro_kalman_q, gyro_kalman_r, gyro_kalman_p);
}
}
}
#endif
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
{
#if defined(USE_GYRO_SLEW_LIMITER)
gyroInitSlewLimiter(gyroSensor);
#endif
#if defined(USE_GYRO_FAST_KALMAN)
gyroInitFilterKalman(gyroSensor, gyroConfig()->gyro_kalman_q, gyroConfig()->gyro_kalman_r, gyroConfig()->gyro_kalman_p);
#endif
gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
@ -749,6 +778,9 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch
float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
#if defined(USE_GYRO_FAST_KALMAN)
gyroADCf = gyroSensor->fastKalmanApplyFn((filter_t *)&gyroSensor->fastKalman[axis], gyroADCf);
#endif
#ifdef USE_GYRO_DATA_ANALYSE
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
#endif
@ -770,6 +802,11 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
// DEBUG_GYRO_NOTCH records the unfiltered gyro output
DEBUG_SET(DEBUG_GYRO_NOTCH, axis, lrintf(gyroADCf));
#if defined(USE_GYRO_FAST_KALMAN)
// apply fast kalman
gyroADCf = gyroSensor->fastKalmanApplyFn((filter_t *)&gyroSensor->fastKalman[axis], gyroADCf);
#endif
#ifdef USE_GYRO_DATA_ANALYSE
// apply dynamic notch filter
if (isDynamicFilterActive()) {

3
src/main/sensors/gyro.h
View file

@ -70,6 +70,9 @@ typedef struct gyroConfig_s {
uint16_t gyro_soft_notch_hz_2;
uint16_t gyro_soft_notch_cutoff_2;
gyroOverflowCheck_e checkOverflow;
uint16_t gyro_kalman_q;
uint16_t gyro_kalman_r;
uint16_t gyro_kalman_p;
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);

1
src/main/target/common_fc_pre.h
View file

@ -170,4 +170,5 @@
#define USE_NAV
#define USE_TELEMETRY_IBUS
#define USE_UNCOMMON_MIXERS
#define USE_GYRO_FAST_KALMAN
#endif