Go Down

Topic: 3D maths to sutract gravity from accelerometer data (Read 29391 times) previous topic - next topic

michinyon

You want the accelerometer to be as close as possible to the centre of mass,  to minimise the apparent acceleration due to the device rotating.

rogerClark

Quote
It should be showing +1g straight up,  due to the force of the tabletop,  or whatever else is stopping your device from falling into a bottomless pit.


The total magnitude of the accelerometers while at rest are approximately 1.0 (allowing for noise and other errors)

I think the whole issue of this acceleration (e.g. 1.0 G )has been mentioned already in this thread.

The key thing is that the MPU9150 reports a value due to gravity, ie in the direction of gravity (or the inverse of this direction, depending on how you interpret the numbers).
So if you want to determine the value of accelerations applied to the sensor, the gravity vector needs to be removed from the accelerometer outputs.

Jim's matrix code has done this very effectively

Freelance developer and IT consultant
www.rogerclark.net

rogerClark

Just a quick update on my progress (or lack of progress with this)

I didn't have much time at the weekend to work on it, but... I soldered a 50cm long ribbon cable to my spare MPU9150 breakout board and have inserted it inside of my 24 sided polyhedron.

However it initially looked like  the magnetometer in my spare board is defective as it was getting wildly varying readings.
Luckily this transpired to be a power supply issue as I'd been running the board on 3.3V but it has its own internal regulator, for 3.3V and although it works OK on sort wires, it must have been loosing too much voltage on longer wires, or there is a difference in the 3.3V output from a Uno to my other test board a Deumilanove.

after that problem was solved, I tried inserting the module in the polyhedron again, but I noticed that the accelerometer values were also very noisy, especially the Z accelerometer (when the board was lying flat on the bench), i.e the one with the largest gravity component.

It looks like these devices are actually very sensitive to the smallest vibration.

Physically holding the module down onto the desk reduced some of the acceleration noise, but not to the point where it was less than the gyro noise.

So I then tried using 2 x 2kg bar bell weights, one above and one below the module, with the weights sitting on top of 10cm of foam rubber, e.g. foam rubber for cushions and seats

So I'm going to need to do some more testing to determine how to even collect the calibration data, as just putting the sensor inside my plastic polyhedron is likely to have too much noise to generate the calibration data e.g. to correct the offsets etc

However it could be that Magneto will smooth out the noise (I will need to investigate further)




Freelance developer and IT consultant
www.rogerclark.net

rogerClark

#48
Sep 23, 2014, 01:02 pm Last Edit: Sep 23, 2014, 01:05 pm by rogerClark Reason: 1
A quick progress update on my quest to use an MPU9150 for even the most basic inertial movement sending

I finally got some data from my second MPU9150 when inside a 24 sided polyhedron (so that there were 24 stable positions in which I could position the sensor without lots of vibration etc to cause the accelerometers problems)

The raw data from the accelerometers was

-16106   2465   -251
-14078   -7971   2745
-14047   -8012   2607
-13858   -8459   2453
-13092   1271   10149
-12979   -4655   -8400
-12913   2301   10213
-12817   -3381   -9188
-12808   -3321   -9277
-12774   -3382   -9319
-12771   2116   10401
-11321   7853   -8491
-11291   7681   -8650
-11232   7861   -8573
-11178   8258   -8327
-11161   7601   -8886
-11082   8454   -8272
-11079   8477   -8207
-11029   11746   3227
-10972   11822   3140
-10772   12073   2933
-9626   -12392   -4366
-9626   -12409   -4393
-9325   -7280   11796
-9122   -7599   11769
-8851   -7728   11883
-6881   1297   -14484
-6794   1229   -14570
-6665   1109   -14604
-6622   9745   11884
-6574   9517   12091
-6304   9607   12197
-5943   -14611   4959
-5544   14817   -3988
-5305   14833   -4148
-3590   -8930   -12959
-3380   273   16574
-1196   10020   -12559
-1139   15518   5710
-1084   10010   -12590
-1057   10013   -12584
1024   -15503   -4919
1102   -10044   13430
1294   -15510   -4755
3519   -250   -15686
3880   9064   13610
5235   -14791   5231
5408   14721   -4245
6352   -9432   -11382
6744   -1041   15403
8685   7655   -11082
8875   12749   5587
8948   12606   5788
11306   -11392   -2516
11507   -11198   -2477
11624   -7860   8863
11668   -7635   8961
11784   -7457   8949
12774   4714   9494
12972   -1745   -9257
12974   -1764   -9279
13223   3860   9206
13694   8471   -1742
13702   8523   -1735
15946   -3067   1105


Then running this through magneto gave this output

Quote
Average length of 65 input vectors = 16372.896147

Expected norm of local field vector?

Combined bias vector B:
 -38.33    -7.82   434.04

Correction matrix Ainv, using Hm=1.000000:
 0.00006   0.00000  -0.00000
 0.00000   0.00006  -0.00000
-0.00000  -0.00000   0.00006

Where Hcalc = Ainv*(H-B)

Code initialization statements...

float B {  -38.33,   -7.82,  434.04};

