Ellipsoid into Sphere Optimization using Numpy and linalg

Last updated on Thu, 2012-11-15 09:30. Originally submitted by fabio on 2012-10-18 17:45.

I'm working on a user friendly calibration program, since I'm completely aware that what I released some time ago using a python script and Octave is very complex to use for FreeIMU library users.

So, I'm creating a new calibration software which will use Python, Numpy and its linalg component to provide an easy to use libre calibration software.

I started by trying what Nirav Patel suggested but wasn't much successful with it. So, I simply ported the Matlab/Octave code into Python+Numpy+linalg.

Below is the code for you to enjoy and here a sample input file.

"""
cal_lib.py - Ellipsoid into Sphere calibration library based upon numpy and linalg
Copyright (C) 2012 Fabio Varesano 

Development of this code has been supported by the Department of Computer Science,
Universita' degli Studi di Torino, Italy within the Piemonte Project
http://www.piemonte.di.unito.it/


This program is free software: you can redistribute it and/or modify
it under the terms of the version 3 GNU General Public License as
published by the Free Software Foundation.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see .

"""

import numpy
from numpy import linalg

def calibrate(x, y, z):
  H = numpy.array([x, y, z, -y**2, -z**2, numpy.ones([len(x), 1])])
  H = numpy.transpose(H)
  w = x**2
  
  (X, residues, rank, shape) = linalg.lstsq(H, w)
  
  OSx = X[0] / 2
  OSy = X[1] / (2 * X[3])
  OSz = X[2] / (2 * X[4])
  
  A = X[5] + OSx**2 + X[3] * OSy**2 + X[4] * OSz**2
  B = A / X[3]
  C = A / X[4]
  
  SCx = numpy.sqrt(A)
  SCy = numpy.sqrt(B)
  SCz = numpy.sqrt(C)
  
  return ([OSx, OSy, OSz], [SCx, SCy, SCz])


if __name__ == "__main__":
  acc_f = open("acc.txt", 'r')
  acc_x = []
  acc_y = []
  acc_z = []
  for line in acc_f:
    reading = line.split()
    acc_x.append(int(reading[0]))
    acc_y.append(int(reading[1]))
    acc_z.append(int(reading[2]))

  (offsets, scale) = calibrate(numpy.array(acc_x), numpy.array(acc_y), numpy.array(acc_z))
  print offsets
  print scale
Posted in:

Values always peak

Submitted by cin0s (not verified) on Sun, 2012-11-11 17:04.

Hi Guys,
I've got a little problem with my sensores. I don't think this issue is caused by Fabios software, but I think it's caused by the sensore I use. Well. I use the 9DOF Sensore Stick (SEN-10724) and 2 of the 3 axis peak always at a certain angle. Here is a screenshot of the calibration tool:

I also get a similar output if I read the acc sensore values "manually". So it's not Fabios fault, but it is a really anoying issue...

Btw: this is the output I get with this kind of sensore data:
const int acc_off_x = -38;
const int acc_off_y = 52;
const int acc_off_z = 423;
const float acc_scale_x = 186.456572;
const float acc_scale_y = 331.873171;
const float acc_scale_z = 112.734495;

const int magn_off_x = 39;
const int magn_off_y = -128;
const int magn_off_z = 4;
const float magn_scale_x = 387.551331;
const float magn_scale_y = 436.820877;
const float magn_scale_z = 381.809318;

So do you have got any idea how to solve this? I don't have much expirience with such sensores, but i think the sensores is somehow to sensitive. So that the 9.81 m/s² let the sensore peak. Is that possible?

What do you exactly consider

Submitted by fabio on Mon, 2012-11-12 11:57.

What do you exactly consider as peaks? I don't see anything wrong with your graphs, except from the fact that you don't have complete circles for the accelerometer.. you always have to draw complete circles before hitting calibrate otherwise you'll get wrong calibration parameters.

Well. Peak was actually the

Submitted by cin0s (not verified) on Mon, 2012-11-12 17:05.

Well. Peak was actually the wrong word.
I simply can't make a complete cricle. As you can see on the magnetometer graph I truned the Sensore Stick 360° on all three axis. But the acc values just stick on a certain value...

Would you mind trying using

Submitted by fabio on Mon, 2012-11-12 18:26.

Would you mind trying using different code for the ADXL345 than the one on the FreeIMU library. It looks very strange that effect and I doubt this is caused by the sensors.. more likely some sign error somewhere in the code..

Please give a try to one of the ADXL345 libraries available for Arduino, eg:
http://code.google.com/p/adxl345-arduino/

