@lauszus

Sorry for the late reply, we've had a lot of work in school these days

As far as I know, the gyro and acc have the same scale, oriented in the same direction, reflect accurate measurements, etc. but nonetheless kalman filtering still produces false (not true to original signal) results. Adjusting the constants of the kalman filtering seem to make the random jumps only more prominent - so I'll just post the code here, and hopefully someone can see my mistake

I'll just post the relevant code (leaving out PID, etc.)

Here is the main file (with setup/loop):

#include <math.h>

#define GYR_X 0

#define GYR_Y 1

#define ACC_X 2

#define ACC_Y 3

#define ACC_Z 4

const float RAD_DEG = 57.2957795;

const float pi = 3.1415926;

const float QUID_DEG = 0.3515625;

const float DEG_QUID = 2.8444444;

int sensorValue[5] = {0,0,0,0,0};

int sensorZero[5] = {0,0,0,0,0};

float xACC_angle = 0;

float xACC_angleDeg = 0;

float xACC_angleQuid = 0;

int xGYRO_rate = 0;

float xGYRO_angle = 0;

float yACC_angle = 0;

float yACC_angleDeg = 0;

float yACC_angleQuid = 0;

int yGYRO_rate = 0;

float yGYRO_angle = 0;

float xfinAngleQuid = 0;

float xfinAngleDeg = 0;

float yfinAngleQuid = 0;

float yfinAngleDeg = 0;

int PIDcorrect = 0;

int error = 0;

float R = 0;

//alternate timing mechanism...

int STD_LOOP_TIME = 10;

int lastLoopTime = STD_LOOP_TIME;

int lastLoopUsefulTime = STD_LOOP_TIME;

unsigned long loopStartTime = 0;

unsigned long timer=0;

unsigned long dtime=0;

void setup() {

analogReference(EXTERNAL);

Serial.begin(115200);

delay(100);

calibrateSensors();

}

void loop() {

updateSensors();

//atan2 angle calculation method (single-axis inclination only)

/*xACC_angle = getAccAngle();

xACC_angleDeg = int (xACC_angle/2.84444);

xGYRO_rate = getGyroRate()*-1;*/

//vector angle calculation method

R = sqrt(pow(sensorValue[ACC_X],2) + pow(sensorValue[ACC_Y],2) + pow(sensorValue[ACC_Z],2));

xACC_angle = (acos(sensorValue[ACC_X]/R)-pi/2)*-1;

xACC_angleDeg = xACC_angle * RAD_DEG;

xACC_angleQuid = xACC_angle * RAD_DEG * DEG_QUID;

yACC_angle = (acos(sensorValue[ACC_Y]/R)-pi/2)*-1;

yACC_angleDeg = yACC_angle * RAD_DEG;

yACC_angleQuid = yACC_angle * RAD_DEG * DEG_QUID;

xGYRO_rate = getGyroRateX();

xGYRO_angle = xGYRO_angle + xGYRO_rate*lastLoopTime/1000;

yGYRO_rate = getGyroRateY();

yGYRO_angle = yGYRO_angle + yGYRO_rate*lastLoopTime/1000;

dtime = millis()-timer;

timer = millis();

//not working-- fix

xfinAngleDeg = kalmanCalculateX(xACC_angleDeg, xGYRO_rate, dTime);

yfinAngleDeg = kalmanCalculateY(yACC_angleDeg, yGYRO_rate, dTime);

//PIDcorrect = updatePid(0, xfinAngleDeg, dtime);

//serialOut_sensor();

serialOut_timing();

//processing();

/*lastLoopUsefulTime = millis()-loopStartTime;

if(lastLoopUsefulTime<STD_LOOP_TIME) delay(STD_LOOP_TIME-lastLoopUsefulTime);

lastLoopTime = millis() - loopStartTime;

loopStartTime = millis();*/

}

and the sensors/calibrate/read code:

void calibrateSensors() {

long v;

for(int n=0; n<5; n++) {

v=0;

for(int i=0; i<50; i++) v += readSensor(n);

sensorZero[n] = v/50;

}

sensorZero[ACC_Z] -= 166;

}