float Ainv {{  0.00006,  0.00000, -0.00000},

            {  0.00000,  0.00006, -0.00000},

            { -0.00000, -0.00000,  0.00006}};



In order to take these readings I had to write 2 programs.

I had to write an Arduino program to only take a rolling average of the data, as its too noisy to use otherwise, and also just to output a value when the output has settled within defined limits for 5 seconds.

I also had to write a Processing program to visualise the points as I was collecting them, as its easy to get mixed up about which faces on the polygon you have done or not done already

This is the processing sketch. Its a bit messy, as I couldn't figure out how to append (add an array index) to an existing array, if the arrays were multi dimensional, so I cheated and had three separate arrays one for X, Y and Z

I may get around to tidying this up later

Code: [Select]

import peasy.*;
import processing.serial.*;
Serial arduino;

PeasyCam cam;

private float[] dataX = {};
private float[] dataY = {};
private float[] dataZ = {};



void setup() {
 size(640,480,P3D);
 cam = new PeasyCam(this, 100);
 cam.setMinimumDistance(50);
 cam.setMaximumDistance(500);
 
 println(arduino.list()); // Use this to print connected serial devices
 arduino = new Serial(this, Serial.list()[0], 115200);
 arduino.bufferUntil('\n'); // Buffer until line feed
}

void createPointAt(float x,float y,float z)
{
 pushMatrix();
 translate(x, y, z);
 noStroke();
// fill(0,0,255);
 box(5);
 popMatrix();
}

void draw() {
 int i=0;
 rotateX(-.5);
 rotateY(-.5);
 background(0);
//  fill(255,0,0);

// if (i>1)
 {
   fill(255,255,255);
   for(i=0;i<dataX.length;i++)
   {
     createPointAt(dataX[i]/200,dataY[i]/200,dataZ[i]/200);
   }
 }
 if (i>0)
 {
  //fill(255,0,0);
  // createPointAt(dataX[i]/200,dataY[i]/200,dataZ[i]/200);
 }
 

}

void serialEvent (Serial arduino)
{
 
 String accelXStr,accelYStr,accelZStr;
 
 arduino.readStringUntil('\t');// read pramble
 // get the ASCII strings:
 accelXStr= arduino.readStringUntil('\t');
 accelYStr= arduino.readStringUntil('\t');
 accelZStr= arduino.readStringUntil('\t');

 // read gyros
 arduino.readStringUntil('\t');
 arduino.readStringUntil('\t');
 arduino.readStringUntil('\t');

 // read magnetometer
 arduino.readStringUntil('\t');
 arduino.readStringUntil('\t');
 arduino.readStringUntil('\t');

 arduino.clear(); // Clear buffer
 println(accelXStr+","+accelYStr+","+accelZStr);

 dataX = append(dataX,float(accelXStr));
 dataY = append(dataY,float(accelYStr));
 dataZ = append(dataZ,float(accelZStr));

}



This is the arduino code (again a bit messy) but may be of use to someone

Code: [Select]

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

#define LED_PIN 13
#define NOISE_MAX 32

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;
int16_t mpuData[9];
int16_t mpuDataLast[9];

enum states { UNSTABLE,STABILITY_TIMING,PRINTING, WAIT_FOR_UNSTABLE};

enum states state = UNSTABLE;// Current state. i.e The charger is waiting for the user to insert the battery

long lastStableTime=0;



float alpha = 0.1; // factor to tune

void setup() {
 // join I2C bus (I2Cdev library doesn't do this automatically)
 Wire.begin();

 // initialize serial communication
 // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
 // it's really up to you depending on your project)
 Serial.begin(115200);

 // initialize device
//  Serial.println("Initializing I2C devices...");
 accelgyro.initialize();

 // verify connection
// Serial.println("Testing device connections...");
// Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

 // configure Arduino LED for
 pinMode(LED_PIN, OUTPUT);
}



int arrayIndexMaxDiffernce(int16_t *array1,int16_t *array2,int items)
{
int maxVal=0;
int diff;

 for(int i=0;i<items;i++)
 {
   diff = abs(array1[i]-array2[i]);
   if (diff>maxVal)
   {
     maxVal=diff;
   }  
 }
 return maxVal;
}

