IMU X,Y + Filtre de Kalman fait maison (version corrigée)

International Français Réalisations et Projets Finis
1

Bonjour à tous,

Suite à mon dernier post, je vous livre (enfin) la version définitive et corrigée de mon filtre de Kalman.
Ce filtre répond à ma demande en terme de vitesse d’exécution et de stabilité sur un Arduino UNO.

La réponse du filtre est la fusion d'un accéléromètre ADXL345 et d'un gyroscope ITG3200 sur les axes X(pitch) et Y(roll).
La communication entre l'IMU et l'Arduino se fait par I2C.

NOTA:
Pour les fans de robotique (Plateforme stabilisée par exemple), le code contient l'intégration de deux servos sur les axes X et Y.
Il faudra veiller à les décommenter (Enlever le // devant les lignes) pour les activer.

// Fusion de capteurs accelerometre et gyroscope
// uniquement fusion sur les axes X et Y par Kalman
// Accelerometre: ADXL345
// Gyroscope: ITG3200
// Communication I2C sur Arduino UNO

#include <Wire.h> 
//#include <Servo.h> DECOMMENTER POUR ACTIVER LES SERVOS

//Servo servo_x; DECOMMENTER POUR ACTIVER LES SERVOS
//Servo servo_y; DECOMMENTER POUR ACTIVER LES SERVOS

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;

//valeur initiales axe X (pitch)
//------------------------------
float Gyro_pitch = 0;
float Accel_pitch = 0;
float Predicted_pitch = 0;

//valeur initiales axe Y (roll)
//-----------------------------
float Gyro_roll = 0;
float Accel_roll = 0;
float Predicted_roll = 0;

//definition des bruits
//---------------------
float Q = 0.1; //bruit de processus de covariance
float R = 5; //bruit de mesure

//erreur de covariance
//--------------------
float P00 = 0.1;
float P11 = 0.1;
float P01 = 0.1;

//gains de Kalman
//---------------
float Kk0, Kk1; 

unsigned long timer;
unsigned long time;

//fonction ecriture I2C
//---------------------
void writeTo(byte device, byte toAddress, byte val) 
{
  Wire.beginTransmission(device);
  Wire.write(toAddress);
  Wire.write(val);
  Wire.endTransmission();
}

//fonction lecture I2C
//--------------------
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++;
  }
}

//lecture gyroscope - datasheet ITG3200
//-------------------------------------
void getGyroscopeReadings(int Gyro_out[])
{
  byte buffer[6];
  readFrom(0x68,0x1D,6,buffer); 
  
  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];
}

//lecture accelerometre - datasheet ADXL345
//-----------------------------------------
void getAccelerometerReadings(int Accel_out[])
{
  byte buffer[6];
  readFrom(0x53,0x32,6,buffer); 
  
  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()
{
  //servo_x.attach(10); DECOMMENTER POUR ACTIVER LES SERVOS
  //servo_y.attach(11); DECOMMENTER POUR ACTIVER LES SERVOS
  
  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);
  
  //configuration gyroscope et accelerometre
  //----------------------------------------
  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; 
  
  //conserver l'echelle +/-180° pour l'axe X du gyroscope
  //-----------------------------------------------------
  if(Gyro_pitch < 180) Gyro_pitch += 360; 
  if(Gyro_pitch >= 180) Gyro_pitch -= 360;
  
  //sortie du filtre de Kalman pour les X (pitch)
  //---------------------------------------------
  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;
   
  //conserver l'echelle +/-180° pour l'axe Y du gyroscope
  //-----------------------------------------------------
  if(Gyro_roll < 180) Gyro_roll += 360; 
  if(Gyro_roll >= 180) Gyro_roll -= 360; 

  //sortie du filtre de Kalman pour les Y (roll)
  //--------------------------------------------
  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;
  
  //mise a jour des gains de Kalman
  //-------------------------------
  Kk0 = P00 / (P00 + R);
  Kk1 = P01 / (P01 + R);

  //mise a jour de la mesure
  //------------------------
  Predicted_pitch += (Accel_pitch - Predicted_pitch) * Kk0;
  Predicted_roll += (Accel_roll - Predicted_roll) * Kk0;

  //mise a jour de l'erreur de covariance
  //-------------------------------------
  P00 *= (1 - Kk0);
  P01 *= (1 - Kk1);
  P11 -= Kk1 * P01;

  float angle_z = Gyro_output[2];

  time = millis();
  
  //affichage des valeurs X et Y
  //----------------------------
  Serial.print("Predicted_X: "); 
  Serial.print(Predicted_pitch);
  Serial.print(" , ");
  Serial.print("Predicted_Y: "); 
  Serial.print(Predicted_roll);
  Serial.println(" ");
  
  //servo_x.write(Predicted_pitch); DECOMMENTER POUR ACTIVER LES SERVOS
  //servo_y.write(Predicted_roll); DECOMMENTER POUR ACTIVER LES SERVOS
  
  timer = millis() - timer;
  timer = (dt * 1000) - timer;
  
  delay(timer);
}

Amusez-vous bien.

Amitiés,

Chris

2

Bonjour,
Je suis très intéressé par votre travail. Je ne connais pas grand chose aux filtres de Kalman (c'est un de mes grands regrets), mais je sais qu'il y a habituellement une tonne de produit matriciel et de normalisation avec des 1/racine qui traînent, choses que je ne vois pas ici. Votre filtre est-il Kalman Kalman, ou kalman bricolé, genre Alpha-Beta amélioré ?

3

Bonjour, je recherche a faire a support de caméra gyroscopique avec un GY 521, votre code est t'il compatible, petite précision je suis tous neuf dans le milieu de la programation.
Cordialement
Mathieu