void updateSensors() {

long v;

for(int n=0; n<5; n++) {

v=0;

v = readSensor(n)+2*readSensor(n)+3*readSensor(n)+4*readSensor(n)+3*readSensor(n)+2*readSensor(n)+readSensor(n); //tri. avg

sensorValue[n] = v/16 - sensorZero[n];

}

}

int readSensor(int channel) {

return (analogRead(channel));

}

//******************************* angle calculations **************************************

int getAccAngle() {

return arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256; // in Quid: 1024/(2*PI))

}

int getGyroRateX() { // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)

return int(sensorValue[GYR_X] / 0.775); // in quid/sec:(1024/360)/1024 * 3.3/0.002)

}

int getGyroRateY() { // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)

return int(sensorValue[GYR_Y] / 0.775); // in quid/sec:(1024/360)/1024 * 3.3/0.002)

}

int arctan2(int y, int x) { // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm

int coeff_1 = 128; // angle in Quids (1024 Quids=360°)

int coeff_2 = 3*coeff_1;

float abs_y = abs(y)+1e-10;

float r, angle;

if (x >= 0) {

r = (x - abs_y) / (x + abs_y);

angle = coeff_1 - coeff_1 * r;

} else {

r = (x + abs_y) / (abs_y - x);

angle = coeff_2 - coeff_1 * r;

}

if (y < 0) return int(-angle);

else return int(angle);

}

Finally, the kalman module (unchanged constants):

float Q_angleX = 0.001;

float Q_gyroX = 0.003;

float R_angleX = 0.03;

float x_angle = 0;

float x_bias = 0;

float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;

float dtX, yX, SX;

float KX_0, KX_1;

float kalmanCalculateX(float newAngle, float newRate,int looptime) {

dtX = float(looptime)/1000; // XXXXXXX arevoir

x_angle += dtX * (newRate - x_bias);

PX_00 += - dtX * (PX_10 + PX_01) + Q_angleX * dtX;

PX_01 += - dtX * PX_11;

PX_10 += - dtX * PX_11;

PX_11 += + Q_gyroX * dtX;

yX = newAngle - x_angle;

SX = PX_00 + R_angleX;

KX_0 = PX_00 / SX;

KX_1 = PX_10 / SX;

x_angle += KX_0 * yX;

x_bias += KX_1 * yX;

PX_00 -= KX_0 * PX_00;

PX_01 -= KX_0 * PX_01;

PX_10 -= KX_1 * PX_00;

PX_11 -= KX_1 * PX_01;

return x_angle;

}

float Q_angleY = 0.001;

float Q_gyroY = 0.003;

float R_angleY = 0.03;

float y_angle = 0;

float y_bias = 0;

float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;

float dtY, yY, SY;

float KY_0, KY_1;

float kalmanCalculateY(float newAngle, float newRate,int looptime) {

dtY = float(looptime)/1000; // XXXXXXX arevoir

y_angle += dtY * (newRate - y_bias);

PY_00 += - dtY * (PY_10 + PY_01) + Q_angleY * dtY;

PY_01 += - dtY * PY_11;

PY_10 += - dtY * PY_11;

PY_11 += + Q_gyroY * dtY;

yY = newAngle - y_angle;

SY = PY_00 + R_angleY;

KY_0 = PY_00 / SY;

KY_1 = PY_10 / SY;

y_angle += KY_0 * yY;

y_bias += KY_1 * yY;

PY_00 -= KY_0 * PY_00;

PY_01 -= KY_0 * PY_01;

PY_10 -= KY_1 * PY_00;

PY_11 -= KY_1 * PY_01;

return y_angle;

}

and just for reference, this is how I'm graphing the data (basically using your processing sketch to graph custom inputs):

void processing()

{

Serial.print(xACC_angleDeg);Serial.print("\t");

Serial.print(0);Serial.print("\t");

Serial.print(yACC_angleDeg);Serial.print("\t");

Serial.print(0);Serial.print("\t");

Serial.print(xfinAngleDeg);Serial.print("\t");

Serial.print(0); Serial.print("\t");

Serial.print(yfinAngleDeg);Serial.print("\t");

Serial.print(0);Serial.print("\t");

Serial.print("\n");

}

Thanks for your help!