void loop() {
 // read raw accel/gyro measurements from device
 accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);

 mpuData[0]=alpha *ax + (1-alpha) *mpuData[0];
 mpuData[1]=alpha *ay + (1-alpha) *mpuData[1];
 mpuData[2]=alpha *az + (1-alpha) *mpuData[2];

 mpuData[3]=alpha *gx + (1-alpha) *mpuData[3];
 mpuData[4]=alpha *gy + (1-alpha) *mpuData[4];
 mpuData[5]=alpha *gz + (1-alpha) *mpuData[5];

 mpuData[6]=alpha *mx + (1-alpha) *mpuData[6];
 mpuData[7]=alpha *my + (1-alpha) *mpuData[7];
 mpuData[8]=alpha *mz + (1-alpha) *mpuData[8];

 switch(state)
 {
   case UNSTABLE:
    // Serial.print("U");
     if (arrayIndexMaxDiffernce(mpuDataLast,mpuData,3)<NOISE_MAX)
     {
       state=STABILITY_TIMING;
       lastStableTime=millis();
     }
    break;
   case STABILITY_TIMING:
     if (arrayIndexMaxDiffernce(mpuDataLast,mpuData,3)>=NOISE_MAX)
     {
       state=UNSTABLE;
     }
     else
     {
       // Must be stable for at least 5 secs
       if (millis() - lastStableTime>5000)
       {
         state=PRINTING;
       }
     }
     break;
   case PRINTING:
     Serial.print("a/g/m:\t");
     Serial.print(mpuData[0]);
     Serial.print("\t");
     Serial.print(mpuData[01]);
     Serial.print("\t");
     Serial.print(mpuData[2]);
     Serial.print("\t");
     Serial.print(mpuData[3]);
     Serial.print("\t");
     Serial.print(mpuData[4]);
     Serial.print("\t");
     Serial.print(mpuData[5]);
     Serial.print("\t");
     Serial.print(mpuData[6]);
     Serial.print("\t");
     Serial.print(mpuData[7]);
     Serial.print("\t");
     Serial.println(mpuData[8]);  
     state=WAIT_FOR_UNSTABLE;
    break;
 
   case WAIT_FOR_UNSTABLE:
     //Serial.print("S");
     if (arrayIndexMaxDiffernce(mpuDataLast,mpuData,3)>=NOISE_MAX)
     {
       state=UNSTABLE;
     }
    break;
 }  
 memcpy(mpuDataLast,mpuData,sizeof(mpuData));
}


I still need to collect the same calibration data for the magnetometers, and process it etc

Then I need to implement the calibration to RTImuLib to replace the existing compass only calibration..

So I still have a lot of work ahead, but I am making slow progress.

However goodness knows if even after calibration the data will be usable for any for of inertial measurement!
Freelance developer and IT consultant
www.rogerclark.net

jremington

#49
Sep 23, 2014, 04:51 pm Last Edit: Sep 23, 2014, 06:06 pm by jremington Reason: 1
Hi, Roger:

In this case, you should be using a value for close to 16000 for Hm as input to Magneto, because you are losing all the precision in the correct matrix. Or, you could divide the values output by the sensor by a suitable number, so that the average length of the input vectors is close to 1.0.
Quote

Correction matrix Ainv, using Hm=1.000000:
 0.00006   0.00000  -0.00000
 0.00000   0.00006  -0.00000
-0.00000  -0.00000   0.00006

Example:
Quote
Average length of 65 input vectors = 16372.896147

Expected norm of local field vector? 16372.

Combined bias vector B:
 -38.33    -7.82   434.04

Correction matrix Ainv, using Hm=16372.000000:
 1.00653   0.00008  -0.00177
 0.00008   0.99752  -0.00036
-0.00177  -0.00036   0.99241

Where Hcalc = Ainv*(H-B)


The diagonals of the matrix are the axial scale factors, so the gains on each axis are already the same to better than 1%.

You are doing nice work, so keep it up. I think a lot of people will be interested in how it works out in the end!

Nice data, BTW. Here are the raw data, scaled by 1/16372, plotted on a unit sphere.

rogerClark

#50
Sep 23, 2014, 11:01 pm Last Edit: Sep 23, 2014, 11:42 pm by rogerClark Reason: 1
Thanks Jim

I did notice that the Ainv matrix values looked very low, but had not had chance to actually use them.

The rather simplistic Processing program I wrote allowed me to view the points in 3D as I collected them, but I will have to find an open source program to plot the data after its collected, as Matlab is too expensive for me.
I know there are some open source equivalents, I just need to work out the best one to learn and use.

I will collect some more data points, as it seems likely that Hm is probably supposed to be 16384, as my average length of 16372 is only 12 from that value, and my current noise threshold is set to 32.

I will also take a look in the spec for the MPU9150 to see if it details where for the accelerometers e.g. Where they would have 14 bit accuracy to read 1.0g

One thing I have noticed, is that the accelerometers are very sensitive. They definitely pick up very small vibrations from the fans in my PC.

At once point, to try to reduce this pickup, I sandwiched the sensor between some 2kg bar bell weights, which were placed on top of some foam.

However to record the points I had to go back to using my polyhedron, which is actually far too light and needs to be weighted somehow. I think I need to fill the polyhedron with something heavy, e.g. Sand or possibly lead shot, but 3d printing a hollow shape with a central chamber is a bit tricky on domestic 3D printers, but that's another very long story ;-)


I will also try collecting the data in another way.
Currently my Arduino program only outputs a point after the data has been stable for 5 seconds, and is heavily averaged.
But I may change this, so that data is collected when I hit a key in Processing, which would give me better visualisation of the current orientation of the polyhedron, as its almost guess work at the moment to work out which way to then the polyhedron to orient it to a point that is missing, hence some duplicate points in the data I posted.

Anyway, we will see how this goes, its just proving to be a little more involved that I'd imagined, but it doesn't hurt too much to be at the bleeding edge ;-)


Edit.

Just looked in the register specification for the MPU9150 and it says that the  sensitivity is 16384 LSB/mg

The g is italicised.

So basically the norm of the acceleration vectors should add up to 16384 in the sensitivity mode that the basic mpu9150 seems to use.
I suspect this is the default range of plus or minus 2 g
However it can be changed to make the unit less sensitive, which may actually be a benefit as long as there is a corresponding reduction in noise.

