Skip to content

Commit

Permalink
quashing ramuh and fixing build issue
Browse files Browse the repository at this point in the history
  • Loading branch information
@orneryd
orneryd committed Jun 28, 2018
1 parent 3adb184 commit cdd4db3
Show file tree
Hide file tree
Showing 2 changed files with 67 additions and 47 deletions.
105 changes: 59 additions & 46 deletions src/filter/kalman.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,21 @@ volatile axisDataInt_t setPointInt;
volatile axisData_t setPoint;
kalman_t kalmanFilterStateRate[3];

#define DT 1 / 32000

void init_kalman(kalman_t *filter, float q)
{
memset(filter, 0, sizeof(kalman_t));
memset((uint32_t *)&setPointInt, 0, sizeof(axisDataInt_t));
memset((uint32_t *)&setPoint, 0, sizeof(axisData_t));
setPointNew = 0;
filter->q = q * 0.001f; //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->x = 0.0f; //set intial value, can be zero if unknown
filter->lastX = 0.0f; //set intial value, can be zero if unknown
filter->e = 1.0f;
filter->acc = 0.0f; //set intial value, can be zero if unknown
}

void kalman_init(void)
Expand All @@ -35,60 +42,62 @@ void kalman_init(void)
#pragma GCC optimize("O3")
void update_kalman_covariance(volatile axisData_t *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 >= filterConfig.w)
varStruct.xWindow[varStruct.windex] = gyroRateData->x - setPoint.x;
varStruct.yWindow[varStruct.windex] = gyroRateData->y - setPoint.y;
varStruct.zWindow[varStruct.windex] = gyroRateData->z - setPoint.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 >= filterConfig.w)
{
varStruct.windex = 0;
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 = ABS(varStruct.xSumVar * varStruct.inverseN - ( varStruct.xMean * varStruct.xMean));
varStruct.yVar = ABS(varStruct.ySumVar * varStruct.inverseN - ( varStruct.yMean * varStruct.yMean));
varStruct.zVar = ABS(varStruct.zSumVar * varStruct.inverseN - ( varStruct.zMean * varStruct.zMean));
varStruct.xyCoVar = ABS(varStruct.xySumCoVar * varStruct.inverseN - ( varStruct.xMean * varStruct.yMean));
varStruct.xzCoVar = ABS(varStruct.xzSumCoVar * varStruct.inverseN - ( varStruct.xMean * varStruct.zMean));
varStruct.yzCoVar = ABS(varStruct.yzSumCoVar * varStruct.inverseN - ( varStruct.yMean * varStruct.zMean));
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 = ABS(varStruct.xSumVar * varStruct.inverseN - (varStruct.xMean * varStruct.xMean));
varStruct.yVar = ABS(varStruct.ySumVar * varStruct.inverseN - (varStruct.yMean * varStruct.yMean));
varStruct.zVar = ABS(varStruct.zSumVar * varStruct.inverseN - (varStruct.zMean * varStruct.zMean));
varStruct.xyCoVar = ABS(varStruct.xySumCoVar * varStruct.inverseN - (varStruct.xMean * varStruct.yMean));
varStruct.xzCoVar = ABS(varStruct.xzSumCoVar * varStruct.inverseN - (varStruct.xMean * varStruct.zMean));
varStruct.yzCoVar = ABS(varStruct.yzSumCoVar * varStruct.inverseN - (varStruct.yMean * varStruct.zMean));

float squirt;
arm_sqrt_f32(varStruct.xVar + varStruct.xyCoVar + varStruct.xzCoVar, &squirt);
kalmanFilterStateRate[ROLL].r = squirt * VARIANCE_SCALE;
arm_sqrt_f32(varStruct.yVar + varStruct.xyCoVar + varStruct.yzCoVar, &squirt);
kalmanFilterStateRate[PITCH].r = squirt * VARIANCE_SCALE;
arm_sqrt_f32(varStruct.zVar + varStruct.yzCoVar + varStruct.xzCoVar, &squirt);
kalmanFilterStateRate[YAW].r = squirt * VARIANCE_SCALE;
arm_sqrt_f32(varStruct.xVar * varStruct.yVar, &squirt);
varStruct.xyCorrelation = varStruct.xyCoVar / squirt;
arm_sqrt_f32(varStruct.xVar * varStruct.zVar, &squirt);
varStruct.xzCorrelation = varStruct.xzCoVar / squirt;
arm_sqrt_f32(varStruct.yVar * varStruct.zVar, &squirt);
varStruct.yzCorrelation = varStruct.yzCoVar / squirt;

kalmanFilterStateRate[ROLL].r = ABS((varStruct.xMean + (varStruct.yMean * varStruct.xyCorrelation) + (varStruct.zMean * varStruct.xzCorrelation))) * VARIANCE_SCALE;
kalmanFilterStateRate[PITCH].r = ABS((varStruct.yMean + (varStruct.xMean * varStruct.xyCorrelation) + (varStruct.zMean * varStruct.yzCorrelation))) * VARIANCE_SCALE;
kalmanFilterStateRate[YAW].r = ABS((varStruct.zMean + (varStruct.yMean * varStruct.yzCorrelation) + (varStruct.xMean * varStruct.xzCorrelation))) * VARIANCE_SCALE;
}

inline float kalman_process(kalman_t* kalmanState, volatile float input, volatile float target) {
//project the state ahead using acceleration
kalmanState->x += (kalmanState->x - kalmanState->lastX);

//figure out how much to boost or reduce our error in the estimate based on setpoint target.
//this should be close to 0 as we approach the sepoint and really high the futher away we are from the setpoint.
kalmanState->x = kalmanState->lastX + (kalmanState->acc * DT);

//update last state
kalmanState->lastX = kalmanState->x;

Expand All @@ -105,6 +114,10 @@ inline float kalman_process(kalman_t* kalmanState, volatile float input, volatil
kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r);
kalmanState->x += kalmanState->k * (input - kalmanState->x);
kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p;

kalmanState->acc = (kalmanState->x - kalmanState->lastX) * DT;
kalmanState->lastX = kalmanState->x;

return kalmanState->x;
}

Expand Down
9 changes: 8 additions & 1 deletion src/filter/kalman.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
#define DEF_WINDOW_SIZE 32
#define MIN_WINDOW_SIZE 6

// #define VARIANCE_SCALE 0.001
// #define VARIANCE_SCALE 1.0f
#define VARIANCE_SCALE 0.3333333f

typedef struct kalman
Expand All @@ -17,8 +17,10 @@ typedef struct kalman
float p; //estimation error covariance matrix
float k; //kalman gain
float x; //state
float acc; //acceleration
float lastX; //previous state
float e;
float processCount; //keeps track of the process covariance
} kalman_t;

typedef struct variance
Expand Down Expand Up @@ -46,9 +48,14 @@ typedef struct variance
float xSumVar;
float ySumVar;
float zSumVar;

float xySumCoVar;
float xzSumCoVar;
float yzSumCoVar;

float xyCorrelation;
float xzCorrelation;
float yzCorrelation;

float inverseN;
} variance_t;
Expand Down

0 comments on commit cdd4db3

Please sign in to comment.