Skip to content

107 ramuh #27

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
rs2k wants to merge 2 commits into
base: master
Choose a base branch
from
Open

107 ramuh #27

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cdd4db3
quashing ramuh and fixing build issue
orneryd Jun 28, 2018
a817fb1
alternate covariance calculation, feel free to revert if it doesn't work
orneryd Jun 28, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
105 changes: 59 additions & 46 deletions src/filter/kalman.c
View file
Open in desktop
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.xyCorrelation + varStruct.xzCorrelation) * VARIANCE_SCALE)));
kalmanFilterStateRate[PITCH].r = ABS((varStruct.yMean * ((varStruct.xyCorrelation + varStruct.yzCorrelation) * VARIANCE_SCALE)));
kalmanFilterStateRate[YAW].r = ABS((varStruct.zMean * ((varStruct.yzCorrelation + 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
11 changes: 9 additions & 2 deletions src/filter/kalman.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@
#define DEF_WINDOW_SIZE 32
#define MIN_WINDOW_SIZE 6

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

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