AGGIORNAMENTI:
Sono riuscito a trovare l'angolo assoluto sia di gyroscopio che di accelerometro ed ho provato ad usare sia il filtro di kalman che quello complementare: quello di kalman mi da come risultato lo stesso valore identico dell'angolo dell'accelerometro, mentre quello complementare (di 1° livello) mi da in uscita un valore accettabile. Il codice che ho usato é il seguente:
#include <Servo.h>
#include <Wire.h>
#include <ITG3200.h>
#include <bma180.h>
#include <math.h>
#define address 0x40#define RAD_TO_DEG 57.295779513082320876798154814105
ITG3200 gyro = ITG3200();
BMA180 bma180 = BMA180();
Servo myservo;
int accAxis[3];
float gyroAxis[3];
float angleGyro[3];
const int X=0, Y=1, Z=2;
float accErr[3] = {0.00, 1.50, 0.00};
long previousMicros = 0;
unsigned long currentMicros;//###### COMPLEMENTARY #######
float tau=0.01;
float a=0.0;
float x_angleC=0;
//##### END COMPLEMENTARY ####//######## KALMAN ##########
float Q_angle = 0.001;//0.01; //0.001
float Q_gyro = 0.003;//0.0003; //0.003
float R_angle = 0.03;//0.01; //0.03
float x_angle = 0;
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float y, S;
float K_0, K_1;
//###### END KALMAN ########long intervallo2, currentMillis, previousMillis;
void setup(void) {
Serial.begin(115200);
Wire.begin();
Serial.println("Inizializzazione accelerometro...");
bma180.setAddress(BMA180_ADDRESS_SDO_LOW);
bma180.SoftReset();
bma180.enableWrite();
bma180.SetFilter(bma180.F10HZ);
bma180.setGSensitivty(bma180.G1);
bma180.SetSMPSkip();
bma180.SetISRMode();
bma180.disableWrite();
Serial.println("Completato.");
myservo.attach(9);
gyro.init(ITG3200_ADDR_AD0_LOW);Serial.print("Calibrazione zero gyro...");
gyro.zeroCalibrate(2500, 2);
Serial.println("Completato.");
delay(100);
previousMicros = micros();
}float gyroY=0;
boolean stato=false;void loop(void) {
currentMillis=millis();while (gyro.isRawDataReady()) {
currentMicros = micros();
float intervallo = currentMicros-previousMicros;
currentMillis=millis();
intervallo2=currentMillis-previousMillis;
if (intervallo2>500 && stato==false)
{
myservo.write(120);
stato=true;
previousMillis=currentMillis;
}
else if (intervallo2>500 && stato==true)
{
myservo.write(60);
stato=false;
previousMillis=currentMillis;
}bma180.readAccel(accAxis);
gyro.readGyro(&gyroAxis[X], &gyroAxis[Y], &gyroAxis[Z]);
//gyroY+=gyroAxis[Y]*(intervallo/1000000); //FUNZIONA
float angolo = Complementary(getAngleAcc(Y),gyroY, intervallo);/*Serial.print(getAngleAcc(Y));
Serial.print("\t");Serial.print((float)((int)(-gyroY10))/10);
//Serial.print(getAngleGyro(Y, intervallo));
Serial.print("\t");/
Serial.println((float)(int)((angolo)*10)/10);
previousMicros = currentMicros;}
}float mapFloat(int x, int in_min, int in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}float getAngleGyro(int asse, float tempo)
{
angleGyro[Y]+=gyroAxis[Y](tempo/1000000);
return angleGyro[Y];
//angleGyro[asse]+=gyroAxis[asse](tempo/1000000);
//return angleGyro[asse];
}float getAngleAcc(int asse)
{
float angVal=0;
angVal = mapFloat(accAxis[asse], -8191,8191, -1, 1);
return acos(angVal)*RAD_TO_DEG-90-accErr[Y];//(float)(int)((acos(angVal)*RAD_TO_DEG-90-accErr[Y])*10)/10;
}float kalmanCalculate(float newAngle, float newRate,int looptime)
{
float dt = float(looptime)/1000;
x_angle += dt * (newRate - x_bias);
P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;x_angle += K_0 * y;
x_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;return x_angle;
}float Complementary(float newAngle, float newRate,int looptime) {
float dtC = float(looptime)/1000.0;
a=tau/(tau+dtC);
x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
return x_angleC;
}
A questo punto ho montato tutta l'imu su un servo facendola girare di 60° (da 60 a 120) ed ho ottenuto il grafico in allegato. Secondo voi come è il risultato del filtro?
