From cdd4db35bc830fd24b012718a1ee448f93386689 Mon Sep 17 00:00:00 2001 From: Tim Sweet Date: Wed, 27 Jun 2018 21:21:22 -0700 Subject: [PATCH] quashing ramuh and fixing build issue --- src/filter/kalman.c | 105 +++++++++++++++++++++++++------------------- src/filter/kalman.h | 9 +++- 2 files changed, 67 insertions(+), 47 deletions(-) diff --git a/src/filter/kalman.c b/src/filter/kalman.c index 95a71cc9..bae48c89 100644 --- a/src/filter/kalman.c +++ b/src/filter/kalman.c @@ -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) @@ -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; @@ -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; } diff --git a/src/filter/kalman.h b/src/filter/kalman.h index 2c6aef4b..3b001ccc 100644 --- a/src/filter/kalman.h +++ b/src/filter/kalman.h @@ -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 @@ -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 @@ -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;