Pages: [1] 2   Go Down
Author Topic: Kalman Filtered Nunchuck & Motion Plus-the seq  (Read 10793 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 88
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.

« Last Edit: September 17, 2009, 11:34:13 am by ardvark » Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 88
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

« Last Edit: September 17, 2009, 11:30:57 am by ardvark » Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 88
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Here is the code that includes the complimentary filter:
Code:
// .......................................................................
// 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
« Last Edit: September 17, 2009, 11:25:00 am by ardvark » Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 88
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
     
 // 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;
}
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 65
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 88
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

@ 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.
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 65
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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....
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 88
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 4
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

How well do complementary filters perform when compared to Kalman filters?
« Last Edit: October 30, 2009, 01:40:34 am by y2k4u » Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 88
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

What are your performance criteria?

Have a look here:



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
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 4
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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!
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 88
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 4
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

0
Offline Offline
Jr. Member
**
Karma: 0
Posts: 88
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 4
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
« Last Edit: November 17, 2009, 11:43:45 am by y2k4u » Logged

Pages: [1] 2   Go Up
Jump to: