Go Down

Topic: Kalman Filtered Nunchuck & Motion Plus-the seq (Read 12150 times) previous topic - next topic

ardvark

Sep 17, 2009, 05:44 pm Last Edit: Sep 17, 2009, 06:34 pm by ardvark Reason: 1
This is a continuation of the Kalman Filtered Nunchuck & Wii Motion Plus thread found here....

http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032

....restarted in the hope that access problems will be reduced.


ardvark

#1
Sep 17, 2009, 06:13 pm Last Edit: Sep 17, 2009, 06:30 pm by ardvark Reason: 1
Here is an example of complimentary filter performance on one axis. The top panel is the accelerometer output and the bottom panel is the comp filter output (x-axis: time). I picked the NC/MP combo up, rolled 90 deg, put in down, picked it up, rolled 90 deg the other way and back, and then started to shake it


ardvark

#2
Sep 17, 2009, 06:19 pm Last Edit: Sep 17, 2009, 06:25 pm by ardvark Reason: 1
Here is the code that includes the complimentary filter:
Code: [Select]
// .......................................................................
// Code for reading data from a Wii Motion plus device connected to a Nunchuck
// Links to other sites
// http://www.windmeadow.com/node/42   .... first time i saw arduino nunchuk interface
// http://www.kako.com/neta/2009-017/2009-017.html# .....Japanese site with lots of Wii info
// http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus    .... the one and only
// http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html
// ....original motion plus but not passthrough
// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/35   .... great Kalman filter code
// thanks to duckhead and knuckles904. I will be using that code for sure.
// http://obex.parallax.com/objects/471/   .... ideas for passthrough
// by Krulkip
// Here is the bitmapping which is changed from the standard nunchuk mapping
// In Nunchuk mode:
//          Bit
// Byte     7       6       5       4       3       2       1       0
// 0       SX<7-----------------------------------------------------0>
// 1       SY<7-----------------------------------------------------0>
// 2       AX<9-----------------------------------------------------2>
// 3       AY<9-----------------------------------------------------2>
// 4       AZ<9---------------------------------------------3>      1
// 5       AZ<2-----1>     AY<1>   AX<1>   BC      BZ       0       0
// Please note also the loss of the bit0 resolution.
// Byte 5 Bit 0 and 1 is used for nunchuk (0 0) = nunchuk or motion plus  (1 0) is motion plus detection.
// Byte 4 Bit 0 which is the extention controller detection bit. (1 = extension present)
// Hardware Arduino with ATMega 168
// Connections SCA to AD4 (Analog4) and SCL to AD5 (Analog5)
/*      
    Constants for scaling accelerometer values: Example of the accelerometer rotation values for y axis on Ardvark's nunchuk.
   
    Exact values differ between x and y axis, and probably between nunchuks, but the range value may be constant:
   
                 298 ayMIN
                 _|_
               /     \
  ayMID 513 - |       | - 513 ayMID
               \ _ _ /
                  |
                 728 ayMAX
             
    ayRange = (ayMID - ayMIN)*2 = ayMAX - ayMIN = 430
*/
//=========================================================
// changelog
// ---------
// 8/9/2009 - by dimkasta @ www.kastaniotis.net
// Adapted for use with the transistor-less switching code originally written by krulkip on
// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/60
// Calibration and utility code based on the code by duckhead
// 9/9/2009 - by dimkasta @ www.kastaniotis.net
// Added Accelerometer calibration
// Removed the casting to double and the multiplying with millis/sec from the cycle saving to further greately
// increase the free time to ~1/50 in comparison to the reading time.
// 18/9/2009 - by Ardvark
// Slow/fast bit reading fixed: Slow degree/sec scale value appears correct.
// Included details for absolute scaling of accelerometer values.
// Included complementary filter for combining single accelerometer/gyro axes to produce drift- and translation-suppressed orientation values.
//=========================================================
// TODO LIST - Known issues
// ---------
// Assign common names to gyro and accel axes
//........................................................................

#include <Wire.h>