Unfortunately I don't have an ADXL345 at hand at the moment and I can't do the tests myself.

Ok. I haven't tryed another

Submitted by cin0s (not verified) on Tue, 2012-11-13 20:18.

Ok. I haven't tryed another lib yet, but I set the sensore range to 4g (instead of 2g default I think) and it seems to work for me. I still have got a lot of drift but I think I just have to be more accurate while turning the Sensore Stick arround in the calibration process...

Please, it would help if you

Submitted by fabio on Tue, 2012-11-13 22:55.

Please, it would help if you could be sure on what was wrong and report it so I can fix it.. thanks!

I'm pretty sure now that I

Submitted by cin0s (not verified) on Wed, 2012-11-14 15:46.

I'm pretty sure now that I know how to fix this problem.
You just have to add one line of code in the FreeIMU.cpp:
acc.setRangeSetting(4);
You should write it right after acc.init(acc_addr); arround line 125.
This work for me and I believe it should work for everyone else as well..

This fixed the drift that you

Submitted by Manor (not verified) on Wed, 2012-11-14 17:25.

This fixed the drift that you were seeing or it fixed the tilt that you were expecting and weren't seeing?

-Manor

I actually don't know. I'm

Submitted by cin0s (not verified) on Wed, 2012-11-14 17:52.

I actually don't know. I'm sure that it fixed the clipping, but I can't say so much about the drift. I believe it got better, but it is still very unstable for me.
This could by caused by me (perhaps I'm to shaky...). I get often very very different calibration values.

For example:
acc_scale_y = 145.226276 vs acc_scale_y = 5.044392
magn_scale_x = 407.442626 vs magn_scale_x = 165.420249

(left and right values are form the same calib process). I sometimes get "nan" as a calibration value. What does that actually mean?

Ok. My results get better and

Submitted by cin0s (not verified) on Wed, 2012-11-14 22:32.

Ok. My results get better and better now. The calibartion values are now everytime almost the same, but I got still drift (way better than without calibration, but not as good as shown by Fabio in his youtube videos).

@Fabio: I don't know how your calibration algorythm works, but I found a very simple to do calibration method. You just have to get the max/min values for each axis and not a whole circle. It works like this:
ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
I don't know exactly what GRAVITY should be, but in the source I found it's also used in the DCM filter.
I also don't know if this is usefull for your implemantation or not and if the results are good, but I remeber a few weeks ago I got nearly drift free results with the Razor AHRS project(https://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs/wiki/Tutorial) which uses this kind of calibration.
I don't use Razor AHRS anymore because the implementation looks not so good for me and it's realy slow (>20ms). Your project is way better in this aspects :) Another point is that Razor AHRS is not implementet as a library. I tryed to rewrite (actually copy&paste) it to a library, but it didn't worked so well.
So I hope I will gain success with your nice project...

Connecting scheme?

Submitted by Ivan (not verified) on Wed, 2012-11-14 18:12.

Please describe your scheme connecting the IMU to the Arduino. Which voltage for the IMU? I use the same IMU and I do not such errors.

9DOF Sensore Stick <->

Submitted by cin0s (not verified) on Wed, 2012-11-14 18:15.

9DOF Sensore Stick <-> Arduino (Mega)
SCL <-> SCL (21)
SDA <-> SDA (20)
GND <-> GND
VCC <-> 3.3V

Should be fine...

I am not sure how you are

Submitted by Manor (not verified) on Wed, 2012-11-14 18:08.

I am not sure how you are calibrating it. I wrote my own calibration program on MATLAB and was able to achieve about the same results as Fabio's calibration GUI. If I were to use the same data for both I presume that I would get the same calibration parameters.

Here is a picture of what achieved

Right one is after calibration. left is raw data. These are the X-Y points of the magnetometer.

-Manor

I tryed Fabios Python

Submitted by cin0s (not verified) on Wed, 2012-11-14 18:15.

I tryed Fabios Python calibration GUI. Could you send me your Mathlab program (mail [at] cin0s [dot] net)? That would be awesome :)

Some questions on your

Submitted by Al Testani (not verified) on Wed, 2012-11-14 18:14.

Some questions on your calibration procedure:

1) I assume you do the same for accelerometers?
2) Specifically how do you move the board during the calibration?
3) Would the MatLab script run in Octave?
4) If yes to #3, would you make you procedure and program available?

Thanks!

Calibration Video?

Submitted by Al Testani (not verified) on Mon, 2012-11-12 14:32.

