Kalman-Filter
//This is a demo of how to use Kalman filtering with a 300d/s gyroscope and accelerometers from wii nunchuck.. //Gyro: http://www.sparkfun.com/commerce/product_info.php?products_id=395 //I extreamly recommend use the gyro with a 150 d/s to increase resolution, and, if the idea //of use it in a Helicopter cross ur mind, try to use the 75d/s, to detect minimal movements and God bless you.. //I don't remember the name or the source, but i extract this code from another complex job, and adapt it to Arduino, //You can remplace the accelerometer for an analog one, is noisier but you will increase performance and reduce code... //By Jordi Munoz... //Si deseas la version en Espanol, no dudes en escribirme... #include <math.h> #include <Wire.h> ///////////////////////////////////////// #define NUMREADINGS 5 //Gyro noise filter int readings[NUMREADINGS]; // the readings from the analog input (gyro) int index = 0; // the index of the current reading int total = 0; // the running total int average = 0; // the average int inputPin =0; //Gyro Analog input /////////////////////////////////////// float dt = .06; //( 1024.0 * 256.0 ) / 16000000.0; (Kalman) int mydt = 20; //in ms. static float P[2][2] = { //(Kalman) { 1, 0 } , //(Kalman) { 0, 1 } ,//(Kalman) }; //(Kalman) /* * Our two states, the angle and the gyro bias. As a byproduct of computing * the angle, we also have an unbiased angular rate available. These are * read-only to the user of the module. */ float angle; //(Kalman) float q_bias; //(Kalman) float rate; //(Kalman) float q_m; //(Kalman) int joy_x_axis = 0; //NunChuck Joysticks potentiometers int joy_y_axis = 0; int ax_m=0; //NunChuck X acceleration int az_m=0; //NunChuck Z acceleration byte outbuf[6]; // array to store arduino output int cnt = 0; //Counter unsigned long lastread=0; /* * R represents the measurement covariance noise. In this case, * it is a 1x1 matrix that says that we expect 0.3 rad jitter * from the accelerometer. */ float R_angle = .3; //.3 deafault, but is adjusted externally (Kalman) /* * Q is a 2x2 matrix that represents the process covariance noise. * In this case, it indicates how much we trust the acceleromter * relative to the gyros. */ static const float Q_angle = 0.001; //(Kalman) static const float Q_gyro = 0.003; //(Kalman) void nunchuck_init () //(nunchuck) { Wire.beginTransmission (0x52); // transmit to device 0x52 (nunchuck) Wire.send (0x40); // sends memory address (nunchuck) Wire.send (0x00); // sends sent a zero. (nunchuck) Wire.endTransmission (); // stop transmitting (nunchuck) } void setup() { Wire.begin (); // join i2c bus with address 0x52 (nunchuck) nunchuck_init (); // send the initilization handshake beginSerial (38400); for (int i = 0; i < NUMREADINGS; i++) readings[i] = 0; // initialize all the readings to 0 (gyro average filter) } void send_zero () //(nunchuck) { Wire.beginTransmission (0x52); // transmit to device 0x52 (nunchuck) Wire.send (0x00); // sends one byte (nunchuck) Wire.endTransmission (); // stop transmitting (nunchuck) } void loop() { if((millis()-lastread) >= mydt) { // sample every dt ms -> 1000/dt hz. lastread = millis(); total -= readings[index]; // subtract the last gyro reading readings[index] = analogRead(inputPin); // read from the gyro sensor total += readings[index]; // add the reading to the total index = (index + 1); // advance to the next index if (index >= NUMREADINGS) // if we're at the end of the array... index = 0; // ...wrap around to the beginning average = (total / NUMREADINGS)-511; // calculate the average of gyro input q_m=((average*(joy_x_axis*0.00392156862))/(180/PI)); //GYRO convertion to Radian and external correction with nunchuk joystick /* Unbias our gyro */ const float q = q_m - q_bias; //(Kalman) const float Pdot[2 * 2] = { Q_angle - P[0][1] - P[1][0], /* 0,0 */ //(Kalman) - P[1][1], /* 0,1 */ - P[1][1], /* 1,0 */ Q_gyro /* 1,1 */ }; /* Store our unbias gyro estimate */ rate = q; //(Kalman) /* * Update our angle estimate * angle += angle_dot * dt * += (gyro - gyro_bias) * dt * += q * dt */ angle += q * dt; //(Kalman) /* Update the covariance matrix */ P[0][0] += Pdot[0] * dt; //(Kalman) P[0][1] += Pdot[1] * dt; //(Kalman) P[1][0] += Pdot[2] * dt; //(Kalman) P[1][1] += Pdot[3] * dt; //(Kalman) Wire.requestFrom (0x52, 6); // request data from nunchuck while (Wire.available ()) //(NunChuck) { outbuf[cnt] = nunchuk_decode_byte (Wire.receive ()); // receive byte as an integer //(NunChuck) cnt++;//(NunChuck) } send_zero (); // send the request for next bytes // If we recieved the 6 bytes, then print them //(NunChuck) if (cnt >= 5) //(NunChuck) { print (); //(NunChuck) } cnt = 0; //(NunChuck) R_angle= (joy_y_axis+1)*0.0098039; //external adjust jitter of accelerometer with nunchuck joystick const float angle_m = atan2( ax_m, az_m ); //(Kalman) const float angle_err = angle_m - angle; //(Kalman) const float C_0 = 1; //(Kalman) const float PCt_0 = C_0 * P[0][0]; //(Kalman) const float PCt_1 = C_0 * P[1][0]; //(Kalman) const float E =R_angle+ C_0 * PCt_0; //(Kalman) const float K_0 = PCt_0 / E; //(Kalman) const float K_1 = PCt_1 / E; //(Kalman) const float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] (Kalman) */ const float t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1] = 0 (Kalman) */ P[0][0] -= K_0 * t_0; //(Kalman) P[0][1] -= K_0 * t_1; //(Kalman) P[1][0] -= K_1 * t_0; //(Kalman) P[1][1] -= K_1 * t_1; //(Kalman) angle += K_0 * angle_err; //(Kalman) q_bias += K_1 * angle_err; //(Kalman) Serial.print(joy_y_axis); //Prints the adjust for accelerometer jitter Serial.print(" "); Serial.print(int(angle_m*57.295779513082)); //Prints the accelometer Serial.print(" "); Serial.print(int(angle*57.2957795130823)); //Prints degrees Acceleromter+Gyros+KalmanFilters Serial.print(" "); Serial.println(joy_x_axis); //Prints the Gyro adjusment } } void print ()//This is the function to decode nintendo wii nunchuck { joy_x_axis = outbuf[0]; joy_y_axis = outbuf[1]; ax_m = (outbuf[2] * 2 * 2) -511; //Axis X Acceleromter //ay_m = (outbuf[3] * 2 * 2) -511; //Axis Y Acceleromter az_m = (outbuf[4] * 2 * 2) -511; //Axis Z Acceleromter // byte outbuf[5] contains bits for z and c buttons // it also contains the least significant bits for the accelerometer data // so we have to check each bit of byte outbuf[5] if ((outbuf[5] >> 2) & 1) { ax_m += 2; } if ((outbuf[5] >> 3) & 1) { ax_m += 1; } /* if ((outbuf[5] >> 4) & 1) { accel_y_axis += 2; } if ((outbuf[5] >> 5) & 1) { accel_y_axis += 1; } */ if ((outbuf[5] >> 6) & 1) { az_m += 2; } if ((outbuf[5] >> 7) & 1) { az_m += 1; } } // Encode data to format that most wiimote drivers except // only needed if you use one of the regular wiimote drivers char nunchuk_decode_byte (char x) { x = (x ^ 0x17) + 0x17; return x; }
page revision: 0, last edited: 08 Jun 2009 19:03