Either way, Hm should be 16384 so I will use this value in Magneto
Freelance developer and IT consultant
www.rogerclark.net

rogerClark

Hi Jim

I've rewritten by data collection sketch using Processing, and have modified the Arduino code as well, to make it easier to capture the data points of the accelerometer

Basically the new Arduino version, continuously averages the incoming data using a rolling average (with alpha of 0.05)
And prints all data to serial every 250ms

The Processing sketch displays the current position as a white cube, and you can move the 3D view using the mouse (this comes for free with the PeasyCam library for Processing !)

Once the sensor is in a stable location, i.e the white cube is not moving on screen, you press the "s" key which saves that value to an ArrayList

The points in the ArrayList are plotted in cyan, so you can see which points you have already recorded data for and which area of the sphere of rotation still needs more points.

When you have collected enough points, you press "p" to save the data to a file "points.txt" which is tab separated for Magento to use !

e.g.

-16015.0   2857.0   -110.0
-5287.0   14863.0   -4167.0
-10897.0   8761.0   -8250.0
-10612.0   12232.0   2610.0
-12746.0   2221.0   10300.0
-13233.0   -4100.0   -8429.0
-7708.0   1209.0   -14219.0
-4166.0   -8446.0   -13218.0
-12945.0   -3152.0   -9222.0
-6444.0   9668.0   11898.0
-3624.0   374.0   16374.0
791.0   -10052.0   13324.0
4613.0   -15099.0   4755.0
10875.0   -11773.0   -2629.0
6054.0   -9542.0   -11610.0
3451.0   -319.0   -15815.0
-1061.0   10056.0   -12693.0
13766.0   8384.0   -2027.0
16168.0   -1491.0   1146.0
12434.0   -6050.0   9044.0
7477.0   13673.0   5406.0
13320.0   -69.0   -9089.0
7343.0   -8563.0   -11584.0
-8135.0   -13438.0   -4356.0
2472.0   -15370.0   -4812.0
3771.0   15207.0   -4485.0
-3087.0   15296.0   5360.0
2843.0   9046.0   13801.0
6171.0   -235.0   15628.0
12527.0   5518.0   9283.0
-7886.0   8697.0   11795.0
-13034.0   -9611.0   2408.0
-6404.0   -14700.0   3737.0
-9509.0   -8055.0   10951.0
7770.0   8658.0   -11231.0


(note there are still some semi duplicate points, but this is because I'm now resting my polyhedron on a piece of fabric as there wasn't enough friction with my desktop and the polyhedron to stop it slipping and wobbling a bit because of the ribbon cable
However, I've not had chance to test again with Magneto.
(I will post a picture of the test rig another day)




-------------------  processing code  ------------------------------------------------
Code: [Select]

import peasy.*;
import processing.serial.*;
Serial arduino;

PeasyCam cam;

ArrayList<Vector3> accelData;


private float accelX,accelY,accelZ;



void setup()
{
  accelData = new ArrayList<Vector3>();
  size(1024,768,P3D);
  cam = new PeasyCam(this, 100);
  cam.setMinimumDistance(50);
  cam.setMaximumDistance(500);
 
  println(arduino.list()); // Use this to print connected serial devices
  arduino = new Serial(this, Serial.list()[0], 115200);
  arduino.bufferUntil('\n'); // Buffer until line feed
}

void createPointAt(float x,float y,float z)
{
  pushMatrix();
  translate(x, y, z);
  noStroke();
// fill(0,0,255);
  box(5);
  popMatrix();
}

void draw() {

  rotateX(-.5);
  rotateY(-.5);
  background(0);
  fill(255,255,255);

  createPointAt(accelX/100,accelY/100,accelZ/100);
 
  fill(0,255,255);
 
  for (int i = 0;i< accelData.size(); i++)
  {
    Vector3 saved = accelData.get(i);
    createPointAt(saved.x/100,saved.y/100,saved.z/100);
  }
}

void keyPressed()
{
// println(key);
  if (key=='s')
  {
    accelData.add(new Vector3(accelX,accelY,accelZ));  // Start by adding one element
  }
 
if (key=='p')
{
    PrintWriter output = createWriter("positions.txt");
   
    for (int i = 0;i< accelData.size(); i++)
    {
      Vector3 saved = accelData.get(i);
      output.println(saved.x + "\t" + saved.y + "\t" + saved.z );
    }   
   
    output.flush(); // Writes the remaining data to the file
    output.close(); // Finishes the file
  }
 
}

void serialEvent (Serial arduino)
{
 
  String accelXStr,accelYStr,accelZStr;
 
  arduino.readStringUntil('\t');// read pramble
  // get the ASCII strings:
  accelXStr= arduino.readStringUntil('\t');
  accelYStr= arduino.readStringUntil('\t');
  accelZStr= arduino.readStringUntil('\t');

  // read gyros
  arduino.readStringUntil('\t');
  arduino.readStringUntil('\t');
  arduino.readStringUntil('\t');

  // read magnetometer
  arduino.readStringUntil('\t');
  arduino.readStringUntil('\t');
  arduino.readStringUntil('\t');

  arduino.clear(); // Clear buffer
//  println(accelXStr+","+accelYStr+","+accelZStr);

  accelX=float(accelXStr);
  accelY=float(accelYStr);
  accelZ=float(accelZStr);
  //println(accelX+","+accelY+","+accelZ+","+Math.sqrt(accelX*accelX+accelY*accelY+accelZ*accelZ));
/*
  dataX = append(dataX,float(accelXStr));
  dataY = append(dataY,float(accelYStr));
  dataZ = append(dataZ,float(accelZStr));
  */

}

class Vector3
{
  public float x;
  public float y;
  public float z;
  public Vector3(float theX,float theY,float theZ)
  {
    x=theX;
    y=theY;
    z=theZ;
  }
}



------------------------- Arduino code -------------------------------

Code: [Select]


// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

#define LED_PIN 13
#define NOISE_MAX 32

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;
int16_t mpuData[9];
int16_t mpuDataLast[9];



float alpha = 0.05; // factor to tune

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();

  // initialize serial communication
  // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
  // it's really up to you depending on your project)
  Serial.begin(115200);

  // initialize device
//  Serial.println("Initializing I2C devices...");
  accelgyro.initialize();

  // verify connection
// Serial.println("Testing device connections...");
// Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  // configure Arduino LED for
  pinMode(LED_PIN, OUTPUT);
}


