Archives for category: IMU sensor

This time I’ll show you the first results of incorporating a 3-axis magnetometer (or compass) into my IMU filter.

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.


I have included a compass into my INS filter. It now is drift-free and accurate over all three axis. It also is very robust to magnetic disturbances. The compass itself is not. It gives me great pleasure to see the first results.

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.

I am proud to announce the winner of the dIMU giveaway. And the winner is Ray McNamara with his Mars Rotacaster rover. This is what Ray is going to do with the sensor.

I’m currently working on my Mars Rotacaster Rover (, 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!

By request I will publish the Lejos software for the dIMU sensor.

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,

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];

float[] accel = new float[3];

float[] tilt = new float[3];

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.


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);

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

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


float[] tilt = new float[3];

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);

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.

It is time for another giveaway. This time you can win an IMU sensor from Dexter Industries. It is the same sensor DI sent me for evaluation purposes. I got it for free, and now you can! It probably is the best tested sensor available.


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.

Some time ago I wrote about transformation matrices. This time I want to show you how to use it. You’ll see how much you can benefit from it.

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();
 for (int i=0;i<elements;i++) {

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.

Last time I reviewed the gyro of Dexter Industries IMU sensor. This time I’ll take a look at the accelerometer of the IMU. I’ll compare this one to the MindSensors accelerometer and to the one on my custom IMU.

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.

If you visited this blog before you might know that I built myself an IMU sensor last year and that I wrote a filter to utilize this sensor to the max. It seems that if you want an IMU there is an easy way.  You can buy one from Dexter Industries.  As a matter of fact, they gave me one to evaluate. And so I did. I will present my findings in this post.

An IMU consists of a rate sensor, more often called a gyro sensor, and an accelerometer. IMU’s are mainly used to determine  position and attitude of a robot. However, an IMU cannot measure this right away, instead it measures movement. The gyro measures rotational movement, the speed at which the sensor turns. The accelerometer measures acceleration, the change in speed of the sensor. These measurements can be integrated to give position and attitude. However, integration is a tricky process, small errors in the measurements build up to large errors over time. Therefore it is very important that the sensors provide data of good quality.

The Dexter Industries IMU, or dIMU, has a three axis gyroscope (L3G4200D) that includes a temperature sensor. It also has a three axis accelerometer (MMA7455L). The sensors are digital sensors and addressed with the I2C protocol. They allow fast I2C, so if you have an environment that supports high I2C speeds like RobotC, LejoS or NXC, you can query this sensor at very short time intervals (>100Hz). There is a bright blue LED on the sensor that is Dexter Industries trademark. Some parts of the sensor board, including the LED get warm, this makes the temperature sensor useless. The components on the board are not covered. This gives the sensor a high tech look, but you should take care not to damage the components. The gyro and accelerometer are not aligned, the X-axis on the Gyroscope corresponds with the Y-Axis on the accelerometer. One should compensate for this in the software.

Below are the results of some tests I did with the sensor. I compared the sensor to others that I have. I will focus on the gyro first.

The gyro I compared to 2 other gyro’s, the analaogue gyro from HiTechnic and the digital gyro (ITG3200) from my custom build IMU (cIMU). The highTechnic gyro is an analogue one, it measures one axis only. he analogue signal from this sensor has to be translated to a digital signal by the NXT brick. This limits the resolution of this sensor to 1023 raw values or 1 degree a second. The maximum dynamic range of this sensor is about 600 degrees a second. The Dexter Industries Gyro has a resolution of  9 to 70 milldegrees a second and a dynamic range of 250 to 2000 degrees a second. Range and resolution are user selectable, where a higher dynamic range gives a lower resolution. The dynamic range of the ITG3200 form my IMU is 2000 gedrees a second with a resolution of 70 millidegrees a second, comparable to the DI gyro.

Gyro’s have an offset, this is the value they return when being motionless. One has to correct gyro readings with the offset to get the true rate of turn.  The problem with offset is that it changes over time or under different circumstances. Also, sensor signals are noisy, so it is not easy to determine a good offset value.  (see a previous post from me for details).  A faulty offset value or a change in offset over time results in faulty readings. Therefore the first test I performed was to see how stable the offset of the different gyro’s is. Below is a graph of the three gyro’s while being motionless. A perfect gyro would return 0 all the time. But perfect gyro’s do not exist, there is always some noise in the signal, that’s why the lines are not straight. The noise of the HiTechnic gyro looks different than that of the other two. This is because the HiTechnig gyro signal is rounded of to degrees per second while the oher two have a finer resolution.  This makes it hard to compare this signal to the other two. The dIMU is more noisy than the cIMU.
After 60 seconds of continuous measurement one of the NXT motors was powered. This gives a drop in the HiTechnic gyro, it’s offset has changed as a result of powering the motor. It seems as if the gyro has started to rotate very slowly, but it hasn’t. Therefore you need to calibrate the gyro under circumstances comparible to those that the gyro will experience when running. Both digital gyro’s do not suffer from powering the motor. This is a strong argument in favour of digital gyro’s.

The second test looks at the quality of the integrated gyro signal. The integrated signal gives the angle of the sensor (in respect to the starting position).  This is a very important aspect of gyro’s as you will quite often use this quantity in your programs. Integration takes away some noise as this is averaged out. However errors in offset are not biased to zero and these errors will add up over time. Again the sensors were motionless, there is no change in angle so one would expect the lines to stay at zero. This time there are four lines in the graph. The, new, red line is also a signal from the HiTechnic gyro. But this time it’s signal is corrected for offset errors using some smart statistical methods. This functionality is part of Lejos.
The HiTechnic gyro without the dynamic offset correction performs worst in this test. The line quickly deviates from the x-axis and this goes even faster when the motor is started (and the offset changes). With the dynamic offset correction the sensor performs very well,it stays close to zero. However, the algorhythm cannot cope with the change in offset after starting the motor, the line then quickly drops.
My gyro gives a smooth line but the line also drops quite fast.  The smoothness of the line is due to the low noise level, the drop is caused by changes in offset. My gyro defenitly could benefit from a dynamic offset correction.
The gyro from Dexter Industries stays around the x-axis. This means it has a very low change in offset or an internal mechanism for offset correction However in this test this sensor performs clearly the best.

The next graph shows the effect of dynamic offset correction on the digital sensors. My sensor clearly benefits from this. It stays closest to zero of the three sensors. The dIMU does not benefit from dynamic offset correction. As a matter of fact, it’s performance is worse than without it. This makes me believe that there is some internal offset correction in this sensor that interferes with the software offset correction I added. I conclude that you do not want to use dynamic offset correction with this sensor.

This is all for this time. Next time I will dive deeper into the accelerometer of the dIMU. Again I will compare this one to two other accelerometers.




In a previous post I showed how the Filter for the IMU sensor combines both gyro and accelerometer data to get drift-free low noise information about the robots orientation in space. The filter performs a second function, it translates sensor output to a world frame of reference. In this post I’ll explain what I mean by that and how the filters works in this respect.

Body frame of reference versus the world frame of reference

Suppose you are standing upright facing north when somebody next to you asks you to turn right for 180 degrees. After you turned so you’ll be facing south. Clear enough. Now suppose you are lying on the ground face down and head pointing south. If you are asked again to turn right for 180 degrees by the same person you will have to consider two options regarding the meaning of the word right. The first option is that right should be in reference to your body. If you turn like this you’ll end up face up while still pointing north. The second option is to treat right in reference to the person that asked the question. If you turn like that you’ll end up still facing down but with the head pointing south.

The same goes with robots and the sensors they carry. Your robot might have a gyro that indicates that the robot turns clockwise, but if the robot is upside down you’ll see itt turning counter clockwise. This is because the robot and the gyro attached to it have a different reference than you have.
To distinguish between the two references we call the first one the body frame of reference and the second the world frame of reference. The world frame of reference is supposed to be inert, a global frame where everything can relate to. Words like up, down, north and south are related to the world frame of reference. The body frame of reference belongs to the robot and the sensors. When the robot moves its body frame of reference moves along with it. Robot commands like drive forward are always in respect to the body frame of reference. Likewise, data from the sensors are also expressed in the body frame of reference.
In robotics, you’ll frequently have to translate information from one frame of reference into the other. For example, when you calculate the position of the robot (world frame of reference) from data from the tachometer (body frame of reference). The process to translate information from one frame to another is called transformation.


A transformation requires the use of trigonometry. Suppose again that you want to calculate the position of the robot (world frame of reference) from data from the tachometer (body frame of reference). Let us assume that the tachometers indicate the the robot has travelled straight forward for 30 cm. In terms of body frame it is said that it moved over the x-axis with 30 cm and over the y-axis with 0 cm. Suppose the robot was facing east when it travelled and you have called the east to west axis the y-axis as is customary. It is clear then that in the world frame of reference the position of the robot increases by 30 cm along the y-axis and maintained its position along the x-axis. In mathematical terms the transformation from body frame to world frame is:

Xw = Xb * cos(a) - Yb * sin(a)
Yw = Xb * sin(a) + Yb * cos(a)

Where the parameter a stands for the angle between the robot (or better the body frame of reference) and the world. In the above example a=90, making cos(a)=0 and sin(a)=1. as a result the x and y values are swapped in the transformation.
Transforming sensor output to world frame of reference is the second function of the IMU filter. However, It does so in a 3D space and not in a 2D space as the example.

The preferred way to model a transformation mathematically is by using matrices and vectors.

This is the preferred way as it offers a convenient and short way to describe both the transformation and sensor output. Without going in too much detail, the transformation is stored in a transformation matrix of 3 by 3 elements. The sensor output is stored in a vector of 3 elements. To transform a vector from body frame of reference to world frame of reference one has to execute a matrix multiplication on the body vector using the transformation matrix. In mathematical terms:

Vw = Vb X T

Where T stands for transformation matrix. To go from world to body the formula is:

Vb = Vw X  Tinv

Where Tinv represents the inverse matrix of T.

Internally the filter maintains a transformation matrix. The robots current attitude can be calculated from the transformation matrix.

Here a quick post of some graphs that show the effect of the filter I use for my IMU.

You might have seen a previous posts where I show you that my sensor is capable of detecting a grain of sugar under the NXT. If you haven’t seen it, check the post and video here. Few months ago I changed from robotC to Lejos. And now I have rewritten the sensor drivers and the filter in Java.

The Lejos developers are currently working on a very nice logging feature that allows users to display data from the NXT real time on the pc. I got hold of the beta code and tested it without any problems. I used the NXTDataLogger (this is the name of the Java class) to made the intrinsics of the IMU filter visible.

The IMU sensor combines a 3-axis gyroscope and a 3-axis accelerometer. The filter reads the output from both sensors, combines there signals in a smart way and provides stable information about the orientation (yaw, pitch and roll) of the robot.

An accelerometer can provide tilt data (pitch and roll) as can be seen in the first graph. This sensor however has two serious drawbacks. First it is quite noisy, that’s why the graph lines seems so hairy. Second, it does not distinguish tilt from acceleration. So, the output from this sensor can have different causes. This is why you cannot make a balancing robot with just an accelerometer.

A gyroscope is far less noisy, that is why the second graph shows smooth lines. It is also not affected by acceleration. However also this sensor has its particularities. First, it doesn’t measure tilt, but it measures rate of turn. This can only be translated into tilt if the starting condition is known. To get the tilt at any time you have to integrate all the readings and the starting condition. In the process small errors are introduced, in the long run they add up to la large error. The green line in the graph shows this effect. At about 42 seconds the sensor rotates faster than its maximum range (>2000 degrees/second), it then gives faulty readings, these are integrated in the calculated tilt. As a result the line ends higher than it started, even though the sensor was turned back to its original position. The second effect that makes a gyro hard to use in long runs is called drift. Due to temperature changes or voltage drops the base level of a gyroscope changes. This means that after some time the gyro seems to be turning slowly when it is still being stationary. This effect is well visible in the blue line. This goes up slowly. This effect can not be eliminated by calibration, unless you are able to calibrate the sensor along the run.

The filter is like a PI-controller. It uses the data from the gyroscope as the basis of it’s output. The lines in the third graph, that shows the output of the filter, are smooth because of this. It uses the data from the accelerometer to correct the output from any errors that have been build up. But it does so slowly in order to keep the noise out of the output signal. The P-value of the PI controller corrects for errors in the gyro signal. It also makes the filter usefull when starting conditions are unknown. The filter also sums past errors to get an I-value, this value is used to correct for drift in the gyro data. The result is a graph with smooth drift-free lines.

Two final notes. First, the green line (the yaw-line) is not drift-free. This is because an accelerometer cannot provide information of orientation in the XY-plane. You need a compass sensor to provide this information. Second, the range is expressed in cosine of the angle, so 1 corresponds to zero degrees, 0 corresponds to 90 degrees.