Skip to content

Commit

Permalink
Merge pull request #5798 from iNavFlight/dzikuvx-gyro-kalman-drop-cro…
Browse files Browse the repository at this point in the history 
…ss-axis

Compute real variance
  • Loading branch information
@DzikuVx
DzikuVx committed Jun 2, 2020
2 parents 7ea19d9 + 8110138 commit 84172d2
Show file tree
Hide file tree
Showing 2 changed files with 49 additions and 97 deletions.
103 changes: 39 additions & 64 deletions src/main/flight/kalman.c
View file
Expand Up @@ -29,17 +29,18 @@ FILE_COMPILE_FOR_SPEED
#include "build/debug.h"

kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT];
variance_t varStruct;
float setPoint[XYZ_AXIS_COUNT];

static void gyroKalmanInitAxis(kalman_t *filter, float q)
static void gyroKalmanInitAxis(kalman_t *filter)
{
memset(filter, 0, sizeof(kalman_t));
filter->q = q * 0.03f; //add multiplier to make tuning easier
filter->q = gyroConfig()->kalman_q * 0.03f; //add multiplier to make tuning easier
filter->r = 88.0f; //seeding R at 88.0f
filter->p = 30.0f; //seeding P at 30.0f
filter->e = 1.0f;
filter->s = gyroConfig()->kalman_sharpness / 10.0f;
filter->s = gyroConfig()->kalman_sharpness / 10.0f;
filter->w = gyroConfig()->kalman_w * 8;
filter->inverseN = 1.0f / (float)(filter->w);
}

void gyroKalmanSetSetpoint(uint8_t axis, float rate)
Expand All @@ -49,65 +50,9 @@ void gyroKalmanSetSetpoint(uint8_t axis, float rate)

void gyroKalmanInitialize(void)
{
memset(&varStruct, 0, sizeof(varStruct));
gyroKalmanInitAxis(&kalmanFilterStateRate[X], gyroConfig()->kalman_q);
gyroKalmanInitAxis(&kalmanFilterStateRate[Y], gyroConfig()->kalman_q);
gyroKalmanInitAxis(&kalmanFilterStateRate[Z], gyroConfig()->kalman_q);

varStruct.w = gyroConfig()->kalman_w * 8;
varStruct.inverseN = 1.0f / (float)(varStruct.w);
}

