├── README.md └── kalman_maison_servos.ino /README.md: -------------------------------------------------------------------------------- 1 | # Kalman-on-Arduino-without-external-libraries 2 | I integrated a simple Kalman filter on Arduino to pilot two servo (x,y), without the need to have a specific library. 3 | Just wire the IMU based on the 10 DoF of DFRobot, and run it. 4 | -------------------------------------------------------------------------------- /kalman_maison_servos.ino: -------------------------------------------------------------------------------- 1 | //--IMU 6DoF I2C 2 | //--Accelerometre = ADXL345 3 | //--Gyroscope = ITG3200 4 | //--Fusion de l'accel et du gyro avec Kalman 5 | //--Integration des servos en X et Y 6 | //--valeur servo (Datasheet Hitech HS-485B): 7 | // -90° = 900us 8 | // 0° = 1500us 9 | // +90° = 2100us 10 | 11 | #include // communication I2C 12 | #include // commande servos 13 | 14 | // declaration des servos (X,Y) 15 | //----------------------------- 16 | 17 | Servo servo_X; 18 | Servo servo_Y; 19 | Servo servo_Z; 20 | 21 | // declaration des positions des servos (X,Y) 22 | //------------------------------------------- 23 | 24 | int pos_X = 0; 25 | int pos_Y = 0; 26 | int pos_Z = 90; 27 | 28 | // vecteur 3 axes par capteurs 29 | //---------------------------- 30 | 31 | int Gyro_out[3],Accel_out[3]; 32 | 33 | // base de temps pour l'acquisition des signaux 34 | //--------------------------------------------- 35 | 36 | float dt = 0.02; 37 | 38 | // declaration des variables 39 | //-------------------------- 40 | 41 | float Gyro_cal_x,Gyro_cal_y,Gyro_cal_z,Accel_cal_x,Accel_cal_y,Accel_cal_z; 42 | 43 | // valeur d'initialisation axe X 44 | //------------------------------ 45 | 46 | float Gyro_X = 0; 47 | float Accel_X = 0; 48 | float Predicted_X = 0; // sortie du filtre de Kalman X 49 | 50 | // valeur d'initialisation axe Y 51 | //------------------------------ 52 | 53 | float Gyro_Y = 0; 54 | float Accel_Y = 0; 55 | float Predicted_Y = 0; // sortie du filtre de Kalman Y 56 | 57 | // prediction des etats initiaux (a modifier si besoin) 58 | //----------------------------------------------------- 59 | 60 | float Q = 2.5; // bruit de covariance 61 | float R = 5; // bruit de mesure gaussien 62 | 63 | // matrice de covariance des erreurs (erreurs estimees non nulles) 64 | //---------------------------------------------------------------- 65 | 66 | float P00 = 0.1; 67 | float P11 = 0.1; 68 | float P01 = 0.1; 69 | 70 | // declaration des gains de Kalman 71 | //-------------------------------- 72 | 73 | float Kk0, Kk1; 74 | 75 | // declaration des unites de temps 76 | //-------------------------------- 77 | 78 | unsigned long timer; 79 | unsigned long time; 80 | 81 | // fonction ecriture I2C librairie "wire.h" 82 | //----------------------------------------- 83 | 84 | void writeTo(byte device, byte toAddress, byte val) 85 | { 86 | Wire.beginTransmission(device); 87 | Wire.write(toAddress); 88 | Wire.write(val); 89 | Wire.endTransmission(); 90 | } 91 | 92 | // fonction lecture I2C librairie "wire.h" 93 | //---------------------------------------- 94 | 95 | void readFrom(byte device, byte fromAddress, int num, byte result[]) 96 | { 97 | Wire.beginTransmission(device); 98 | Wire.write(fromAddress); 99 | Wire.endTransmission(); 100 | Wire.requestFrom((int)device, num); 101 | 102 | // condition de test pour la lecture I2C 103 | //-------------------------------------- 104 | 105 | int i = 0; 106 | 107 | while(Wire.available()) 108 | { 109 | result[i] = Wire.read(); 110 | i++; 111 | } 112 | } 113 | 114 | //datasheet lecture gyroscope I2C ITG3200 115 | //--------------------------------------- 116 | 117 | void getGyroscopeReadings(int Gyro_out[]) 118 | { 119 | byte buffer[6]; 120 | readFrom(0x68,0x1D,6,buffer); 121 | 122 | Gyro_out[0]=(((int)buffer[0]) << 8 ) | buffer[1]; 123 | Gyro_out[1]=(((int)buffer[2]) << 8 ) | buffer[3]; 124 | Gyro_out[2]=(((int)buffer[4]) << 8 ) | buffer[5]; 125 | } 126 | 127 | //datasheet lecture accelerometre I2C ADXL345 128 | //------------------------------------------- 129 | 130 | void getAccelerometerReadings(int Accel_out[]) 131 | { 132 | byte buffer[6]; 133 | readFrom(0x53,0x32,6,buffer); 134 | 135 | Accel_out[0]=(((int)buffer[1]) << 8 ) | buffer[0]; 136 | Accel_out[1]=(((int)buffer[3]) << 8 ) | buffer[2]; 137 | Accel_out[2]=(((int)buffer[5]) << 8 ) | buffer[4]; 138 | } 139 | 140 | // routine de calibration 141 | //----------------------- 142 | 143 | void setup() 144 | { 145 | 146 | servo_X.attach(12); // plug le servo_X sur pin 10 (a changer si besoin) 147 | servo_Y.attach(11); // plug le servo_Y sur pin 11 (a changer si besoin) 148 | servo_Z.attach(10); 149 | 150 | int Gyro_cal_x_sample = 0; 151 | int Gyro_cal_y_sample = 0; 152 | int Gyro_cal_z_sample = 0; 153 | 154 | int Accel_cal_x_sample = 0; 155 | int Accel_cal_y_sample = 0; 156 | int Accel_cal_z_sample = 0; 157 | 158 | int i; 159 | 160 | delay(5); 161 | 162 | // configuration du port serie 163 | //---------------------------- 164 | 165 | Wire.begin(); 166 | Serial.begin(115200); // vitesse a 115200 bds 167 | 168 | // calibration des capteurs selon les datasheets constructeurs 169 | //------------------------------------------------------------ 170 | 171 | writeTo(0x53,0x31,0x09); //accel 11 bits - +/-4g 172 | writeTo(0x53,0x2D,0x08); //accel en mode mesure 173 | writeTo(0x68,0x16,0x1A); //gyro +/-2000 deg/s + passe-bas a 100Hz 174 | writeTo(0x68,0x15,0x09); //gyro echantillonage a 100Hz 175 | 176 | delay(100); 177 | 178 | for(i = 0;i < 100;i += 1) 179 | { 180 | 181 | // lecture des sorties capteurs 182 | //----------------------------- 183 | 184 | getGyroscopeReadings(Gyro_out); 185 | getAccelerometerReadings(Accel_out); 186 | 187 | // acquisition du gyro sur les 3 axes 188 | //----------------------------------- 189 | 190 | Gyro_cal_x_sample += Gyro_out[0]; 191 | Gyro_cal_y_sample += Gyro_out[1]; 192 | Gyro_cal_z_sample += Gyro_out[2]; 193 | 194 | // acquisition de l'accel sur les 3 axes 195 | //-------------------------------------- 196 | 197 | Accel_cal_x_sample += Accel_out[0]; 198 | Accel_cal_y_sample += Accel_out[1]; 199 | Accel_cal_z_sample += Accel_out[2]; 200 | 201 | delay(50); 202 | } 203 | 204 | // lecture du gyro sur les 3 axes 205 | //------------------------------- 206 | 207 | Gyro_cal_x = Gyro_cal_x_sample / 100; 208 | Gyro_cal_y = Gyro_cal_y_sample / 100; 209 | Gyro_cal_z = Gyro_cal_z_sample / 100; 210 | 211 | // lecture de l'accel sur les 3 axes 212 | //---------------------------------- 213 | 214 | Accel_cal_x = Accel_cal_x_sample / 100; 215 | Accel_cal_y = Accel_cal_y_sample / 100; 216 | Accel_cal_z = (Accel_cal_z_sample / 100) - 256; //code sur 8 bits (256LSB) pour annuler la gravite => 0 = -256 217 | } 218 | 219 | // programme principal 220 | //-------------------- 221 | 222 | void loop() 223 | { 224 | timer = millis(); 225 | 226 | // lecture discrete (loop) des sorties capteurs 227 | //--------------------------------------------- 228 | 229 | getGyroscopeReadings(Gyro_out); 230 | getAccelerometerReadings(Accel_out); 231 | 232 | // equation du mouvement selon l'axe des X 233 | //---------------------------------------- 234 | 235 | Accel_X = atan2((Accel_out[1] - Accel_cal_y) / 256,(Accel_out[2] - Accel_cal_z)/256) * 180 / PI; 236 | Gyro_X = Gyro_X + ((Gyro_out[0] - Gyro_cal_x)/ 14.375) * dt; 237 | 238 | // lecture de -90°/+90° sur X 239 | //--------------------------- 240 | 241 | if(Gyro_X < 180) Gyro_X += 360; 242 | if(Gyro_X >= 180) Gyro_X -= 360; 243 | 244 | // mise a jour de la matrice de prediction sur X (time update phase 1) 245 | //------------------------------------------------------------------------ 246 | 247 | Predicted_X = Predicted_X + ((Gyro_out[0] - Gyro_cal_x)/14.375) * dt; 248 | 249 | // equation du mouvement selon l'axe des Y 250 | //---------------------------------------- 251 | 252 | Accel_Y = atan2((Accel_out[0] - Accel_cal_x) / 256,(Accel_out[2] - Accel_cal_z)/256) * 180 / PI; 253 | Gyro_Y = Gyro_Y + ((Gyro_out[1] - Gyro_cal_y)/ 14.375) * dt; 254 | 255 | // lecture de -90°/+90° sur Y 256 | //--------------------------- 257 | 258 | if(Gyro_Y < 180) Gyro_Y += 360; 259 | if(Gyro_Y >= 180) Gyro_Y -= 360; 260 | 261 | // mise a jour de la matrice de prediction sur Y (time update phase 1) 262 | //------------------------------------------------------------------------ 263 | 264 | Predicted_Y = Predicted_Y - ((Gyro_out[1] - Gyro_cal_y)/14.375) * dt; 265 | 266 | // erreur de covariance donne par les resultats de derivation (time update phase 2) 267 | //--------------------------------------------------------------------------------- 268 | 269 | P00 += dt * (2 * P01 + dt * P11); 270 | P01 += dt * P11; 271 | P00 += dt * Q; 272 | P11 += dt * Q; 273 | 274 | // mise a jour des gains de Kalman (measure update phase 1) 275 | //--------------------------------------------------------- 276 | 277 | Kk0 = P00 / (P00 + R); 278 | Kk1 = P01 / (P01 + R); 279 | 280 | // mise a jour du filtre de Kalman (measure update phase 2) 281 | //--------------------------------------------------------- 282 | 283 | Predicted_X += (Accel_X - Predicted_X) * Kk0; 284 | Predicted_Y += (Accel_Y - Predicted_Y) * Kk0; 285 | 286 | // mise a jour de la matrice de covariance des erreurs (measure update phase 3) 287 | //----------------------------------------------------------------------------- 288 | 289 | P00 *= (1 - Kk0); 290 | P01 *= (1 - Kk1); 291 | P11 -= Kk1 * P01; 292 | 293 | float angle_z = Gyro_out[2]; 294 | 295 | time = millis(); 296 | 297 | // conversion et mise a l'echelle angle/temps pour les servos 298 | //----------------------------------------------------------- 299 | 300 | pos_X = map(Predicted_X, -90, 90, 900, 2200); // servo_X 301 | servo_X.write(pos_X); 302 | 303 | pos_Y = map(Predicted_Y, 90, -90, 900, 2200); // servo_Y 304 | servo_Y.write(pos_Y); 305 | 306 | /* 307 | // affichage des resultats sur port serie (115200 bds) 308 | //---------------------------------------------------- 309 | 310 | Serial.print("Predicted_X: "); //affichage X en angle d'Euler 311 | Serial.print(Predicted_X); 312 | Serial.print(" "); 313 | Serial.print("Predicted_Y: "); //affichage Y en angle d'Euler 314 | Serial.print(Predicted_Y); 315 | Serial.println(" "); 316 | Serial.print("Pos_X: "); // affichage valeur servo X 317 | Serial.print(pos_X); 318 | Serial.print(" "); 319 | Serial.print("Pos_Y: "); //affichage valeur servo Y 320 | Serial.print(pos_Y); 321 | Serial.print(" "); 322 | */ 323 | 324 | timer = millis() - timer; //base de temps en millisecondes 325 | timer = (dt * 1000) - timer; //mise a jour de la base de temps 326 | 327 | delay(timer); 328 | } 329 | --------------------------------------------------------------------------------