A quick round-up first. I have a IMU sensor that houses a 3-axis accelerometer and a 3-axis gyro sensor. Well, actually I got two, one that I built myself and one from Dexter Industries. To use this type of sensor I have written a non-linear complementary filter (NLC-filter) who’s job it is to fuse the output of the two sensors to a drift free low noise attitude signal. The filter tells me the tilt over the x-axis and tilt over the y-axis. It also tells me the heading. The heading signal is not drift-free. The reason for this is that the accelerometer can provide tilt data but it cannot provide heading data. For this you need a compass.

A compass sensor should really be called an magnetometer because it doesn’t give you heading like a sailors compass does. Instead it gives you the force of the magnetic field over three axis. From this one can calculate heading.

It was technically not very difficult to incorporate the magnetometer data into the filter. Although it took me a long time to figure out how to do this. The main thing was to tune the PI-controller for the compass. I’m not finished with this but I can show you some first results of the filter anyway. The measurements were taken with the setup that you see in the picture. The NXT is mounted on a platform some 20 cm above the ground. This is to minimize effects of the steel enforced concrete floor. It can rotate endlessly as it is mounted on a turn table and driven by a NXT motor and a worm wheel. The worm wheel in combination with a regulated motor gives a very stable rotation speed. The sensors are mounted 10 cm from the NXT to prevent disturbances from the NXT. Measurements are sent over bluetooth to the PC in real time.

this first graph was taken while the NXT was stationary. It shows how stable the filter signal is. Note that the scale of the vertical ax is 0.1 degree, the vertical axis ranging from -4 to 0 degrees. The roll and pitch signals are very stable. During the 15 minutes of the test these signals stayed within a bandwidth of 0.4 degrees. This is all that is left of the gyro drift. The yaw signal (that is controlled by the compass) is less stable, it floats between -0.8 and -2.0 degrees. But also in the yaw signal the gyro drift is eliminated to a large extend. I am very, very pleased with these noise levels as the bandwidth of noise from an accelerometer is around 5 degrees.

The second graph compares the output signal from the compass with that of the filter. This time the NXT was spinning around the Z-axis. The graph shows the results of a full spin. . You basically see two descending lines, indicating a constant movement of the sensors. The signal from the compass, in blue, is not exactly over the signal from the filter. There is a slight difference in the heading that these two report. The reason for this is that the setup of the NXT is not exactly level. The compass signal suffers from this. Its line is not really straight but it is like an S-curve due to this. The filter signal on the other hand is tilt compensated and almost straight. The smoother red line also indicates a lower noise level.

This last image shows the effect of a magnetical disturbance on the compass sensor and on the filter. While taking this graph I slowly moved a head phone along side of the sensor.
The compass suffers from this disturbance, at the peak the signal is over 10 degrees of. The filter on the other hand is not affected by the disturbance. It detects the disturbance and ignores the compass signal while it is disturbed. During this time the filter relies solely on the gyro to keep track of heading.

I think the low noise levels of the filter are nice. But the true advantages are in the tilt compensation and the robustness to magnetic disturbances. The filter is equally robust to disturbances in acceleration I think. This however I cannot show you using this setup.

Tuning the filter is difficult though. I have used the NXJChartingLogger that will be part of Lejos 0.9.1 a lot. The filter provides all kinds of output that is displayed real time on the pc. Different parameters are logically grouped into different graphs. The graphs provide information on the state of the filter and thus help in tuning.
I have also included a method to introduce errors in the sensor data. This enables me to examine the performance of the filter.

As a last thing I have made the filter calculate the initial state of the filter. With this the filter settles accurately (<.5degrees) within half a second.

The performance dropped to 97 Hertz with three sensors attached. But I haven't optimized the compass code yet.

In later posts I'll demonstrate the performance of the filter.

What is the effect of this speed improvement you might wonder? Well the main thing is that one iteration of the filter takes less processing time. This leaves more processing time for other processes. Another effect is that the integration error is less. So the filter should be more accurate.

How did I realize the speed improvement? The filter uses vectors and matrices and a lot of lineair algebra. Before I had a 3×3 matrices based on a 2 dimensional array of 3×3 elements and vectors based on one dimensional arrays of 3 elements. I replaced the arrays with variables and I got rid of the loops around the arrays. So what used to be something like this:

For (int i=0;i<3;i++)
val[i]+=x;

Now has become something like this:

val0+=x;
val1+=x;
val2+=x;

It makes the code ugly and a bit harder to maintain. But who cares.

Soon to come. Compass integration in the IMU filter! This makes it a 9DOF filter and will prevent filter drift over the Z-axis.