long lastPrintTime;
#define PRINT_INTERVAL 250

void loop()
{
  // read raw accel/gyro measurements from device
 
  accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);

  mpuData[0]=alpha *ax + (1-alpha) *mpuData[0];
  mpuData[1]=alpha *ay + (1-alpha) *mpuData[1];
  mpuData[2]=alpha *az + (1-alpha) *mpuData[2];

  mpuData[3]=alpha *gx + (1-alpha) *mpuData[3];
  mpuData[4]=alpha *gy + (1-alpha) *mpuData[4];
  mpuData[5]=alpha *gz + (1-alpha) *mpuData[5];

  mpuData[6]=alpha *mx + (1-alpha) *mpuData[6];
  mpuData[7]=alpha *my + (1-alpha) *mpuData[7];
  mpuData[8]=alpha *mz + (1-alpha) *mpuData[8];

  if (millis()-lastPrintTime > PRINT_INTERVAL)
  {
    lastPrintTime=millis();
    Serial.print("a/g/m:\t");
    Serial.print(mpuData[0]);
    Serial.print("\t");
    Serial.print(mpuData[01]);
    Serial.print("\t");
    Serial.print(mpuData[2]);
    Serial.print("\t");
    Serial.print(mpuData[3]);
    Serial.print("\t");
    Serial.print(mpuData[4]);
    Serial.print("\t");
    Serial.print(mpuData[5]);
    Serial.print("\t");
    Serial.print(mpuData[6]);
    Serial.print("\t");
    Serial.print(mpuData[7]);
    Serial.print("\t");
    Serial.println(mpuData[8]); 
  }     
  memcpy(mpuDataLast,mpuData,sizeof(mpuData));
}

Freelance developer and IT consultant
www.rogerclark.net

rogerClark

Hi Jim ;-)

Just thought I'd post about my progress, or lack of it.

I've written an improved data collection / visualization program in Processing, (I'll post it to the forum some other time) and have logged a load of accelerometer points.

I've run the data through Magneto using the norm value of 16384 (which is what the module should be outputting according to the spec, however the data is actually around 16400)

I've also modified Richards RTIMULib to pass the accelerometer data through the correction matrix generated by Magneto

Specifically


I replaced the code in RTIMULib:ReadIMU  where it takes the raw data from the IMU and converts it into a vector

To this code.

i.e I'm using the existing call to convertToVector to read the bytes and put them into a float (but not to scale them, hence the 1.0 value in the call to convertToVector

 
Code: [Select]
 
RTMath::convertToVector(fifoData, m_accel, 1.0, true);
RTFLOAT mA[3];
const float B[3]= {   33.28,    5.67,  175.59};
const float Ainv[3][3] {{  1.00766,  0.00021, -0.00172},
{  0.00021,  0.99880, -0.00008},
{ -0.00172, -0.00008,  0.99231}};
mA[0]=m_accel.data(0)-B[0];
    mA[1]=m_accel.data(1)-B[1];
    mA[2]=m_accel.data(2)-B[2];


  m_accel.setX((mA[0]*Ainv[0][0] + mA[1]*Ainv[1][0] + mA[2]*Ainv[2][0])/16384) ;
  m_accel.setY((mA[0]*Ainv[0][1] + mA[1]*Ainv[1][1] + mA[2]*Ainv[2][1])/16384) ;
  m_accel.setZ((mA[0]*Ainv[0][2] + mA[1]*Ainv[1][2] + mA[2]*Ainv[2][2])/16384) ;



However the gravity component removal doesn't seem significantly better than before using the calibration.

e.g. here is the data corrected for gravity