unsigned long Now = millis();
unsigned long lastread = Now;
unsigned long lastreadMP = Now;
//The system clock function millis() returns a value in milliseconds.  We need it in seconds.
//Instead of dividing by 1000 we multiply by 0.001 for performance reasons.

boolean debug = true;

boolean calibrating = false;

uint8_t data[6];            // array to store arduino output
int cnt = 0;
int ledPin = 13;
int xID;

//wiibrew info seems to be incorrect with regards to scaling, see instead Xevel's comments: http://www.assembla.com/wiki/show/alicewiimotionplus/slow_and_fast_modes
static const int wmpSlowToDegreePerSec = 16; //when gyro speed 1, 16 units = 1°/s
static const int wmpFastToDegreePerSec = 4; //when gyro speed 0, 4 units = 0,25°/s

// Hardware identifiers. Polling data[5]&0x03 for present device
static const int NC = 0x00;
static const int MP = 0x02;
int currentDevice;

// WM+ state variables - if true, then in slow (high res) mode
boolean slowYaw = 0;
boolean slowPitch = 0;
boolean slowRoll = 0;

int rollScale;
int pitchScale;
int yawScale;

// RK integration not currently in use
struct RungeKutta {
 double val_i_3;
 double val_i_2;
 double val_i_1;
 double previous;
};

struct RungeKutta rollRK;
double roll_intRK;

// Calibrated gyro values (- yaw0 etc)
int yaw = 0;
int pitch = 0;
int roll = 0;
float rollf = 0;

float roll_int = 0;
float roll_int_rad = 0;

float dt; // msec
float dtMP; // this is the time elapsed between readings of the motion plus (MP)

float rollCF = 0;
float pitchCF = 0;

static const double DegreeToRadian = 0.0174532925;  // PI/180

// Complimentary filter: inspired by Shane Colton's description in filter.pdf at http://web.mit.edu/first/segway/segspecs.zip

// Tweak tau to alter the performance of the complimentary filter.
// Increase tau: influence of the gyro input increases (less translation artifacts).
// Reduce tau: influence of the accelerometer input increases (faster drift-correction).
// This filtering is temporal: translations that occur over a time scale shorter than tau will be suppressed,
// and gyro changes that occur over a time scale longer than tau will have their drift corrected.
// NB: changes in the read rate of the gyro (dtMP) will change the filter's performance (change tau to compensate).

static const float tau = 0.95; // unit: seconds.
float compFilter(float prevValue, int gyro, double accel, float timeStep) {
 float a = tau/(tau + dtMP);
 float b = 1 - a;
 return (float) (a*(prevValue + ((gyro*DegreeToRadian)*timeStep)) + (b*accel));
}

// accelerometer values
int ax_m = 0;
int ay_m = 0;
int az_m = 0;

// Exact values differ between x and y axis, and probably between nunchuks, but the range value may be constant:
static const int axMIN = 290;
static const int axMAX = 720;
static const int axMID = (axMAX - axMIN)/2 + axMIN;
static const int axRange = (axMID - axMIN)*2;

static const int ayMIN = 298;
static const int ayMAX = 728;
static const int ayMID = (ayMAX - ayMIN)/2 + ayMIN;
static const int ayRange = (ayMID - ayMIN)*2;

// Not sure what a meaningful scale is for the z accelerometer axis so:

static const int azMID = ayMID;
static const int azRange = ayRange;

// raw 3 axis gyro readings MP+ //gyro readings => yaw/yawscale
int readingsX;
int readingsY;
int readingsZ;      

// Nunchuck variables //NunChuck X angle NOT readings
float accelAngleX=0;      //NunChuck X angle
float accelAngleY=0;      //NunChuck Y angle
float accelAngleZ=0;      //NunChuck Z angle
float theta=0;  

int z_button = 0;
int c_button = 0;
int joy_x_axis = 0;
int joy_y_axis = 0;

// calibration zeroes
//gyros
int yaw0 = 0;
int pitch0 = 0;
int roll0 = 0;

//accelerometers
int yawAcc0 = 0;
int pitchAcc0 = 0;
int rollAcc0 = 0;

double x;
double y;
double z;


//Nunchuk accelerometer value to radian conversion.
float angleInRadians(int range, int measured) {
 float x = range;
 return (float)(measured/x) * PI;
}

void
setup ()
{
 Serial.begin (115200);
 Wire.begin();

 initializeSensors();
 calibrateZeroes();
 
 rollRK.val_i_1 = 0;
 rollRK.val_i_2 = 0;
 rollRK.val_i_3 = 0;
 rollRK.previous = 0;

}

void send_zero ()
{
   Wire.beginTransmission(0x52);
   Wire.send(0x00);
   Wire.endTransmission();
}

void loop ()
{
 Now = millis();
 dt = Now - lastread; //compute the time delta since last iteration, in msec.
 if (dt >= 10) //Only process delta angles if at least 1/100 of a second has elapsed. NB: 9 ms is the lowest possible interval before things jam up on an ATmega 328
 {
    readData();
    lastread = Now;
 }
 else // just for demo of the free cycles
 {
 //delay(10);
 //Serial.println("Free cycle");
 }
}

void readData()
{  
 long startReading = millis();
 //digitalWrite(13,HIGH);  // first flash the LED to show activity
 
   send_zero (); // send the request for next bytes
   //delay (10);
   
   // now follows conversion command instructing extension controller to get
   // all sensor data and put them into 6 byte register within the extension controller
   Wire.requestFrom (0x52,6);
   data[0] = Wire.receive();
   data[1] = Wire.receive();
   data[2] = Wire.receive();
   data[3] = Wire.receive();
   data[4] = Wire.receive();
   data[5] = Wire.receive();


   parseData();
 if(false)
 {
   long endReading = millis() - startReading;
   Serial.print("Finished Reading in ");
   Serial.print(endReading);
   Serial.println(" milliseconds");
 }
 // digitalWrite(13,LOW);  // first flash the LED to show activity
 
}