Fabio,

When may we have the video of the calibration procedure? I too am having a hell of a time getting a good calibration.

Despite numerous calibration runs I am still seeing a large amount of Yaw drift. In fact, to determine if the gyro scale calibration (and offset) were impacting this, I temporarily modified the code to disable the gyros completely. I did this by forcing the gyro readings (and offsets) to zero. This had a minimal effect on the drift.

Thanks

Al

PS Please see the DIY Drones forum as I asked two questions your help on would be appreciated.

Video Calibration

Submitted by Manor (not verified) on Thu, 2012-11-15 05:28.

I had time today to throw together a video of how I calibrated my sensor. (Sparkfun 10724)

Video

And this is whats in the box :

Image

Good luck,
-Manor

Drift after calibration

Submitted by Al Testani (not verified) on Thu, 2012-11-15 17:09.

The key question is whether a good calibration eliminates or effectively minimizes drift. Can anyone report "drift-free" operation? If so, what IMU board are you using?

Fabio made a video of performance of a properly calibrated FreeIMU board and there was residual yaw drift. People who are running FreeIMU on alternate hardware platforms (e.g. ArduIMU, Sparkfun, etc.) seem to be having much larger drifts. Some of these have been improved by adjusting the signs of the magnetometer readings passed to FreeIMU::AHRSupdate(). Gyro and accelerometer signs are the same for all IMU boards. Has anyone adjusted these and fixed the drift?

In terms of gyros, there is only a start-up calibration of gyro offsets but no calibration of gyro scale factors. Instead of building a rate table to calibrate gyro scale factors, I did a test where I modified the code to provide fixed zero gyro readings (simulating a tilt-compensated compass) and the yaw drift was essentially unchanged. Assuming this is valid math, it indicates the gyros are not the major source of the drift.

Other data I have collected in trying to solve this problem are:

- In FreeIMU::zeroGyro the start-up delay for the MPU60x0 is too short as the datasheet states it is 30ms typical. I changed this delay to 100mSec but it had no effect since my board is continually powered up but could possibly cause problems in other applications.

- There is no temperature compensation for any of the sensors but I do not believe this is an issue regarding drift (at least on the bench) since, again, my board (ArduIMU) is continually powered up and should be at thermal equilibrium.

- If the proportional and integral gains (Kp and Ki) are zeroed, drift is significantly reduced which I assume is expected but I do not understand the algorithm or math behind it to use this information to track down the problem or understand how to tune these parameters for my application.

Any help on this would be appreciated. Thanks

Hi Manor, thanks for sharing

Submitted by fabio on Thu, 2012-11-15 09:31.

Hi Manor, thanks for sharing your video. However, this is still not a proper calibration approach.

You are not making a circle on the acc XY plane and you are not making the biggest circles on the magnetometer. Both these are crucial to get better results.

This is what you have in your XY plane for the accelerometer:

This is wrong. You did not made complete circle on this one. The green line is what it should look like for proper calibration.

This is what you have for the XY plane for the magnetomter. This is also wrong.. See those areas marked in red by myself? These are synthoms of the fact that you didn't make the biggest circle possible... the green line shows you how this should look like.

Rotation XY plane

Submitted by Sjef (not verified) on Thu, 2012-11-15 09:45.

Hello Manor,

In addition to what Fabio said:

You have to rotate around the 4 sides of the box too.
In a way that each side of the box has touched the "ground".
You get the biggest circle in the XY plane when you do this.

The last movement you did in the video (movement of rotating the bottom of the IMU box on the "ground") isn't necesarry.

Hope this helps,
Sjef

Thank you for the video

Submitted by cin0s (not verified) on Thu, 2012-11-15 06:57.

Thank you for the video :)
I'll build such a box today and look what my results looks like...

I understand the

Submitted by fabio on Mon, 2012-11-12 18:48.

I understand the difficulties.. unfortunately my time is limited by other matters and I don't know when I'll be able to post a video.

I understand I'm becoming a bottleneck in the FreeIMU community growth and I'm evaluating options to better provide tools for the community to grow.. I'm interested in any suggestion. The problem is that very few people have a complete understanding on the math/programming/hardware issues behind a project like FreeIMU so that I'm currently the only one capable of helping people here.. I'm publishing thesis, papers and other documentation but evidently this is still not enough.

Btw, generally speaking, you can't zero out fundamental parts of the algorithm and expect a sound output.. not sure in your specific case. It would help if you could post pictures of your graphs pre and post calibration (the circles drawn during the sampling and those generated after hitting calibrate).

