IMU 6DoF + Filtre Kalman fait maison

Et voila le code définitif sans fautes de signes....

//--IMU 6DoF I2C + filtre de Kalman
//--Accelerometer = ADXL345
//--Gyroscope = ITG3200

#include <Wire.h> //communication I2C

int Gyro_output[3],Accel_output[3];

float dt = 0.02;

float Gyro_cal_x,Gyro_cal_y,Gyro_cal_z,Accel_cal_x,Accel_cal_y,Accel_cal_z;

float Gyro_pitch = 0;
float Accel_pitch = 0;
float Predicted_pitch = 0;

float Gyro_roll = 0;
float Accel_roll = 0;
float Predicted_roll = 0;

float Q = 0.1;
float R = 5;

float P00 = 0.1;
float P11 = 0.1;
float P01 = 0.1;

float Kk0, Kk1;

unsigned long timer;
unsigned long time;

void writeTo(byte device, byte toAddress, byte val) 
{
  Wire.beginTransmission(device);
  Wire.write(toAddress);
  Wire.write(val);
  Wire.endTransmission();
}

void readFrom(byte device, byte fromAddress, int num, byte result[])
{
  Wire.beginTransmission(device);
  Wire.write(fromAddress);
  Wire.endTransmission();
  Wire.requestFrom((int)device, num);
  
  int i = 0;
  
  while(Wire.available())
  {
    result[i] = Wire.read();
    i++;
  }
}

void getGyroscopeReadings(int Gyro_out[])
{
  byte buffer[6];
  readFrom(0x68,0x1D,6,buffer); //datasheet ITG3200
  
  Gyro_out[0]=(((int)buffer[0]) << 8 ) | buffer[1];
  Gyro_out[1]=(((int)buffer[2]) << 8 ) | buffer[3];
  Gyro_out[2]=(((int)buffer[4]) << 8 ) | buffer[5];
}

void getAccelerometerReadings(int Accel_out[])
{
  byte buffer[6];
  readFrom(0x53,0x32,6,buffer); //datasheet ADXL345
  
  Accel_out[0]=(((int)buffer[1]) << 8 ) | buffer[0];
  Accel_out[1]=(((int)buffer[3]) << 8 ) | buffer[2];
  Accel_out[2]=(((int)buffer[5]) << 8 ) | buffer[4];
}

void setup()
{
  int Gyro_cal_x_sample = 0;
  int Gyro_cal_y_sample = 0;
  int Gyro_cal_z_sample = 0;
  
  int Accel_cal_x_sample = 0;
  int Accel_cal_y_sample = 0;
  int Accel_cal_z_sample = 0;
  
  int i;
  delay(5);
  Wire.begin();
  
  Serial.begin(115200);
  
  writeTo(0x53,0x31,0x09); //accel 11 bits - +/-4g
  writeTo(0x53,0x2D,0x08); //accel en mode mesure
  writeTo(0x68,0x16,0x1A); //gyro +/-2000 deg/s + passe-bas a 100Hz
  writeTo(0x68,0x15,0x09); //gyro echantillonage a 100Hz
  
  delay(100);

  for(i = 0;i < 100;i += 1)
  {
    getGyroscopeReadings(Gyro_output);
    getAccelerometerReadings(Accel_output);
    
    Gyro_cal_x_sample += Gyro_output[0];
    Gyro_cal_y_sample += Gyro_output[1];
    Gyro_cal_z_sample += Gyro_output[2];
    
    Accel_cal_x_sample += Accel_output[0];
    Accel_cal_y_sample += Accel_output[1];
    Accel_cal_z_sample += Accel_output[2];
    
    delay(50);
  }
  
  Gyro_cal_x = Gyro_cal_x_sample / 100;
  Gyro_cal_y = Gyro_cal_y_sample / 100;
  Gyro_cal_z = Gyro_cal_z_sample / 100;
  
  Accel_cal_x = Accel_cal_x_sample / 100;
  Accel_cal_y = Accel_cal_y_sample / 100;
  Accel_cal_z = (Accel_cal_z_sample / 100) - 256; //sortie a 256 LSB/g (gravite terrestre) => offset a 256 pour mise a 0
}

void loop()
{
  timer = millis();
  
  getGyroscopeReadings(Gyro_output);
  getAccelerometerReadings(Accel_output);
  
  Accel_pitch = atan2((Accel_output[1] - Accel_cal_y) / 256,(Accel_output[2] - Accel_cal_z)/256) * 180 / PI;

  Gyro_pitch = Gyro_pitch + ((Gyro_output[0] - Gyro_cal_x)/ 14.375) * dt; 

  if(Gyro_pitch < 180) Gyro_pitch += 360; 
  if(Gyro_pitch >= 180) Gyro_pitch -= 360;

  Predicted_pitch = Predicted_pitch + ((Gyro_output[0] - Gyro_cal_x)/14.375) * dt; 

  Accel_roll = atan2((Accel_output[0] - Accel_cal_x) / 256,(Accel_output[2] - Accel_cal_z)/256) * 180 / PI;
  Gyro_roll = Gyro_roll + ((Gyro_output[1] - Gyro_cal_y)/ 14.375) * dt;
   
  if(Gyro_roll < 180) Gyro_roll += 360; 
  if(Gyro_roll >= 180) Gyro_roll -= 360; 

  Predicted_roll = Predicted_roll - ((Gyro_output[1] - Gyro_cal_y)/14.375) * dt; 
  
  P00 += dt * (2 * P01 + dt * P11);
  P01 += dt * P11;
  P00 += dt * Q;
  P11 += dt * Q;
  
  Kk0 = P00 / (P00 + R);
  Kk1 = P01 / (P01 + R);

  Predicted_pitch += (Accel_pitch - Predicted_pitch) * Kk0;
  Predicted_roll += (Accel_roll - Predicted_roll) * Kk0;

  P00 *= (1 - Kk0);
  P01 *= (1 - Kk1);
  P11 -= Kk1 * P01;

  float angle_z = Gyro_output[2];

  time = millis();
  
  Serial.print("Predicted_X: "); 
  Serial.print(Predicted_pitch);
  Serial.print(" ");
  Serial.print("Predicted_Y: "); 
  Serial.print(Predicted_roll);
  Serial.println(" ");
  
  timer = millis() - timer;
  timer = (dt * 1000) - timer;
  
  delay(timer);
}

J'attends votre retour...un petit feedback me permettra de m'améliorer. Merci.

Amitiés,