void parseData ()
{
 // check if nunchuk is really connected
 if ((data[4]&0x01)==0x01) {
 //printLine("Ext con: ");
 
 currentDevice = data[5]&0x03;
 }
   if (currentDevice == NC)
   {

 // Dimitris version of accl readings
     ax_m = (data[2] << 2) + ((data[5] >> 3) & 2) ;
     ay_m = (data[3] << 2) + ((data[5] >> 4) & 2);
     az_m = (data[4] << 2) + ((data[5] >> 5) & 6) ;

       //Zero the values on a#MID.
       ax_m -= axMID;
       ay_m -= ayMID;
       az_m -= azMID;
     

       //  stitch

ardvark

Code: [Select]
     
// stitch

// Convert to radians by mapping 180 deg of rotation to PI
       x = angleInRadians(axRange, ax_m);
       y = angleInRadians(ayRange, ay_m);
       z = angleInRadians(azRange, az_m);
       
       //compute values that are used in multiple places
//        double xSquared = x*x;
//        double ySquared = y*y;
//        double zSquared = z*z;
//      
//        accelAngleX = atan(x / sqrt(ySquared + zSquared));
//        accelAngleY = atan(y / sqrt(xSquared + zSquared));
//        theta       = atan(sqrt(xSquared + ySquared) / z);

  }
 else
 if  (currentDevice == MP)
 {
   dtMP = (Now - lastreadMP)/1000.0; //compute the time delta since last MP read, in sec.
   lastreadMP = Now;
   
     // duck's hardware setup for this code has the MP spun 90 deg relative to the NC, so I swapped the roll and pitch to match a setup with the NC and MP on the same axis
     // Gyroscopes: dual-axis IDG-600, see http://www.invensense.com/products/idg_600.html and EPSON TOYOCOM labelled X3500W (yaw)
   roll = (((data[4]&0xFC)<<6) + data[1]);
   pitch = (((data[5]&0xFC)<<6) + data[0]);
   yaw = (((data[3]&0xFC)<<6) + data[2]);
   
   if(calibrating == false) //Dont compensate if we are doing the calibration measurements.
   {
     
     slowPitch = (data[4]&0x02)>>1;
     slowRoll = (data[3]&0x01)>>0;
     slowYaw = (data[3]&0x02)>>1;
     
     rollScale  = slowRoll  ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec; // if speed == 1, then wmpSlowToDegreePerSec, else wmpFastToDegreePerSec
     pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
     yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
     
     //Calibrating
     roll -= roll0;
     pitch -= pitch0;
     yaw -= yaw0;
     
     roll = roll/rollScale;    // deg/s = mV/(mV/deg/s)
     pitch = pitch/pitchScale;
     yaw = yaw/yawScale;
     
     // Complimentary filter: inspired by Shane Colton's description in filter.pdf at http://web.mit.edu/first/segway/segspecs.zip
       rollCF = compFilter(rollCF, roll, x, dtMP);
       pitchCF = compFilter(pitchCF, pitch, y, dtMP);

   }

 }
 
 
 if(debug)
 {
   //Uncomment the set that you want to monitor.
   //Having everything spammed is not very helpful
   
  // Serial.print ("joyx= ");
  // Serial.println (joy_x_axis);
 
  // Serial.print ("joyy= ");
  // Serial.println (joy_y_axis);
//  
//     Serial.print (" ");
//     Serial.print(slowRoll==1);
//  
//    Serial.print (" ");
//    Serial.print(slowPitch==1);
//    
//    Serial.print (" ");
//    Serial.print(slowYaw==1);
//    
//     Serial.print (" ");
//     Serial.print (rollScale);
//    
   Serial.print (" ");
   Serial.print (x);
     
//    Serial.print (" ");
//    Serial.print (y);

//    Serial.print (" ");
//    Serial.print (z);
   
   Serial.print (" ");
   Serial.println (rollCF);
   
//    Serial.print (" ");
//    Serial.println (pitchCF);
//    
//    Serial.print (" ");
//    Serial.println (a);
//    
//    Serial.print (" ");
//    Serial.print (accelAngleX);
//      
//    Serial.print (" ");
//    Serial.print (accelAngleY);
//      
//    Serial.print (" ");
//    Serial.println (theta);
//
//    Serial.print (" ");
//    Serial.print (ax_m);
//      
//    Serial.print (" ");
//    Serial.print (ay_m);
////      
//    Serial.print (" ");
//    Serial.print (az_m);
     
//    Serial.print (" ");
//    Serial.print (ax_m_rad);
//      
//    Serial.print (" ");
//    Serial.print (ay_m_rad);
//      
//    Serial.print (" ");
//    Serial.println (az_m_rad);
   
  // if (z_button == 0) { Serial.println ("z_button "); }
  // if (c_button == 0) { Serial.println("c_button "); }
//  
//     Serial.print (" ");
//     Serial.print (roll);
//  
//     Serial.print (" ");
//     Serial.print (pitch);
//  
//     Serial.print (" ");
//     Serial.println (yaw);
//    
//     Serial.print (" ");
//     Serial.print(slowPitch);
//     Serial.print (" ");
//     Serial.println(slowYaw);
//     Serial.print (" ");
//     Serial.println(dt);
 }
 // Finished with data assignment, now we need calibration

}