0.0018919   0.0014599   0.0001704
0.0031373   0.0012835   0.0017567
0.0047418   0.0024864   -0.0009026
0.0031524   -0.0000058   -0.00165
0.0031689   0.0006141   -0.0017962
0.0031681   -0.0005702   -0.0009043
0.0040097   0.000357   -0.0055767
0.0027707   0.001875   0.0020655
0.0018037   0.0013485   0.0021994
0.0042856   0.0006297   -0.0024435
0.0033   0.0009674   -0.0000077
0.0002093   -0.000059   -0.0018391
0.0026497   -0.0004541   0.0012401
0.0019178   0.0003506   -0.0008347
0.0037711   0.0008411   -0.0009435
0.0026653   0.0009934   0.0007575
0.004013   0.0008397   -0.0031794
0.0002097   0.0011096   0.0005183
0.0007066   -0.0003783   -0.0008366
0.0025557   -0.0008546   0.0010871
0.0019444   -0.0015502   -0.0017663
0.0021873   0.000241   0.0002646
0.0051282   -0.0005965   -0.0007019
0.0010861   0.0002038   0.000004
0.0031642   0.0001577   -0.0032402
0.0022926   0.0010415   0.0018161
0.0040169   0.0021095   -0.0003793
0.0041595   0.0003021   0.0008044
0.0056288   0.0011443   -0.0054634
0.0048873   -0.0006093   0.0025563
0.002426   0.0004905   -0.0009135
0.0047688   0.0014672   0.002122
0.0063796   -0.0011358   -0.0006563
0.0030499   0.0011712   0.0010752
0.0004792   0.001616   0.0014413
0.0030464   -0.0000384   -0.0042373
0.0025423   -0.0007216   0.0022721
0.0032759   0.0010418   0.0007539
0.0047487   -0.0003394   -0.0002442
0.0057451   0.0018519   0.0011868
0.0025552   0.0013436   -0.0003062
0.0022039   0.0000167   0.0022955
0.0019432   0.0021595   0.0000551
0.0023009   0.0007673   0.000376
0.0029233   0.0021742   -0.0004745
0.0046384   -0.0005088   0.0000578
0.0021787   -0.0004098   0.0024315
0.0018421   -0.0013523   0.0003736
0.0040774   0.0014133   0.0021778
0.0029175   0.0001216   -0.001635
0.0025465   0.0002813   -0.0020852
0.0013124   -0.0009246   0.0003555
0.002304   -0.0013456   -0.0023066
0.0034308   0.0002393   0.0015787
0.00293   -0.0025219   -0.00241
0.0044138   0.000138   0.0021692
0.0035568   0.0014652   -0.0004961
0.0058768   0.0000114   -0.0013364
-0.0018679   -0.0026498   -0.0022747
0.0027935   -0.0003032   0.0012427
-0.0016251   0.0007131   0.0036529
0.0010821   0.0015057   -0.0014166
0.0046248   -0.0008992   0.0024662
0.0000771   -0.0004114   -0.0009543
-0.0020131   -0.0012183   0.0002942
-0.0015084   0.0002153   0.0006348
0.0011773   0.0008064   0.0009953



Anyway. Thinking about it...

If the "pose" vector being reported by RTMIU is not actually aligned with gravity, then were are bound to be errors

Admitidly, I have not applied full calibration to the compass data yet, so this may help, but I will need to contact Richard (of RTIMULib) to ask about how he calculates the "pose" value, and whether its referenced to where he thinks is level / horizontal in two planes, or somehow related to the magnetometer readings.


And... I've got another annoying issue.

The code hangs after around 30 seconds.

So it looks like RTIMU either has a memory leak or a buffer overflow, or perhaps my sensor has stopped sending data.
However the MPU6050 library that I also use, doesnt have these issues, so I know its not fundamentally a hardware issue, and also both of my MPU9150 boards also have this problem, so its definately a software issue, and most likely in RTIMULib :-(

I also just thought I'd post my test rig.

   



Freelance developer and IT consultant
www.rogerclark.net

rogerClark

Jim,

And anyone else ;-)

I'm beginning to wonder if I'm doing this fundamentally the wrong way.

I wonder if a better approach would be to try to generate a vector angle correction matrix which would take as its input the magnetometer data and output the vector difference between the mag data and the gravity vector (this could also do the inversion at the same time I suppose)

This would require collection of a lot of data points, but I have a rig and software to do that.

However I'd need some equivalent of Magneto that did vector correction based on two sets of input data.

Looking at the 2 sets of data as I collected them (visualising them in Processing), the two sets of data look quite different.

I guess the main difference is the magnetic variation / declination vector, which is specific to where I am

i.e I could build a lookup table with mag vectors, but I'd have to limit the size of this lookup table on the Uno etc, as there is only around 10k space left of this , even with a very basic implementation of getting data from the MPU (e.g. using the MPU6050 lib )


Also...
I'm not sure how RTIMUlib removes this, but I've posted to Richards blog, so he may get back to me with information and possibly some ideas.

However at the moment I've reached a bit of an impass.

I do ultimately believe that some basic movement and position information can be obtained my devices like the MPU9150, but that I've just not worked out the best way to do this.

Perhaps I need to post this to some other forums, as it looks like I'm the only one currently investigating this ;-(
Freelance developer and IT consultant
www.rogerclark.net

jremington

Roger:

