IMU 6DoF + Filtre Kalman fait maison

Bonjour à tous,

Après quelques tests sur un IMU 6DoF en I2C avec les bibliothèques associées, je me suis rendu compte qu'il était possible d'améliorer le filtrage de Kalman en optimisant le code et en enlevant quelques bibliothèques "inutiles".

Résultat: Un filtre de Kalman fait maison, rapide et "presque" optimisé pour le gyroscope ITG3200 et l’accéléromètre ADXL345.

Je vous livre donc le code.
Il n'est pas très commenté, mais si vous avez des questions, n'hésitez pas j'y répondrai avec plaisir.

Comme vous le verrez, je n'ai qu'une seule bibliothèque importée <Wire.h>, le reste est directement intégré dans le code.

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

#include <Wire.h> //communication I2C

int Gyro_out[3],Accel_out[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_X = 0;
float Accel_X = 0;

float Gyro_Y = 0;
float Accel_Y = 0;

float Predicted_X = 0;
float Predicted_Y = 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.h> fonction ecriture I2C
{
  Wire.beginTransmission(device);
  Wire.write(toAddress);
  Wire.write(val);
  Wire.endTransmission();
}

void readFrom(byte device, byte fromAddress, int num, byte result[]) //<wire.h> fonction lecture I2C
{
  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_out);
    getAccelerometerReadings(Accel_out);
    
    Gyro_cal_x_sample += Gyro_out[0];
    Gyro_cal_y_sample += Gyro_out[1];
    Gyro_cal_z_sample += Gyro_out[2];
    
    Accel_cal_x_sample += Gyro_out[0];
    Accel_cal_y_sample += Gyro_out[1];
    Accel_cal_z_sample += Gyro_out[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_out);
  getAccelerometerReadings(Accel_out);
  
  Accel_X = atan2((Accel_out[1] - Accel_cal_y) / 256,(Accel_out[2] - Accel_cal_z)/256) * 180 / PI;
  Gyro_X = Gyro_X + ((Gyro_out[0] - Gyro_cal_x)/ 14.375) * dt; //sensibilite gyro ITG3200 de 14.375 LSB °/s 
  
  Predicted_X = Predicted_X + ((Gyro_out[0] - Gyro_cal_x)/14.375) * dt;   
 
  Accel_Y = atan2((Accel_out[1] - Accel_cal_x) / 256,(Accel_out[2] - Accel_cal_z)/256) * 180 / PI;
  Gyro_Y = Gyro_Y + ((Gyro_out[1] - Gyro_cal_y)/ 14.375) * dt;
   
  Predicted_Y = Predicted_Y + ((Gyro_out[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_X += (Accel_X - Predicted_X) * Kk0;
  Predicted_Y += (Accel_Y - Predicted_Y) * Kk0;

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

  float angle_z = Gyro_out[2];

  time = millis();
  Serial.print("time: ");
  Serial.print(time);
  Serial.print(" ");
  
  Serial.print("Gyro_X: ");
  Serial.print(Gyro_X);
  Serial.print(" ");
  
  Serial.print("Accel_X: ");
  Serial.print(Accel_X);
  Serial.print(" ");
  
  Serial.print("Predicted_X: "); 
  Serial.print(Predicted_X);
  Serial.print(" ");
  
  Serial.print("Gyro_Y: ");
  Serial.print(Gyro_Y);
  Serial.print(" ");
  
  Serial.print("Accel_Y: ");
  Serial.print(Accel_Y);
  Serial.print(" ");
  
  Serial.print("Predicted_Y: "); 
  Serial.print(Predicted_Y);
  Serial.println(" ");
  
  timer = millis() - timer;
  timer = (dt * 1000) - timer;
  
  delay(timer);
}

Amusez-vous bien !

Amitiés,

Christophe Herault

Merci !

Tu as fait ça pour un projet particulier ?

Bonjour 3Sigma,

Non je n'ai pas de projet en particulier.
Je voulais simplement optimiser le code pour qu'il soit rapide et efficace sans avoir à intégrer tout un tas de librairies..
Il reste encore un peu de travail à faire, notamment sur l'estimation de la matrice de covariance, mais ce n'est pas grand chose.

Maintenant, sur un drone aérien ou terrestre, ça doit le faire XD

Le fait est que j'aimerai bien refaire la plateforme stabilisée que j'avais fait avant (Elle est quelque part dans ce forum...), en l'optimisant avec ce bout de code.

Et toi, as-tu une application dédiée liée à ce code ?

Amitiés,

Une petite modification à faire pour rester cohérent avec les angles d'Euler.

Le Gyro doit être calibré selon les angles +/- 180° comme suit:

//pour le gyro en X
if(Gyro_X < 180) Gyro_X += 360; //reste dans la fenetre 0-180 deg
  if(Gyro_X >= 180) Gyro_X -= 360;

//pour le gyro en Y
if(Gyro_Y < 180) Gyro_Y += 360; //reste dans la fenetre 0-180 deg
  if(Gyro_Y >= 180) Gyro_Y -= 360;

cherault:
Et toi, as-tu une application dédiée liée à ce code ?

Potentiellement oui, j'ai une IMU dans mon gyropode Geeros (voir mon avatar à gauche). Pour l'instant j'utilise un filtre complémentaire sur un MPU6050 parce que le Kalman consommait trop de temps de calcul (et n'était pas vraiment utile).
Je n'ai pas pris le temps d'essayer de l'optimiser. Mais là, du coup, en reprenant ton code et en l'adaptant pour le MPU6050, je vais pouvoir faire un test relativement rapidement.

Oui, c'est pour cette raison que j'ai revu le filtre de Kalman.

Le fait est qu'un filtre complémentaire utilise des constantes...donc on va accumuler la dérive du gyro qu'on le veuille ou non. =(

L'avantage avec Kalman est sa totale automatisation du processus de filtrage (matrice de covariance) qui si elle est bien définie (Diagonale avec les écarts types au carré = variance) te permet de gommer une mauvaise calibration et de ne pas utiliser de constantes.

De plus, le temps d'acquisition est défini sur l'inverse de la fréquence d’échantillonnage, ce qui te permet de ne pas accumuler la dérive gyroscopique.

Donne moi un retour sur ton IMU, je suis très intéressé par ta manip et tes résultats.

NOTE: pour le moment, je ne fonctionne que sur deux axes.
Prochainement, j’intègrerai l'axe Z et peut-être aussi le magnétomètre...à voir...

Amitiés,

cherault:
Le fait est qu'un filtre complémentaire utilise des constantes...donc on va accumuler la dérive du gyro qu'on le veuille ou non. =(

Tel que j'ai conçu l'asservissement, ce n'est pas gênant, ça se traduit uniquement par un offset sur la position, et comme il est piloté en vitesse, ça vit bien comme ça.
Mais bon, c'est toujours mieux de partir de quelque chose de propre.

En repensant à ce que j'avais fait, je me souviens que j'avais mis le filtre de Kalman sur tout le système afin de récupérer le vecteur d'état complet (donc pas seulement les accélérations et rotations mais aussi les vitesses moteur, etc...). D'où le fait que ça ne tenait pas. Bêtement, je n'avais pas pensé à faire le filtre uniquement sur l'IMU, ce que tu fais dans ton code (et qui est aussi le principe du filtre complémentaire). Il est donc très probable que ça fonctionne, les test me le diront.

Ok, bon courage et donne moi ton sentiment sur le filtre.
C'est toujours sympa de partager ses connaissances et résultats.

Amitiés,

ATTENTION

Il y a des erreurs de signes dans l'algo.
Je corrige ca vite fait et je test avant de le remettre en ligne de façon définitive.

Avec toutes mes excuses pour ces erreurs...

Amitiés,

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

//--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,