void initializeSensors()
{
 
 digitalWrite(13,HIGH);  // first flash the LED to show activity
 Serial.println ("Now detecting WM+");
 //delay(100);
 // now make Wii Motion plus the active extension
 // nunchuk mode = 05, wm+ only = 04, classic controller = 07
 Serial.println ("Activating WM+ in NC mode (5)...");
  Wire.beginTransmission(0x53);
  Wire.send(0xFE);
  Wire.send(0x05);// nunchuk
  Wire.endTransmission();
 Serial.println (" OK done");
  //delay (100);
 // now innitiate Wii Motion plus
  Serial.println ("Innitialising Wii Motion plus ........");
   Wire.beginTransmission(0x53);
   Wire.send(0xF0);
   Wire.send(0x55);
   Wire.endTransmission();
 Serial.println(" OK done");
   //delay (100);
 Serial.println ("Reading address at 0xFA .......");
   Wire.beginTransmission(0x52);
   Wire.send(0xFA);
   Wire.endTransmission();
 Serial.println(" OK done");
  //delay (100);
  Wire.requestFrom (0x52,6);
  data[0] = Wire.receive();//Serial.print(outbuf[0],HEX);Serial.print(" ");
  data[1] = Wire.receive();//Serial.print(outbuf[1],HEX);Serial.print(" ");
  data[2] = Wire.receive();//Serial.print(outbuf[2],HEX);Serial.print(" ");
  data[3] = Wire.receive();//Serial.print(outbuf[3],HEX);Serial.print(" ");
  data[4] = Wire.receive();//Serial.print(outbuf[4],HEX);Serial.print(" ");
  data[5] = Wire.receive();//Serial.print(outbuf[5],HEX);Serial.print(" ");
 
  xID= data[0] + data[1] + data[2] + data[3] + data[4] + data[5];
 Serial.println("Extension controller xID = 0x");
  Serial.print(xID,HEX);
  if (xID == 0xCB) { Serial.println ("MP+ on but not active"); }
  if (xID == 0xCE) { Serial.println ("MP+ on and active"); }
  if (xID == 0x00) { Serial.println ("MP+ not on"); }
  //delay (100);
  // Now we want to point the read adress to 0xa40008 where the 6 byte data is stored
 Serial.println ("Reading address at 0x08 .........");
   Wire.beginTransmission(0x52);
   Wire.send(0x08);
   Wire.endTransmission();
 Serial.println(" OK done");
 Serial.println ("Finished setup");
 
 digitalWrite(13,LOW); // Led off
}

/*
* Find the steady-state of the WM+ gyros.  This function reads the gyro values 100 times and averages them.
* Those values are used as the baseline steady-state for all subsequent reads.  As a result it is very
* important that the device remain relatively still during startup.
*/


void calibrateZeroes(){  // wiibrew: While the Wiimote is still, the values will be about 0x1F7F (8,063)
 calibrating = true;
 if(debug)
 {
   Serial.println("Starting calibration...");
 }
 //Gyros
 long y0 = 0;
 long p0 = 0;
 long r0 = 0;
 
 //Accelerometers for application calibration, not for zeroing the starting points.
 long ya0 = 0;
 long pa0 = 0;
 long ra0 = 0;
 
 const int avg = 200; //100 reads of the gyros and 100 of the accelerometers
 
 digitalWrite(13,HIGH);  // first flash the LED to show activity
 
 for (int i=0;i<avg; i++)
 {
   delay(10); // bypasses the normal global delays
   readData();
   
   if(currentDevice == MP)
   {
     if(debug)
     {
       Serial.println("<= Data from MP");
     }
   
     y0+=yaw;
     r0+=roll;
     p0+=pitch;
   }
   else
   {
      if(debug)
     {
       Serial.println("<= Data from NC...");
     }
     ya0 += az_m;
     pa0 += ay_m;
     ra0 += ax_m;
     
   }
 }
 
 yaw0 = y0/(avg/2);
 roll0 = r0/(avg/2);
 pitch0 = p0/(avg/2);
 
 yawAcc0 = ya0/(avg/2);
 rollAcc0 = ra0/(avg/2);
 pitchAcc0 = pa0/(avg/2);
 
 calibrating = false;
 
 digitalWrite(13,LOW); // Led off
 if(debug)
 {
   Serial.println("Calibration results...");
   Serial.print("Yaw0:");
   Serial.println(yaw0);
   Serial.print("Roll0:");
   Serial.println(roll0);
   Serial.print("Pitch0:");
   Serial.println(pitch0);
   Serial.print("YawAcc0:");
   Serial.println(yawAcc0);
   Serial.print("RollAcc0:");
   Serial.println(rollAcc0);
   Serial.print("PitchAcc0:");
   Serial.println(pitchAcc0);
 }
}

/*
* Compute the common fourth-order Runge-Kutta algorithm as part of the integrating the gyro's yaw signal.  This
* will smooth the values a tad.  Gyro drift is still a problem, however.
*
* rk       the RungaKutta struct to hold previous values
* val_i_0  the latest raw value to integrate
*
* returns the RK4 approximation of all values up until time t
*/
double computeRK4(struct RungeKutta *rk, double val_i_0) {
 rk->previous += 0.16667*(rk->val_i_3 + 2*rk->val_i_2 + 2*rk->val_i_1 + val_i_0);
 rk->val_i_3 = rk->val_i_2;
 rk->val_i_2 = rk->val_i_1;
 rk->val_i_1 = val_i_0;
 return rk->previous;
}

Dimitris

I just love its simplicity...
It might be perfect for me, especially since I am running out of program space.

Could you please do some repeatability testing?
Like pointing the device towards predefined objects or putting it in predefined positions and seeing if you get the same readings?

ardvark

@ Dimitris

Quote

Could you please do some repeatability testing?
Like pointing the device towards predefined objects or putting it in predefined positions and seeing if you get the same readings?


This is shown graphically in the figure, where the NC/MP is first lifted from a lying position and then returned to a lying position.

More quantitatively, I can tell you that the 'repeatability' limits of orientation measurement comes down to accelerometer performance. If the accel gives you the same value when returned to a specific position the comp filter will do the same, but will take longer to reach that value (see in the figure how the CF output 'creeps' towards the accel output when the NC/MP is stable). How long this takes depends on the choice of tau.

Dimitris

Ah nice. so we prioritize accelerometers with taf.

Damn Fedex ruined my progress flow... I have to wait for another 5 days now for the magnetometer and GPS... 130 euros fedex fee + custom's tax for a 70 euro package....

ardvark

I had to remind myself how to directly connect the motion plus (MP) to the nunchuk (NC) so here is the color-code in case it is of use:

NC        ---> MP

yellow   ---> 2
white    ---> 6
red       ---> 1 + 3 (need a jumper between 1 and 3)
green    ---> 5

y2k4u

#8
Oct 30, 2009, 06:05 am Last Edit: Oct 30, 2009, 07:40 am by y2k4u Reason: 1
How well do complementary filters perform when compared to Kalman filters?

ardvark

What are your performance criteria?

Have a look here:

http://www.youtube.com/watch?v=HzkSJKIBofs

and here:

http://www.google.com.au/search?hl=en&safe=off&client=firefox-a&rls=org.mozilla:en-GB:official&hs=DXP&ei=4HTqSqmiNI3q6AOcpp3mCw&sa=X&oi=spell&resnum=0&ct=result&cd=1&ved=0CAkQBSgA&q=complementary+filter+kalman&spell=1

y2k4u

My criteria are resistance to noise/vibration and minimal drift.  The video seems to show that Kalman filter responds quickest but has the most jitter when compared to Complementary & extended Kalman.  I could not tell the difference between Complementary & extended Kalman, could anyone?

ardvark: Thanks for posting the codes for Complementary filter using passthrough.  Great work!

ardvark

Glad you found the code useful.

The implementation in the video shows the extended kalman combining the smoothness of the comp filter with the speed of the kalman.

To some extent the comp filter code above allows the user to easily tweak their own balance between drift correction (gyro sensor problem) and displacement 'noise' (accel sensor problem). See comments in code for more details. This performance adjustability makes comp/kalman filter comparisons a bit more murky.

y2k4u

Any chance that some smart programmers out there would take on the task of implementing Extended Kalman filter?
It had been many decades since I last took college math and those Kalman theories and equations looked more or less like Greek.  I am sure others would also appreciate Extended Kalman filter implementation as well.

ardvark

If you really want an extended kalman your best path would probably be to start with the kalman code posted in the thread that this thread came from and perhaps personally message the posters to ask if they would be willing to work with you on it. What do you want it for?

y2k4u

#14
Nov 17, 2009, 05:43 pm Last Edit: Nov 17, 2009, 05:43 pm by y2k4u Reason: 1
I have been playing with complimentary & Kalman filters and both work reasonably well as long as the sensors are not rolled or pitched inverted.  Both filter codes do not know when the sensors are inverted and output attitudes as if the sensors are right-side-up.

Also, how do I convert the accelerometer data (ax_m, ay_m, az_m) into G-force?  What are the formula?

Go Up