The subtraction of gravity for those data worked spectacularly well.

I calculated the magnitude of the vectors, and see that the largest is about 0.0079 g (or 8 millig). Before, you had magnitudes larger than 0.1 g, if I recall correctly. One could conclude that calibration led to an overall improvement of better than a factor of 10.

In the following, the fourth column is the vector magnitude.
Edit: I see you've posted while I was typing, but I don't understand the reason for your discouragement.

Quote
0.0018919   0.0014599   0.00017040   0.0023958
0.0031373   0.0012835   0.0017567   0.0038179
0.0047418   0.0024864   -0.00090260   0.0054297
0.0031524   -5.8000e-06   -0.0016500   0.0035581
0.0031689   0.00061410   -0.0017962   0.0036940
0.0031681   -0.00057020   -0.00090430   0.0033436
0.0040097   0.00035700   -0.0055767   0.0068778
0.0027707   0.0018750   0.0020655   0.0039318
0.0018037   0.0013485   0.0021994   0.0031479
0.0042856   0.00062970   -0.0024435   0.0049733
0.0033000   0.00096740   -7.7000e-06   0.0034389
0.00020930   -5.9000e-05   -0.0018391   0.0018519
0.0026497   -0.00045410   0.0012401   0.0029606
0.0019178   0.00035060   -0.00083470   0.0021208
0.0037711   0.00084110   -0.00094350   0.0039773
0.0026653   0.00099340   0.00075750   0.0029435
0.0040130   0.00083970   -0.0031794   0.0051882
0.00020970   0.0011096   0.00051830   0.0012425
0.00070660   -0.00037830   -0.00083660   0.0011586
0.0025557   -0.00085460   0.0010871   0.0029058
0.0019444   -0.0015502   -0.0017663   0.0030502
0.0021873   0.00024100   0.00026460   0.0022164
0.0051282   -0.00059650   -0.00070190   0.0052103
0.0010861   0.00020380   4.0000e-06   0.0011051
0.0031642   0.00015770   -0.0032402   0.0045317
0.0022926   0.0010415   0.0018161   0.0031047
0.0040169   0.0021095   -0.00037930   0.0045529
0.0041595   0.00030210   0.00080440   0.0042473
0.0056288   0.0011443   -0.0054634   0.0079273
0.0048873   -0.00060930   0.0025563   0.0055490
0.0024260   0.00049050   -0.00091350   0.0026383
0.0047688   0.0014672   0.0021220   0.0054219
0.0063796   -0.0011358   -0.00065630   0.0065131
0.0030499   0.0011712   0.0010752   0.0034394
0.00047920   0.0016160   0.0014413   0.0022178
0.0030464   -3.8400e-05   -0.0042373   0.0052189
0.0025423   -0.00072160   0.0022721   0.0034852
0.0032759   0.0010418   0.00075390   0.0035193
0.0047487   -0.00033940   -0.00024420   0.0047671
0.0057451   0.0018519   0.0011868   0.0061518
0.0025552   0.0013436   -0.00030620   0.0029031
0.0022039   1.6700e-05   0.0022955   0.0031823
0.0019432   0.0021595   5.5100e-05   0.0029056
0.0023009   0.00076730   0.00037600   0.0024544
0.0029233   0.0021742   -0.00047450   0.0036740
0.0046384   -0.00050880   5.7800e-05   0.0046666
0.0021787   -0.00040980   0.0024315   0.0032904
0.0018421   -0.0013523   0.00037360   0.0023155
0.0040774   0.0014133   0.0021778   0.0048338
0.0029175   0.00012160   -0.0016350   0.0033466
0.0025465   0.00028130   -0.0020852   0.0033033
0.0013124   -0.00092460   0.00035550   0.0016443
0.0023040   -0.0013456   -0.0023066   0.0035270
0.0034308   0.00023930   0.0015787   0.0037842
0.0029300   -0.0025219   -0.0024100   0.0045555
0.0044138   0.00013800   0.0021692   0.0049200
0.0035568   0.0014652   -0.00049610   0.0038786
0.0058768   1.1400e-05   -0.0013364   0.0060268
-0.0018679   -0.0026498   -0.0022747   0.0039604
0.0027935   -0.00030320   0.0012427   0.0030724
-0.0016251   0.00071310   0.0036529   0.0040612
0.0010821   0.0015057   -0.0014166   0.0023334
0.0046248   -0.00089920   0.0024662   0.0053178
7.7100e-05   -0.00041140   -0.00095430   0.0010421
-0.0020131   -0.0012183   0.00029420   0.0023714
-0.0015084   0.00021530   0.00063480   0.0016506
0.0011773   0.00080640   0.00099530   0.0017398

rogerClark

Hi Jim

Thanks for the reply.

I guess I was hoping that I would not end up with an offset, as this is going to cause position drift

Of course I can easily average the offset out, using a simple rolling average.

BTW. I did the same as you, ie calculated the total size of the offset vector and it was indeed very low.

I'm probably expecting too much of consumer grade electronics ;-)

If you think that it is indeed working as you'd expect. I'll modify the code to use the magnetometer correction data from Magneto (as I did spend nearly an hour collecting data  ---- yes it takes ages to make sure I have a full set of points covering the whole spherical shape of possible value)

