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