I would be very interested in

Submitted by Al Testani (not verified) on Mon, 2012-11-12 19:18.

I would be very interested in reading any documentation you have published on this subject. Would you provide links, please?

In terms of disabling the gyros, my application is specifically to measure yaw and to do so I need a "tilt-compensated compass". My understanding of these devices is they are the fusion of accelerometers and magnetometers. FreeIMU library runs fine without the gyros and I will probably keep them in the final application assuming a scale factor calibration can be added. Eventually, I hope to end up with a "GPS-aided-tilt-compensated compass". Before I can progress I must determine where the drift is coming from and fix it. Zeroing the gyros was only a test to eliminate the gyros as a major source of the drift.

In terms of suggestions of how to best support the FreeIMU community, one suggestion I have previously made may help. That is to set up a forum specifically for FreeIMU with various forum "boards" on it. This way you only need to support users/developers in one place vs the many places (such as DIY Drones, Sparkfun, multiple blogs etc. etc. etc.). This would also simplify your blog(s)and make it much easier for community members to locate information (it is quite difficult now). You could have boards that were for your posts/announcements, etc. only and others specific to hardware, software, sensors, etc.

Another thing to consider is once the forums are well underway, you could designate "MVPs" such as Microsoft and many other companies do to assist you in responding to questions and problems.

FreeIMU Community website

Submitted by fabio on Wed, 2012-11-21 10:28.

FreeIMU Community website online at http://freeimu.varesano.net/
Hope to see you there.

Forum

Submitted by Anonymous (not verified) on Fri, 2012-11-16 00:48.

Having a forum to discuss this issues would be a great help, I like the idea.

Have a look at my

Submitted by fabio on Tue, 2012-11-13 14:43.

Have a look at my institutional webpage http://www.di.unito.it/~varesano/
Interesting are my MSc thesis and the papers on PALLA.

Thanks for your suggestion, I'm evaluating options.

Oooops...

Submitted by cin0s (not verified) on Sun, 2012-11-11 17:11.

Drift after calibration

Submitted by Manor (not verified) on Wed, 2012-10-24 23:38.

Hello Fabio,
Thank you for the great about of time you put into this project. It is appreciated. I calibrated my sensors and I am still seeing a drift on the Yaw. I was very careful when making the calibration. I then recorded a FreeIMU_calibrated and plotted it. http://i45.tinypic.com/2zss605.png

acc (x,y,z) top
gyro (x,y,z) middle
magn (x,y,z) bottom

I noticed that the z component of the magn is greater then 1. This does not seem right to me. I believe that is the culprit. I have calibrated and achieved the same results. I have done the calibration in the same place that I have made the recording. I am stumped. I am using the sparkfun sensor 10724.

Thank you in advance,

Manor

Yeah, once calibrated and in

Submitted by fabio on Thu, 2012-10-25 10:40.

Yeah, once calibrated and in stationary state the acc and magn values coming from FreeIMU::getValues should be normalized (3D magnitude = 1).

Let me finish my calibration GUI. With that it will be very easy to perform properly the calibration.

Calibration in v18?

Submitted by Al Testani (not verified) on Sat, 2012-10-27 03:57.