And put in the code to do the same thing I did for the accelerometers.

However I still need to work out why RTIMULib is hanging after a short time.
i.e determine if there is a memory leak or some sort of recursion in the code or buffer overflow, or possibly some command to the MPU via I2C which is causing it to actually stop recoding data.

I had a quick look at richards code and I don't think its a buffer overflow in reading the data, as he just reads 12  byes of data from the IMU at a time and relies on the internal FIFO

He basically reads the data and keeps reading the fifo and chucks it away until its empty, or if its totally full he resets it in the MPU.

Personally I think it may be better to aggregate / average any data in the fifo until its empty, as this will probably improve overall accuracy (at least for accelerometer) but thats another story !

Anyway, thanks again for your positive response, at least I know I'm not banging my head against a brick wall with this ;-)
Freelance developer and IT consultant
www.rogerclark.net

jremington

Roger:

I think you are certainly on the right track, and right now, the errors in subtracting gravity aren't that much larger than the expected noise in the accelerometer measurements. Definitely calibrate the magnetometer, though!

However, I agree that there is room for improvement. I took a closer look at the residuals (the vector list that you posted) and there definitely is a bias. This suggests that the offset correction is not quite right, or perhaps there is some systematic error in the overall algorithm to determine the IMU orientation (magnetometer, perhaps?).

As you can see from the "quiver plot" below, the majority of the residuals are pointing roughly in the positive direction along all three axes, mostly X. So, if  you integrate those data, the sensor would appear to wander off in the positive X direction.

I don't have any of the the MPUxxx sensors, but have been working with several of the LSM303 series. I'm inspired compare their performance with the results of your efforts.

Good work!

Jim

PS: the exact value that you use for Hm is not critical, you just need to avoid the situation of losing significant figures in the correction matrix.

rogerClark

Thanks Jim,

I don't have Matlab so I can't do those nice plots ;-)  but just visually looking at the figures I could see that they all appeared to be in the same quadrant.

Which as we both surmised is probably caused by an inaccuracy between the "pose" (orientation) vector from RTIMULib and the real direction of gravity.

It looks like the RTIMULib supports the LSM9DS0, but I'm not sure if it would work for your LSM303   its probably worth a shot

I'll post my revised Processing data visualation / collection program later (its on my other PC), when I get chance, in case its any use to you

Cheers

Roger
Freelance developer and IT consultant
www.rogerclark.net

rogerClark

Jim

In case you want to have a go at collecting points from your sensor, here is my Processing sketch to visualize and record the data.

It uses the Peasy camera library http://mrfeinberg.com/peasycam/

and it expects tab separated data

row header (redundant) "a/g/m:"
accelX
accelY
accelZ
gyroX
gyroY
gryoZ
magX
magY
magZ

The code ignores the giro and only displays accel and mag values in 3D
They are scaled to suit my sensor which outputs 16384 for 1G and I think its degrees for the Mag (I just found that there was no need to scale (divide) the mag values, but the accel values needed to be divided by 100

pressing   v   toggles viewing both accel and mag, then just accel then just mag
pressing   s   saves the current point (either accel or mag or both depending on view mode)
Saved points are displayed, as well as the current point readings but in different colors)

pressing p writes the saved value to 2 separate files (note this just appends to existing files or creates the file if its not there)

The files it creates are directly compatible with magneto


I've also attached (in Excel format) are my raw readings when the board was as close to level as possible, well when the accel values were reading as close to zero as I could physically get them (using a piece of blue tac to act as a mount )

The values for accel are only shown to 2 dp, so values of 0 are possibly 0.00999  but I can't really position the sensor any better anyway, so re-coding to print to more dp's is probably a waste of time.


I have solved one major issue that I had, which was the code hung completely at random intervals. In the end I had to stick an oscilloscope on the I2C data line, and I found the crash was in the I2C low level code, and was caused by crosstalk between the 2 I2C lines because I was using a 50cm long ribbon cable.

When I pulled the 2 wires apart so the cable is kinda split in two, lengthwise, it seems to have fixed the issue.

But its taken quite a while to figure this out, so I've not had chance to implement the compass calibration.
So I'll try to do that at the weekend.

Overall, I think the errors in the "pose" angle and the accelerometer readings for where level / horizontal should be, are so small, perhaps they are just the product of the lack of double precision floating point on the Arduino

I guess the compass calibration may help, so its worth a try, but I'm not sure it will make much difference.

I think my best course of action is to just generate a separate correction offset using a rolling average, and apply this to the output of the correct accelerometer data to slowly filter out any errors.

So that any drift will eventually become negligible.

Anyway, I'll keep you posted on any progress
Freelance developer and IT consultant
www.rogerclark.net

jremington

Hi, Roger:

Thanks for posting the code. I will try a few things this weekend.
Re:
Quote
I guess the compass calibration may help, so its worth a try, but I'm not sure it will make much difference.

I do not have any experience with the MPUxxxx sensors, but for those that I have experimented with, the magnetometer was far more in need of calibration than the accelerometer. So, I think you can expect a significant improvement in the accuracy of the calculated device orientation, if you make the effort.

Go Up