Kalman Filtered Nunchuck & Wii Motion Plus

Great...can you post the new code ?
Earl

Are the times here GMT ?

this question is probably better placed in the FAQ section but you can set the offset from GMT as an option. I don't think there is a user friendly way of getting to your options settings, I use the link with the user name appended:
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?action=myprofileOptions;username=
(put your user name after the equals sign)

Hi Adr1an -

Thanks for the post.

I've looked through your code and have a few comments.

  1. In Jordi's orginal 'loop' method code, he would subtract the previous reading from the total, read the new value and add it to the total, then take the average. In your 'loop', you are only adding it - and it's being added before reading the current values, which is incorrect. Shouldn't it be more along these lines?:
void loop()
{
  if((millis()-lastread) >= mydt) { // sample every dt ms -> 1000/dt hz.
    lastread = millis();
    totalX -= readingsX[index];
    totalY -= readingsY[index];
    totalZ -= readingsZ[index];               // subtract the reading from the total
    switchtowmp();
    receiveData();
    switchtonunchuck();

    readingsX[index] = int(roll/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
    readingsY[index] = int(pitch/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
    readingsZ[index] = int(yaw/20); // read from the gyro sensor (/20 to get degree/second) pitch == roll
    totalX += readingsX[index];
    totalY += readingsY[index];
    totalZ += readingsZ[index];

    index = (index + 1);                    // advance to the next index
...
...
  1. Also in the 'loop' method, there are computations for q_m_X, q_m_Y, and q_m_Z. All 3 of those equations use joy_x_axis. Is that intended? Shouldn't q_m_Y use joy_y_axis? At a more basic level, I don't understand why Jordi had the need to use the joystick values here anyway. It seems odd. If you were to just use a non-Nunchuck 3-axis accelerometer, you would not have those values. I need to study his math more to fully understand I guess.
  2. I think there are cut-and-paste errors in the PCt_0_1, PCt_1_1, and t_1_1 constant equations (Should be P1 instead of P).
    const float            PCt_0_1 = C_0_1 * P1[0][0];  //(Kalman)
    const float            PCt_1_1 = C_0_1 * P1[1][0]; //(Kalman)
    const float            t_1_1 = C_0_1 * P1[0][1]; /* + C_1 * P[1][1]  = 0 (Kalman) */

I also think that there's a lot of unnecessary logic in the code that can be removed. For example readings, total, average, and P are dupes of readingsX, totalX, averageX, and P1. I don't think that affects the outcome, just memory size and processing time.

Even with these changes my code is acting wacky. I'll keep investigating.

Cheers -
Duck

@Duck: Yes! my original paste here was when I had accidentally deleted the second set of -'s - so the correction is correct.

As for the joy_x part, its just a nasty hack to calibrate all gyros off one axis.

I suspect the calibration needs more tweaking - i'm halfway through playing with it and have a clean y-axis and an almost clean x-axis... Z is just off-the-planet at the moment :wink:

re logic that can be culled: I have no doubt - its really a massage of about 4 sets of code, so lots of it is probably now unrequired or in just an ugly state in general. the original set of variables without the X on them are just for a quick way to jump back to jordi's original code.. they are entirely artifacts and can be ditched...

Try tweaking the 0.3 calibration part for each axis.
Also - I know my degrees calculation needs some extra work - it needs to pickup the LSB from the WM+ to detect fast or slow rotation.. the resolution does appear to be different...

@Duck
Any more progress ?

Slowly but surely. I hope to make a lot more progress this evening. Here's one small fix that I posted on another blog. (Note that I have switched roll and pitch from Adrian's (and other's) posts to make it inline with Wiimote/Extension Controllers - WiiBrew data structure definition.

void calibrateZeroes(){
  long y0 = 0;
  long p0 = 0;
  long r0 = 0;
  const int avg = 10;
  for (int i=0;i<avg; i++){
    wmpSendZero();
    Wire.requestFrom(0x52,6);
    for (int t=0;t<6;t++){
      data[t]=Wire.receive();
    }
    y0+=(((data[3] >> 2) << 8)+data[0]);
    r0+=(((data[4] >> 2) << 8)+data[1]);
    p0+=(((data[5] >> 2) << 8)+data[2]);
  }
  
  yaw0 = y0/avg;
  roll0 = r0/avg;
  pitch0 = p0/avg;
}

Some other changes that I'm making is honoring the dual speed modes of the Wii Motion Plus. There are 3 extra bits in the data that indicate if the output data is fast or slow. This presumably maps directly to the 500 degree/sec and the 2000 degree/sec modes of the Invensense IDG-600.

I also went all the way back to the original articles and code by Tom Pycke. A lot of changes have been made along the way - not all of them valid. Plus I wanted to wrap my head around the real issues with Kalman Filtering.

I'll post more soon if I can make progress.

Duck

If I can be of any help let me know. I have both a 368 and a 168 arduino and a MP+ and NC wired together and a spare NC and spare WM+.
I am starting to get the hang of the arduino programming. I am using 016...
Earl

On the 500/sec or 2000/sec on the idg 600, My impression is that it is a factory set calibration that you cant change,. Or is it not like the idg 650 ?
Earl

Earl: It signifies what mode its in by 1's or 0's in the last 3 bytes of the payload data... you then have to alter the degrees/sec calculations based on if its in 'fast' mode or 'slow' mode...

Duck: Thanks heaps... I tried to keep the history chain of code valid... Looks like your making great progress.
I Know the Pitch/Roll/Yaw is backwards - but that was because I was deliberately maintaining the naming from Knuckles code... hence the need to use 'pitch' for the 'roll' correction etc etc.

I've been stuck making a new board for the last few days... I should be able to finish my other calibration tests soon.

Keep it coming Duck !! :slight_smile:
(Kalman... its interesting isn't it :wink: )

Oh., I thought the 6 bytes of data were xyzXYZ. You combine the xX yY and zZ to get the 10 bit resolution of the a/d converter. Or for one resolution you only use 8 bits and for the higher resolution you use the all 10 bits?

Checkout Wiimote/Extension Controllers - WiiBrew if you haven't.

In 'slow' mode, 20 represents turning at about 1 degree per second. So you divide by 20 to get the degrees per second. At high speed (Low speed bit = 0) 20 represents turning at about 5 degree per second, So you have to divide by 4 to get the degrees per second.

You will also see in the Data Format table that Byte 3 and 4, bits 0 and 1 contain flags for what mode it is in.

Thats why my original code is off - It only calculates in the 'slow mode' 20/sec mode correctly and doesn't look at the mode-bits to compensate...

Ahh...got it ! and I meant 14 bits from the a/d. It all makes sense now. Hope your coming along on the code. A lot of autopilots and other things sure can use an inexpensive GOOD 6DOF. I think your in the right track incorporating the kalman filter. That is going to be the secret in making this work as a 6DOF. Good work.
Earl

Yes, that's right. Here's a code snippet of what this would look like:

bool slowYaw;
bool slowPitch;
bool slowRoll;

const int wmpSlowToDegreePerSec = 20;
const int wmpFastToDegreePerSec = 4;


void receiveRaw(){
  wmpSendZero(); //send zero before each request (same as nunchuck)
  Wire.requestFrom(0x52,6);//request the six bytes from the WM+
  for (int i=0;i<6;i++){
    data[i]=Wire.receive();
  }
  yaw=((data[3] >> 2) << 8)+data[0];
  roll=((data[4] >> 2) << 8)+data[1];
  pitch=((data[5] >> 2) << 8)+data[2];

  slowPitch = data[3] & 1;
  slowYaw = data[3] & 2;
  slowRoll = data[4] & 2;
}


void loop() {
  ....

  const int rollScale  = slowRoll  ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
  const int pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
  const int yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    
  readingsX[index] = int(roll/rollScale);  
  readingsY[index] = int(pitch/pitchScale); 
  readingsZ[index] = int(yaw/yawScale); 
 ....        
}

Duck

Has anyone actually confirmed what wiibrew says about fast and slow divisions to be true. I had originally posted on that site that the division factor for the yaw axis only was 5 on fast mode. Since then it seems to have been changed to what it now says. I am running a similar code to what duckhead just posted through simple integration program and pitch and roll axes do not seem to be agreeing with wiibrew. Only yaw works as it should with the division factor of 4. Remember that wiibrew is a wiki so take it for what its worth.

Can anyone confirm correct working of the 3 axes in fast mode?

Knuckles,
Can you post the latest code your playing with ?
Earl

Sure, here's the integration sketch i have been using. Its also configured so that it can be used with the rgb cube tester with minimal modifications for visual. But using this you can see that while turning the wm+ quickly, only one axis gives valid angular position.

//Trapezoid or Runge-kutta 4th order integration program of wm+

#include <Streaming.h>
#include <Wire.h>
//#include <WiiChuck.h>

#define steps_per_deg_slow 20
#define steps_per_deg_fast 4

byte data[6];          //six data bytes
int yaw, pitch, roll;  //three axes
int yaw0, pitch0, roll0;  //calibration zeroes
int time, last_time;
float delta_t;
int last_yaw[3], last_pitch[3], last_roll[3];
float yaw_deg, pitch_deg, roll_deg;
int yaw_deg2, pitch_deg2, roll_deg2;
int startTag=0xDEAD;
int accel_x_axis, accel_y_axis, accel_z_axis;

void wmpOn(){
  Wire.beginTransmission(0x53);    //WM+ starts out deactivated at address 0x53
  Wire.send(0xfe);                 //send 0x04 to address 0xFE to activate WM+
  Wire.send(0x04);
  Wire.endTransmission();          //WM+ jumps to address 0x52 and is now active
}

void wmpSendZero(){
  Wire.beginTransmission(0x52);    //now at address 0x52
  Wire.send(0x00);                 //send zero to signal we want info
  Wire.endTransmission();
}

void calibrateZeroes(){
  for (int i=0;i<10;i++){
    wmpSendZero();
    Wire.requestFrom(0x52,6);
    for (int i=0;i<6;i++){
      data[i]=Wire.receive();
    }
    yaw0+=(((data[3]>>2)<<8)+data[0])/10;        //average 10 readings for each zero
    pitch0+=(((data[4]>>2)<<8)+data[1])/10;
    roll0+=(((data[5]>>2)<<8)+data[2])/10;
  }
  Serial.print("Yaw0:");
  Serial.print(yaw0);
  Serial.print("  Pitch0:");
  Serial.print(pitch0);
  Serial.print("  Roll0:");
  Serial.println(roll0);
}

void receiveData(){
  wmpSendZero();                   //send zero before each request (same as nunchuck)
  Wire.requestFrom(0x52,6);        //request the six bytes from the WM+
  for (int i=0;i<6;i++){
    data[i]=Wire.receive();
  }
  if(bitRead(data[3], 1)==1) yaw=(((data[3]>>2)<<8)+data[0]-yaw0)/steps_per_deg_slow;        //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
  else yaw=(((data[3]>>2)<<8)+data[0]-yaw0)/steps_per_deg_fast;
  if(bitRead(data[3], 0)==1) pitch=(((data[4]>>2)<<8)+data[1]-pitch0)/steps_per_deg_slow;    //for info on what each byte represents
  else pitch=(((data[4]>>2)<<8)+data[1]-pitch0)/steps_per_deg_fast;
  if(bitRead(data[4], 1)==1) roll=(((data[5]>>2)<<8)+data[2]-roll0)/steps_per_deg_slow;
  else roll=(((data[5]>>2)<<8)+data[2]-roll0)/steps_per_deg_fast;
}

void setup(){
  Serial.begin(115200);
  Serial.println("WM+ tester");
  pinMode(3, OUTPUT);
  digitalWrite(3, HIGH);
  delay(1);
  Wire.begin();
  wmpOn();                        //turn WM+ on
  calibrateZeroes();              //calibrate zeroes
  delay(1000);
}

void loop(){
  receiveData();                  //receive data and calculate yaw pitch and roll
  time=millis();
  delta_t=(time-last_time);
  /*yaw_deg+=rk4Integrate(yaw, last_yaw[0], last_yaw[1], last_yaw[2], delta_t);
  pitch_deg+=rk4Integrate(pitch, last_pitch[0], last_pitch[1], last_pitch[2], delta_t);
  roll_deg+=rk4Integrate(roll, last_roll[0], last_roll[1], last_roll[2], delta_t);*/
  yaw_deg+=trapIntegrate(yaw, last_yaw[0], delta_t);
  pitch_deg+=trapIntegrate(pitch, last_pitch[0], delta_t);
  roll_deg+=trapIntegrate(roll, last_roll[0], delta_t);
  /*last_yaw[2]=last_yaw[1];
  last_pitch[2]=last_pitch[1];
  last_roll[2]=last_roll[1];
  last_yaw[1]=last_yaw[0];
  last_pitch[1]=last_pitch[0];
  last_roll[1]=last_roll[0];*/
  last_yaw[0]=yaw;
  last_pitch[0]=pitch;
  last_roll[0]=roll;
  last_time=time;
  
  yaw_deg2=yaw_deg;//+360;
  pitch_deg2=pitch_deg;//+360;
  roll_deg2=roll_deg;//+360;
  
  Serial<<yaw_deg2<<"\t"<<pitch_deg2<<"\t"<<roll_deg2<<"\n";
  /*
  Serial.write((unsigned byte*)&startTag, 2);
  Serial.write((unsigned byte*)&accel_x_axis, 2);    
  Serial.write((unsigned byte*)&accel_y_axis, 2);
  Serial.write((unsigned byte*)&accel_z_axis, 2);
  Serial.write((unsigned byte*)&pitch_deg2, 2);
  Serial.write((unsigned byte*)&yaw_deg2, 2);
  Serial.write((unsigned byte*)&roll_deg2, 2);*/
  delay(10);
}

float trapIntegrate(int y2, int y1, float deltax){
  float area=0;
  area=(y2+y1)/2*deltax/1000;
  return area;
}

float rk4Integrate(int y4, int y3, int y2, int y1, float deltax){
  float area=0;
  area=((y4+2*y3+2*y2+y1)/6)*deltax/1000;
  return area;
}

As for the kalman sketch im working on, its going really slowly. No real kalman filtering stuff as of now cuz im still trying to wrap my head around it-I like jordi's code but its worthless to me unless I understand what everything does. Ill have to be the one to tune and problem solve so I need to fully understand it. Right now im working on that aspect. Luckily there are lots of great posts like tom pycke's but unluckily, very few give the right amount of physical examples and (lack of) abstraction for me to figure it all out.

OK thanks. The thing I noticed is if you turn on or reset the arduino the wmp numbers are about 8000 on yaw pitch and roll. So far so good.
Then if you don't touch anything, the numbers keep increasing and increasing at a rate of about 4 counts per second. If I physically move the wmp, the numbers don't change that much, and then they go back to increasing at a rate of about 4 counts per second. Is that what yours does?
I have another program that seams to work properly almost. So I don't suspect the wmp itself. I tried a second wmp and the same behavior.

Hope this helps your work.
Earl

Ahh... I think I found my problem. I was just using the WMP alone and not the WMP/NC with the transistor switches. I'll hook that circuit back up and see what I get.
Earl

Yaw seams to work ok. That was the problem. I now am on with the WMP/NC combo.

That's strange. Having just the WMP should work fine also because we aren't getting any data from the NC.
Must be because there is a state digital port 3 and 4 both low. But if it isn't connected to anything it shouldn't matter.

In the mode with the WMP always connected, the data gets screwed up.
Mmmm......
Earl

Here's my latest. It's seems to be working well for me, but I have more cleanup and optimizations, as well as some more thought on correlating the linear axes from the nunchuk to the rotational axes of the WMP.

// Modified Kalman code using Roll, Pitch, and Yaw from a Wii MotionPlus and X, Y, and Z accelerometers from a Nunchuck.
// Also uses "new" style Init to provide unencrypted data from the Nunchuck to avoid the XOR on each byte.
// Requires the WM+ and Nunchuck to be connected to Arduino pins D3/D4 using the schematic from Knuckles904 and johnnyonthespot
// See http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html
// Kalman Code By Tom Pycke. http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data
// Original zipped source with very instructive comments: http://tom.pycke.be/file_download/4
//
// Some of this code came via contributions or influences by Ed Simmons, Jordi Munoz, and Adrian Carter
//
// Created by Duckhead   v0.2   some cleanup and other optimization work remains.  also need to investigate correlation of nunchuk and WMP (which axis is which)

#include <math.h>
#include <Wire.h>
/////////////////////////////////////////

struct GyroKalman
{
  /* These variables represent our state matrix x */
  float x_angle,
        x_bias;

  /* Our error covariance matrix */
  float P_00,
        P_01,
        P_10,
        P_11;      

  /* 
   * Q is a 2x2 matrix of the covariance. Because we
   * assume the gyro and accelerometer noise to be independent
   * of each other, the covariances on the / diagonal are 0.
   *
   * Covariance Q, the process noise, from the assumption
   *    x = F x + B u + w
   * with w having a normal distribution with covariance Q.
   * (covariance = E[ (X - E[X])*(X - E[X])' ]
   * We assume is linear with dt
   */
  float Q_angle, Q_gyro;

  /*
   * Covariance R, our observation noise (from the accelerometer)
   * Also assumed to be linair with dt
   */
  float R_angle;
};


struct GyroKalman rollData;
struct GyroKalman pitchData;
struct GyroKalman yawData;

// test for 3 axis
int readingsX;
int readingsY;
int readingsZ;                // the readings
// End of 3 axis stuff
int index = 0;                            // the index of the current reading
int inputPin =0;                          // Gyro Analog input
//WM+ stuff
byte data[6]; //six data bytes
int yaw = 0;
int pitch = 0;
int roll = 0; //three axes
int yaw0 = 0;
int pitch0 = 0;
int roll0 = 0; //calibration zeroes
bool slowYaw;
bool slowPitch;
bool slowRoll;

const int wmpSlowToDegreePerSec = 20;
const int wmpFastToDegreePerSec = 4;

///////////////////////////////////////

float accelAngleX=0;      //NunChuck X angle
float accelAngleY=0;      //NunChuck Y angle
float accelAngleZ=0;      //NunChuck Z angle
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.
 */
static const float      R_angle      = .3; //.3 default

/*
 * 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.01; //(Kalman)
static const float      Q_gyro      = 0.04; //(Kalman)

//These are the limits of the values I got out of the Nunchuk accelerometers (yours may vary).
const int lowX = -215;
const int highX = 221;

const int lowY = -215;
const int highY = 221;

const int lowZ = -215;
const int highZ = 255;

/*
 * Nunchuk accelerometer value to degree conversion.  Output is -90 to +90 (180 degrees of motion).  
 */
float angleInDegrees(int lo, int hi, int measured) {
  float x = (hi - lo)/180.0;
  return (float)measured/x;
}

void switchtonunchuck(){
/*  PORTE ^= 32; // Toggle D3 LOW
  PORTG |= 32; // Force D4 HIGH */ // Fast switching of pins... Arduino MEGA specific...
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(4, HIGH);

}

void switchtowmp(){
/*  PORTG ^= 32; // Toggle D4 LOW
  PORTE |= 32; // Force D3 HIGH */ // Fast switching of pins... Arduino MEGA Specific pin maps, so switched back to 'normal' code for example
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(3, HIGH);
}

void nunchuck_init () //(nunchuck)
{
  // Uses New style init - no longer encrypted so no need to XOR bytes later... might save some cycles
  Wire.beginTransmission (0x52);      // transmit to device 0x52
  Wire.send (0xF0);            // sends memory address
  Wire.send (0x55);            // sends sent a zero.
  Wire.endTransmission ();      // stop transmitting
  delay(100);
  Wire.beginTransmission (0x52);      // transmit to device 0x52
  Wire.send (0xFB);            // sends memory address
  Wire.send (0x00);            // sends sent a zero.
  Wire.endTransmission ();      // stop transmitting
}

void setup()
{
  Serial.begin(115200);
/*  DDRG |= 32; // Force D3 to Output // Arduino MEGA "fast" pin config. Reverted to 'normal' for example post.
  DDRE |= 32; // Force D4 to Output
  PORTG ^= 32; // Toggle D3 HIGH
  PORTE ^= 32; // Toggle D4 HIGH */

  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  digitalWrite(3, HIGH);
  digitalWrite(4, HIGH);

  Wire.begin ();            // join i2c bus with address 0x52 (nunchuck)
  switchtowmp();
  wmpOn();
  calibrateZeroes();
  switchtonunchuck();
  nunchuck_init (); // send the initilization handshake

  initGyroKalman(&rollData, Q_angle, Q_gyro, R_angle);
  initGyroKalman(&pitchData, Q_angle, Q_gyro, R_angle);
  initGyroKalman(&yawData, Q_angle, Q_gyro, R_angle);
  
  lastread = millis();
}

void initGyroKalman(struct GyroKalman *kalman, const float Q_angle, const float Q_gyro, const float R_angle) {
  kalman->Q_angle = Q_angle;
  kalman->Q_gyro  = Q_gyro;
  kalman->R_angle = R_angle;
  
  kalman->P_00 = 0;
  kalman->P_01 = 0;
  kalman->P_10 = 0;
  kalman->P_11 = 0;
}

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 wmpOn(){
  Wire.beginTransmission(0x53);//WM+ starts out deactivated at address 0x53
  Wire.send(0xfe); //send 0x04 to address 0xFE to activate WM+
  Wire.send(0x04);
  Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
}

void wmpOff(){
  Wire.beginTransmission(82);
  Wire.send(0xf0);//address then
  Wire.send(0x55);//command
  Wire.endTransmission();
}

void wmpSendZero(){
  Wire.beginTransmission(0x52);//now at address 0x52
  Wire.send(0x00); //send zero to signal we want info
  Wire.endTransmission();
}

void calibrateZeroes(){
  long y0 = 0;
  long p0 = 0;
  long r0 = 0;
  const int avg = 10;
  for (int i=0;i<avg; i++){
    wmpSendZero();
    Wire.requestFrom(0x52,6);
    for (int t=0;t<6;t++){
      data[t]=Wire.receive();
    }
    y0+=(((data[3] >> 2) << 8)+data[0]);
    r0+=(((data[4] >> 2) << 8)+data[1]);
    p0+=(((data[5] >> 2) << 8)+data[2]);
  }
  
  yaw0 = y0/avg;
  roll0 = r0/avg;
  pitch0 = p0/avg;
}

void receiveData(){
  receiveRaw();
  //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
  //for info on what each byte represents
  yaw -= yaw0;
  roll -= roll0;
  pitch -= -pitch0;
}

void receiveRaw(){
  wmpSendZero(); //send zero before each request (same as nunchuck)
  Wire.requestFrom(0x52,6);//request the six bytes from the WM+
  for (int i=0;i<6;i++){
    data[i]=Wire.receive();
  }
  yaw=((data[3] >> 2) << 8)+data[0];
  roll=((data[4] >> 2) << 8)+data[1];
  pitch=((data[5] >> 2) << 8)+data[2];
  
  slowPitch = data[3] & 1;
  slowYaw = data[3] & 2;
  slowRoll = data[4] & 2;
}

void loop()
{
  unsigned long now = millis();
  float dt = ((float)(now - lastread))/1000.0; //compute the time delta since last iteration, in seconds.

  if (dt >= 0.01) {  //Only process delta angles if at least 1/100 of a second has elapsed
    lastread = now;

    switchtowmp();
    receiveData();
    switchtonunchuck();
    readNunchuck();

    // Handle the fast/slow bits of the Wii MotionPlus
    const int rollScale  = slowRoll  ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    const int pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    const int yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
    
    readingsX = (roll/rollScale); // read from the gyro sensor 
    readingsY = (pitch/pitchScale); // read from the gyro sensor 
    readingsZ = (yaw/yawScale); // read from the gyro sensor 
    
    float tmpX = accelAngleX * (PI/180.0);
    float tmpY = accelAngleY * (PI/180.0);
    float tmpZ = accelAngleZ * (PI/180.0);

    predict(&rollData, readingsX, dt);
    predict(&pitchData, readingsY, dt);
    predict(&yawData, readingsZ, dt);
  
    float rollAngle = update(&rollData, tmpX);
    float pitchAngle = update(&pitchData, tmpY);
    float yawAngle = update(&yawData, tmpZ);

    Serial.print("Roll: ");
    Serial.print(rollAngle*(180.0/PI));
    Serial.print("Pitch: ");
    Serial.print(pitchAngle*(180.0/PI));
    Serial.print("Yaw: ");
    Serial.println(yawAngle*(180.0/PI));
  }
}

//Stitch Here

Duck