static void update_kalman_covariance(float *gyroRateData)
{
varStruct.xWindow[varStruct.windex] = gyroRateData[X];
varStruct.yWindow[varStruct.windex] = gyroRateData[Y];
varStruct.zWindow[varStruct.windex] = gyroRateData[Z];

varStruct.xSumMean += varStruct.xWindow[varStruct.windex];
varStruct.ySumMean += varStruct.yWindow[varStruct.windex];
varStruct.zSumMean += varStruct.zWindow[varStruct.windex];
varStruct.xSumVar = varStruct.xSumVar + (varStruct.xWindow[varStruct.windex] * varStruct.xWindow[varStruct.windex]);
varStruct.ySumVar = varStruct.ySumVar + (varStruct.yWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
varStruct.zSumVar = varStruct.zSumVar + (varStruct.zWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
varStruct.xySumCoVar = varStruct.xySumCoVar + (varStruct.xWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
varStruct.xzSumCoVar = varStruct.xzSumCoVar + (varStruct.xWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
varStruct.yzSumCoVar = varStruct.yzSumCoVar + (varStruct.yWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
varStruct.windex++;
if (varStruct.windex >= varStruct.w)
{
varStruct.windex = 0;
}
varStruct.xSumMean -= varStruct.xWindow[varStruct.windex];
varStruct.ySumMean -= varStruct.yWindow[varStruct.windex];
varStruct.zSumMean -= varStruct.zWindow[varStruct.windex];
varStruct.xSumVar = varStruct.xSumVar - (varStruct.xWindow[varStruct.windex] * varStruct.xWindow[varStruct.windex]);
varStruct.ySumVar = varStruct.ySumVar - (varStruct.yWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
varStruct.zSumVar = varStruct.zSumVar - (varStruct.zWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
varStruct.xySumCoVar = varStruct.xySumCoVar - (varStruct.xWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
varStruct.xzSumCoVar = varStruct.xzSumCoVar - (varStruct.xWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
varStruct.yzSumCoVar = varStruct.yzSumCoVar - (varStruct.yWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);

varStruct.xMean = varStruct.xSumMean * varStruct.inverseN;
varStruct.yMean = varStruct.ySumMean * varStruct.inverseN;
varStruct.zMean = varStruct.zSumMean * varStruct.inverseN;

varStruct.xVar = fabsf(varStruct.xSumVar * varStruct.inverseN - (varStruct.xMean * varStruct.xMean));
varStruct.yVar = fabsf(varStruct.ySumVar * varStruct.inverseN - (varStruct.yMean * varStruct.yMean));
varStruct.zVar = fabsf(varStruct.zSumVar * varStruct.inverseN - (varStruct.zMean * varStruct.zMean));
varStruct.xyCoVar = fabsf(varStruct.xySumCoVar * varStruct.inverseN - (varStruct.xMean * varStruct.yMean));
varStruct.xzCoVar = fabsf(varStruct.xzSumCoVar * varStruct.inverseN - (varStruct.xMean * varStruct.zMean));
varStruct.yzCoVar = fabsf(varStruct.yzSumCoVar * varStruct.inverseN - (varStruct.yMean * varStruct.zMean));

float squirt;
arm_sqrt_f32(varStruct.xVar + varStruct.xyCoVar + varStruct.xzCoVar, &squirt);
kalmanFilterStateRate[X].r = squirt * VARIANCE_SCALE;

arm_sqrt_f32(varStruct.yVar + varStruct.xyCoVar + varStruct.yzCoVar, &squirt);
kalmanFilterStateRate[Y].r = squirt * VARIANCE_SCALE;

arm_sqrt_f32(varStruct.zVar + varStruct.yzCoVar + varStruct.xzCoVar, &squirt);
kalmanFilterStateRate[Z].r = squirt * VARIANCE_SCALE;
gyroKalmanInitAxis(&kalmanFilterStateRate[X]);
gyroKalmanInitAxis(&kalmanFilterStateRate[Y]);
gyroKalmanInitAxis(&kalmanFilterStateRate[Z]);
}

float kalman_process(kalman_t *kalmanState, float input, float target)
Expand Down Expand Up @@ -142,9 +87,39 @@ float kalman_process(kalman_t *kalmanState, float input, float target)
return kalmanState->x;
}

static void updateAxisVariance(kalman_t *kalmanState, float rate)
{
kalmanState->axisWindow[kalmanState->windex] = rate;

kalmanState->axisSumMean += kalmanState->axisWindow[kalmanState->windex];
float varianceElement = kalmanState->axisWindow[kalmanState->windex] - kalmanState->axisMean;
varianceElement = varianceElement * varianceElement;
kalmanState->axisSumVar += varianceElement;
kalmanState->varianceWindow[kalmanState->windex] = varianceElement;

kalmanState->windex++;
if (kalmanState->windex >= kalmanState->w) {
kalmanState->windex = 0;
}

kalmanState->axisSumMean -= kalmanState->axisWindow[kalmanState->windex];
kalmanState->axisSumVar -= kalmanState->varianceWindow[kalmanState->windex];

//New mean
kalmanState->axisMean = kalmanState->axisSumMean * kalmanState->inverseN;
kalmanState->axisVar = kalmanState->axisSumVar * kalmanState->inverseN;

float squirt;
arm_sqrt_f32(kalmanState->axisVar, &squirt);
kalmanState->r = squirt * VARIANCE_SCALE;
}

void gyroKalmanUpdate(float *input, float *output)
{
update_kalman_covariance(input);
updateAxisVariance(&kalmanFilterStateRate[X], input[X]);
updateAxisVariance(&kalmanFilterStateRate[Y], input[Y]);
updateAxisVariance(&kalmanFilterStateRate[Z], input[Z]);

output[X] = kalman_process(&kalmanFilterStateRate[X], input[X], setPoint[X]);
output[Y] = kalman_process(&kalmanFilterStateRate[Y], input[Y], setPoint[Y]);
output[Z] = kalman_process(&kalmanFilterStateRate[Z], input[Z], setPoint[Z]);
Expand Down
43 changes: 10 additions & 33 deletions src/main/flight/kalman.h
View file
Expand Up @@ -27,39 +27,6 @@

#define VARIANCE_SCALE 0.67f

typedef struct variance
{
float xVar;
float yVar;
float zVar;
float xyCoVar;
float xzCoVar;
float yzCoVar;

uint32_t windex;
float xWindow[MAX_KALMAN_WINDOW_SIZE];
float yWindow[MAX_KALMAN_WINDOW_SIZE];
float zWindow[MAX_KALMAN_WINDOW_SIZE];

float xSumMean;
float ySumMean;
float zSumMean;

float xMean;
float yMean;
float zMean;

float xSumVar;
float ySumVar;
float zSumVar;
float xySumCoVar;
float xzSumCoVar;
float yzSumCoVar;

float inverseN;
uint16_t w;
} variance_t;

typedef struct kalman
{
float q; //process noise covariance
Expand All @@ -70,6 +37,16 @@ typedef struct kalman
float lastX; //previous state
float e;
float s;

float axisVar;
uint16_t windex;
float axisWindow[MAX_KALMAN_WINDOW_SIZE];
float varianceWindow[MAX_KALMAN_WINDOW_SIZE];
float axisSumMean;
float axisMean;
float axisSumVar;
float inverseN;
uint16_t w;
} kalman_t;

void gyroKalmanInitialize(void);
Expand Down

0 comments on commit 84172d2

Please sign in to comment.