I’m currently working on my Mars Rotacaster Rover (http://www.rjmcnamara.com/?p=4571), based on the latest NASA Rovers sent to Mars. Currently I’m using a HiTechnic Accelerometer to keep the Chassis Horizontal at all times, independent of the ‘Roker-Bogie’ Suspension.

With access to the Dexter Industries IMU sensor, I could not only control chassis alignment, but also regulate the heading of the robot travels. The drive/power from the wheel motors could be controlled, in order to keep the robots direction of travel constant, even if the wheels on one side are forced to travel further due to uneven terrain.

I choose Ray’s entry for a number of reasons. His idea uses both the accelerometer, to keep level, and gyro, to keep direction. Also, the robot has an innovative suspension system and it does exists already. So there is a good chance that we’ll be seeing the robot delivering a glass of water over rough terrain without spilling any soon. I’m looking forward to the video. Ray, congratulations!

The software contains of drivers for the Dexter Industries IMU sensor, programs to calibrate the sensor and the IMU filter that I wrote about on this blog before. The filter can be used for other IMU sensors as well. Also included is the sample program that utilizes this filter. It is the very same program I used in the video’s published in a previous post. There is no formal support and no warranty. If you are going to use it you will have to rely on your own wit, the javadoc and on this blog.

Download and install

You can download the software from this link. After downloading you will have to extract it keeping the directory structure intact. Make sure yor compiler can find the code.

Using the IMU drivers

The dIMU consists of two sensors, a gyroscope and a accelerometer, each has it’s own driver.  The gyroscope driver is called L3G4200D and the driver for the accelerometer is MMA7455L. (I use the technical names of the sensors a driver names.) On top of these drivers I made  user interfaces that allowes you to examine, configure and calibrate the sensors. The user interfaces are called L3G4200D_E and MMA7455L_E respectively. You can use these programs as driver as well. It gives your programs access to the additional functionality but it needs the NXT screen and might result in larger programs. There is a sample program included that gives access to the user interfaces, it is called testDIMU.

This is how you enable the drivers in your code,

SensorPort.S4.i2cEnable(I2CPort.HIGH_SPEED);
MMA7455L accel = new MMA7455L(SensorPort.S4);
L3G4200D gyro = new L3G4200D(SensorPort.S4);

The first line instructs Lejos to use high speed I2C. The sensors support this.

This is how you get data (rate, acceleration and tilt) from the sensors.

float[] rate = new float[3];
gyro.fetchAllRate(rate);

float[] accel = new float[3];
accel.fetchAllAccel(accel);

float[] tilt = new float[3];
accel.fetchAllTilt(tilt);

As you can see the drivers return data from all three axis is one call. If you just need data from one axis you can get it from the arrays. The order is X, Y, Z. The gyro returns degrees per second by default. The accelerometer returns Milli-G for acceleration and Degrees for tilt, also by default. If you want to use other units in your programs you can change the default values like this.

gyro.setRateUnit(RateUnits.RPS);
accel.setAccelUnit(AccelUnits.MS2);
accel.setTiltUnit(TiltUnits.RADIANS);

This changes the default units to radians per second, meter per second^2 and radians respectively. Other units are available, check the javaDoc. The default unit can be overridden per data request by specifying the desired unit as second parameter in the FetchAll… methods.

Configuring the sensors

The gyro can be configured for dynamic range and sample rate using the setRange and setSampleRate methods. As a rule one should select the lowest value that your application allows for. This gives data of best possible quality.

The accelerometer cannot be configured. I found this  of little use.

Calibrating the sensors

The gyro of my dIMU doesn’t really need calibrating. however there is the possibility to do so. Calibration is started b  calling gyro.calculateOffset(). During calibration the sensor should remain motionless. Calibration settings of the gyro are not stored, so they are lost when your program terminates.  (Storing calibration settings of gyro sensors is of no use as the calibration values depend on environmental factors and are not stable over time.)

The accelerometer needs calibration. The user interface driver provides functionality to calibrate the sensor and to store the calibration settings. The (base) driver looks for stored calibration settings upon initialization and loads these automatically when they are available. Calibration settings of the accelerometer are stable over time so you’ll need to do this just once. Each of the three axes has to be calibrated separately. To calibrate an axis one has to point it straight up first and hold still while the calibration routine collects data samples.  Then the axis has to be pointed straight down and held still for some time. Follow the on screen instructions and do not forget to save the settings after calibration.

Using the IMU filter

The IMU filter can be used with any three-axis gyro and any three-axis accelerometer as long as their drivers implement the RataData and TiltData interfaces. This is how you initialise the filter

NLCFilter attitude = new NLCFilter(gyro, accel);
attitude.start();

The two parameters to the filter constructor are the gyro driver and accelerometer driver. One could leave out the accelerometer, the filter will work but values will drift over time. The second line of code starts the filter. The filter needs 2 to 5 seconds to settle at start up, therefore you need to keep the sensor motionless and more or less level for a few seconds. You can find out if the filter is ready settling with the Initializing() method.

The IMU filter keeps track of the attitude, or pose, of your sensor/robot. You can query the roll, pitch and yaw angles this way

attitude.setTiltUnit(TiltUnits.RADIANS);
float roll=attitude.getRoll();
float pitch=attitude.getPitch();
float yaw=attitude.getYaw();

or

float[] tilt = new float[3];
attitude.fetchAllTilt(tilt);

By default these methods return angles in radians. You can change this by setting a different unit with the setTiltUnit() method.

You can also use the filter to transform data from a body frame to a world frame. This is useful if another sensor returns data that needs to be corrected for the robots current attitude. the next example transforms a distance returned by a UltraSonic sensor to world coordinates. The example assumes the us and IMU sensors are aligned and that the US sensor measures along the X-axis.

Matrix usBody=new Matrix(1,3);
Matrix usWorld=null;
us = new UltrasonicSensor(SensorPort.S1);
usBody.set(0,0,us.getDistance());
usWorld=attitude.transformToWorld(usBody);

The matrix usWorld now contains the distance from sensor to the detected object over the X, Y and Z axis.

Configuring and tuning the IMU filter

By default the IMU filter updates as often as possible. It’s update frequency is over 100 Hertz. To free computer resources one can lower the update frequency by using the setFrequency() method. The filter will try to match the specified frequency. A parameter value of 0 will run the filter at maximum speed.

The filter can be tuned to increase the quality of its output. I advise you not to tune the filter until you are familiar with it and understand its inner workings. Tuning consists of altering the P and I values of it’s internal PI controller. The I value takes care of gyro drift cancellation and the P value controls how fast attitude is corrected by use of tilt data from the accelerometer. The values can be modified by using the setKp() and setKi() methods.

There are two ways the filter can assist you in  tuning. It keeps track of the integral of absolute errors, or absI. This is a measure of the total error of the filter over time. The smaller the error (given a fixed period) the better the filter performs. /* The filter also allows you to send data over bluetooth to the pc for examination. For this one has to use the NXTChartingLogger on the pc that is part of the Lejos distribution. You instruct the filter to send its state to the pc by supplying  a NXTDataLogger object with the setDataLogger() method. */

Running the demo program

The demo program is called testIMU.  At startup of this program the sensor must be horizontal and motionless. The sensor is assumed to be aligned ith the NXT brick with the sensor plug facing to the same direction as the sensor ports. Once you see the wire frame you can start moving the sensor.The demo has four display modes:

• Wire frame. Here it shows a wire frame of the sensor on the NXT screen
• Rotation matrix. The screen will show the content of the rotation matrix. In  this matrix the current attitude is stored. The matrix is also used to convert body coordinates to world coordinates by matrix multiplication..
• Roll, Pitch, Yaw. The screen shows the Roll, Pitch, Yaw angles of the sensor.
• Update speed. The screen shows the actual update speed of the filter.

You can browse through the display modes by using the arrow keys. The enter key resets the filter.  The clip below shows the demo program in action in wire frame mode.

Do you have a nice application for this sensor? Submit your plan in a reply to this blog. The most original or inspiring idea will be rewarded with this advanced sensor. I am to judge the submitted idea’s and to point out the winner. If you like a particular submission very much you can respond to this. You might be able to influence my decision.

Dexter Industries currently has a dIMU week. They to give some IMU’s away. So, you can increase your chance of winning one on their site.

I wrote a small class that can draw a compass rose on the NXT screen. The class contains a collection of points that are the start and end points of the lines that make the compass rose. It has a method to draw these lines. But before drawing the lines they are transformed using the current attitude of the IMU sensor. The transformation is a matrix multiplication of the compass rose definition and the attitude matrix that the IMU filter maintains. This is done in line 4 of the code example below. R is the transformation matrix, rose is the compass definition. The function toScreen on the same line centers the image on the screen.

public void draw(Matrix R) {
 Matrix ss;
 Graphics g=new Graphics();
 ss=toScreen(R.times(rose));
 for (int i=0;i<elements;i++) {
 g.drawLine((int)ss.get(0,i*2),(int)ss.get(1,i*2),(int)ss.get(0,i*2+1),(int)ss.get(1,i*2+1));
 }
}

As you can see the transformation matrix allows you to perform very complex functionality, three rotations, using a very simple function, times().

Below is a video of the application in action. You might want to play this video in HD as it is quite hard to see the details on the NXT screen.

This application just shows a compass rose. But the same technique can be used in a wide variety of applications. One such application could be to transform data from range sensors to world coordinates. This will ease the process of mapping the environment. As the matrix maintained by the IMU filter is 3 dimensional you can even make a 3D map.

Transformation matrices are also very useful for robots with holonomic wheels. Here they can be used to transform robot speed into wheel speed.

Here is another video. This time it features the dIMU from Dexter Industries.

The three accelerometers in the test all all 3-axis digital sensors that support hi-speed I2C. They all allow you to set dynamic range, the MindSensors sensor supports 1.5, 2, 4 and 6G. The dIMU supports 2, 4 and 8G and my cIMU supports 2,4,8 and 16G. On the NXT you would normally use 2 or 4G dynamic range.

I tested the sensors at 6 and 8G. The resolution (the minimum difference between to samples) of the Mindsensors accelerometer is 1 milli-G for all ynamic ranges, the dIMU and the cIMU both have 16 milli-G at 8G. The MindSensors sensor has the best resolution. Later, I’ll explain why.

The test protocol is very simple. The sensors are tested at the same time. First they are horizontal, then they are tilted manually to an angle of 90 degrees and then they are turned back. The test tells something about the accuracy and noise levels of the sensors.

The first graph shows the unprocessed output of the Z-axis of each of the sensors. You would expect a value of 1000 milli-G when the sensor is horizontal and 0 milli-G when tilted. As you can see in the graph the three lines are quite different. The MindSensor gives smooth output with little noise. My sensor is very noisy, the line goes up and down. And the dIMU has some noise. What is more, the three lines are on different heights and none is on the expected level. Apparantly the sensors need calibration, to get the lines on the expected level, and the two IMU’s need some smoothing as well. My IMU allows to set the internal sample rate, lowering this to 100 Hz removed some of the noise. In later tests I used the lower sample rate.

Calibration of accelerometers is quite easy. The MindSensors sensor has an on board calibration routine but I advise you not to use it and to write your own instead (or use the program that MindSensors provide on their website). Calibration consists of determining an offset and a scale factor. The offset is a value that you substract from the raw value. It is equal to the value the sensor returns when it experiences zero G. For the MindSensors sensor used in the test this is about 125 milli-G. For the other two sensors it is a lot harder to find the offset value as the returned values are not stable at all. One has to take the average of multiple measurements to find the offset. Offset values of accelerometers are quite stable, once you found it you can keep using it. But keep in mind that each of the axes has its unique offset value.Then there is scale. After an offset correction has been applied to the signal one gets a zero at zero gravity conditions. This does not mean that one gets 1000 Milli-G under earth’s gravity. Normally this is a few percent off. A scale factor can correct for this. The scale factor equals 1000 divided by the offset-corrected-value the sensor returns under earth’s gravity. The formula to get a calibrated acceleration signal is:
value=(raw_value-offset) * scale
That’s all there is to it. Scale too is quite stable. You can hard code it in your programs once you calculated it. You’ll have to calculate it for each of the axes though. The next graph shows calibrated output of the three sensors.
Apart from the noise levels, all sensors perform very well after calibration.

The MindSensors sensor has a lower noise level and a better resolution than the other two sensors. This clearly is a big advantage of this sensor. But how come this sensor is so good? I suspect it is not the sensor itself but some on board filter that stabilizes the signal. Therefore I decided to write a filter for the other two sensors as well. At first I tried a low pass filter as this is very easy to program and uses little memory, but this filter added a delay in the response, so I abandoned this idea. Then I tried a technique called over-sampling. Basically this means that one tries to get measurements as often as the sensor allows and then calculate a moving average from it. This did the trick, the noise levels of both IMU sensors dropped dramatically and the resolution rose. Both IMU still are more noisy that the MindSensors accelerometer, the difference however is acceptable.

Of course a filter like this does come with some extra costs. One has to program it, It will have to run in a separate task, so that your program can still do the other useful things it is meant to do. And the filter consumes processor time.

The test I perormed was limited and not scientifically sound. However I conclude that all sensors perform well under test conditions. The MindSensors accelerometer is the best accelerometer for most of us as its signals are most easy to use. It does not have a gyro sensor on board as IMU sensors do. So if you also want a 3-axis gyro then you should consider an IMU sensor. The price dIMU is just 15$ more than the MindSensors accelerometer. This could be a good reason to go for the IMU straight away.

Some time ago I asked you to come up with a name of my omniwheel robot. Now, It is time to announce the winner.

the name of the robot is Koios, after one of the Greek Titans. Stephen, aka PigsCanFly, suggested the name Coeus, translated into Dutch gives Koios. So Stephen is the winner.

The prize, A second hand Hitechnic gyro, will be sent by airpig to Stephen. Congratulations.

Thanks to everybody that participated.