I tried to run the calibration in the latest version (#18) by running cal_gui.py and it complains of missing files. Please provide some instructions on how to use your latest calibration stuff! Thanks

The code on the repository is

Submitted by fabio on Sat, 2012-10-27 10:43.

The code on the repository is now 80% done. It calibrates the IMU from FreeIMU_serial sketch and correctly produce a calibration.h file. There are still missing stuff, like 2D and 3D calibration graphs and the code may have still bugs but for now it should work ok.

The new code is a GUI, so in order to run the development version you'll need various python libraries dependences to run it: pyqt4, qt, pyopengl, pyqtgraph.

Once the GUI is completed, I'll release Win, Mac, Linux installer which should make installing the stuff pretty easy.

pyqt4, qt, pyopengl, pyqtgraph Binaries

Submitted by Al Testani (not verified) on Mon, 2012-10-29 02:51.

I have begun preparation for the GUI based calibration by trying to locate binaries for pyqt4, qt, pyopengl, pyqtgraph. Please provide links to the binaries for these libraries as it is looking like we may have to get the appropriate development environment and build one or more ourselves. This will make this process much more tedious than the previous calibration procedure. Please don't give us a procedure that requires us to find and build the required libraries!

Question: is the NumPy version of the calibration functional? I have created a CalAcc.py and equivalent CalMagn.py programs. Using the acc.txt and magn.txt files created by the calibrate.py program (with the Octave stuff commented out) I get calibration factors and can create a calibration.h file from them. Is this valid?

This file, when executed

Submitted by fabio on Mon, 2012-10-29 10:04.

This file, when executed already reads data from acc.txt and magn.txt and output calibration parameters. It only requires Numpy as dependency.

Once the code is finished, I'll be releasing installers for the Calibration GUI. You won't need to compile anything nor install strange libraries.

Unfortunately, at development stage you do need developemnt libraries on your system which may be difficult depending on the system you are using. I'm using Linux and on it obtaining a functional PyQt/Numpy environment is a matter of minutes.

That works but a question

Submitted by Al Testani (not verified) on Wed, 2012-10-24 23:08.

That works to get calibration factors but I have a question. In the ArduIMU port of the FreeIMU library I initialized the MPU-6000 as follows:

GyroZ clock
Sample rate = 50Hz
Gyro scale = 2000 dps
LPF = 20Hz
Accel scale = 2g (16384LSB/g)

This is giving me the following calibration factors which look much different from your examples. Also, should the offsets be rounded to an integer for the calibration.h file? Note: the other thing I did was to override the acc and magn calculations in FreeIM so FreeIMU serial was outputting true raw values not those corrected by the example calibration.h file. Is this necessary?

With the values below included in calibration.h, the drift is better but still quite bad.

Acc
>>>
[1066.5463357522274, -349.17800111990823, -3845.8171570792156]
[13688.193312798399, 16222.604236130575, 18588.048602593797]
>>>

Magn
>>>
[6.924602850632791, -49.772170630932848, 2.1547764454005343]
[475.64214751750183, 453.97140787072522, 407.13241325738579]
>>>

Setup Parameters for ArduIMU

Submitted by Al Testani (not verified) on Sat, 2012-10-27 03:52.

These are the correct setup parameters included in the ArduIMU version of FreeIMU:

GyroZ clock
Sample rate = 50Hz
Gyro scale = 2000 dps
LPF = 20Hz
Accel scale = 4g (16384LSB/g)
Magnetometer scale = ± 4.0 Ga= 440 (440LSB/Ga)

Set integral gain to zero (freeimu.h)

Submitted by TimoK (not verified) on Fri, 2012-10-26 13:00.

#define twoKiDef (2.0f * 0.1f) // 2 * integral gain
-->set to zero

I don't know if this your challenge, but for me it worked that I set the integral gain to zero

Each sensor is different, so

Submitted by fabio on Thu, 2012-10-25 10:19.

Each sensor is different, so different calibration parameters are ok. The issue is probably that it's pretty difficult to move the board to see a complete 3D sphere without any visual feedback. That's what I'm currently working on.

In terms of the MPU-6000

Submitted by Al Testani (not verified) on Thu, 2012-10-25 12:40.

In terms of the MPU-6000 setup parameters I listed, are they correct?

It doesn't matter what

Submitted by fabio on Thu, 2012-10-25 14:06.

It doesn't matter what parameters will you use.. as long as you don't change them, it will work fine.

FreeIMU Magnetometer Calibration?

Submitted by Al Testani (not verified) on Wed, 2012-10-24 15:19.

The AccMagnCalib.m Octave procedure included magnetometer calibration as well as accelerometer and output to a calibration.h file. I can see how to manually modify the calibration.h file for the accelerometer offset and scale but what is to be done for the magnetometer calibration?

The acc.txt and magn.txt have

Submitted by fabio on Wed, 2012-10-24 21:15.

The acc.txt and magn.txt have the same syntax. Just edit the code in this page changing acc.txt into magn.txt and rerun it. Save the output values into the config.h file. That's it.

Post new comment

The content of this field is kept private and will not be shown publicly.
If you have a personal or company website insert its address in the form http://www.example.com/ .
  • Web page addresses and e-mail addresses turn into links automatically.
  • Allowed HTML tags: <a> <em> <strong> <cite> <code> <ul> <ol> <li> <dl> <dt> <dd> <pre> <small> <del> <img> <h2> <h3> <h4> <b> <video> <sub> <sup>
  • Lines and paragraphs break automatically.
  • Images can be added to this post.
  • You may use [inline:xx] tags to display uploaded files or images inline.
  • You may insert videos with [video:URL]
  • Each email address will be obfuscated in a human readable fashion or (if JavaScript is enabled) replaced with a spamproof clickable link.

More information about formatting options