Archives for category: Sensors

One of the most fundamental problems in mobile robotics is to know the position of the robot.  The question might seem simple, to get an answer is very difficult.  There are two fundamentally different approaches to answer this question. The first approach uses dynamics to keep track of the robots position in respect to its starting position. Most often this technique uses wheel encoders to keep track of wheel movement. This is translated into a position of the robot. This is known as odometry. The second approach uses external references of which the location is known to calculate the position of the robot. Navigation based on stars is the oldest example of this technique, the GPS system is a recent example. As much as we come to rely on GPS nowadays, it is not very useful for our small indoor robots. Indoors the GPS signal is week and the error of a GPS position is in most cases bigger than the range of our robots.

I created a robot that uses the same principles as the GPS system is based on to localize itself. Instead of GPS satellites I use  blinking LEDs (dLights from Dexter Industries) as beacons. The beacons are detected and identified using a standard NXT light sensor. Below you can see the robot in action. The robot is placed on a random location in a random direction in my room. It then has to find out where it is and then drive home.

The robot locates beacons by evaluating the variance in light level while scanning the horizon. Whenever a spot with large variation in light level is found, the robot stops to find out if this variation comes in a frequency that identifies one of the beacons. If so, the current heading of the robot is stored along with the identity of the beacon. If after a full scan three or more beacons are located then the robot has enough information to estimate its position. It does so by triangulation using a Snellius construction. Lejos software for the NXT has a class, lejos.robotics.localization.BeaconTriangle ,that implements all the difficult calculations. If more than tree beacons are located, the robot estimates its position by averaging the position estimated from all unique combinations three beacons. The result is an improved estimation.


This time I will discuss the gyro sensor (again). What it measures. How to cope with offset and drift. And how to use it.

Gyro sensors are among the most widely used sensors in robots. They are often used in self balancing robots for keeping upright. They are also used  in autonomous robots for keeping track of orientation. But also game controllers might use them to detect angular motion. A gyro sensor measures the angular velocity. In other words they measure how fast the sensor is rotating. This is most often expressed as degrees per second or radians per second. Some gyro sensors measure the angular velocity over one axis, others take measurements over two or three axes.

One should be aware that a  rotating object always rotates over just one axis. This axis however can have any orientation. Compare this to speed. An object can move in just one direction at any given time. This direction however can be any direction in a three dimensional space. Most often we do express the speed of an object over three perpendicular axes. A landing plane might fly at a speed of 600 km/s and descent at a rate of 5 m/sec. Wind might blow it off course at a rate of 0.5 m/sec. But still this plane goes in one direction only. The same is true for  angular velocity, an object rotates just with one speed, but we express its speed over three separate axes.

Let us take a look at the output of a typical gyro sensor. The graph below shows the output of a digital three axis gyro sensor (ITG-3200) over a period of 17 seconds. The X-axis is the time axis, the Y-axis shows the angular velocity expressed in degrees per second. After five seconds from the start of the measurement I rotated the gyro sensor clockwise for about 180 degrees. This took me about 3 seconds. After about 11.5 seconds I rotated the sensor back (counter clockwise).


This graph tells us quite a lot. First, we notice that the sensor is a three axis sensor. It returns three different signals at any given time. Second, we can see that the rotation I made took place of the third axis, this is the Z-axis. It means that I rotated the sensor in the XY-plane. This is true, I had the sensor flat on my desk and I rotated it while keeping it flat. Third, a clockwise rotation is expressed as a negative angular velocity and a counter-clockwise rotation as a positive angular velocity. This is a matter of convention and is called the right-handed orientation system. Fourth, we can see that  when at rest the gyro signal is close to, but not exactly, zero. This is because the sensor is not perfect, it has a bit of an error. We’ll look into this error and ways to compensate for this later. And finaly, we cannot read the change of 180 degrees in orientation from the graph. This is because the sensor does not measure the orientation, instead it measures the angular velocity (speed of rotation). It is however possible to calculate the (change in) orientation from the angular velocity as I’ll explain later.

Offset and drift correction

Let us concentrate on the error in the sensor signal right now. Below is a graph taken over a period of ten minutes while the sensor was at stand still all the time.gyro-2

A perfect sensor would output a rate of velocity of zero for all three axes all the time. Obviously this sensor does not.  The X-axis signal is around 2 instead of zero. This error is called the offset error. Every axis has its own offset error. For the X-asis this is around 2, for the Y-axis it is about 3.3 and for the Z-axis it is about -1.5. It is easy to correct for the offset error once you know how big it is. You just substract the offset error from the sensor signal to get a corrected value. The offset error itself can be calculated by taking the mean of a number of samples, take 100 for example.

The offset itself may seem constant, but in reality it is not. The offset of a sensor  is influenced by several factors and can change over time as a result. This is called sensor drift. One of the biggest factors contributing to sensor drift is temperature. You can notice this when one starts using a sensor. When being used, the temperature of a sensor rises a bit. It gets hotter then the ambient temperature. As a result the offset of the sensor changes. If you need a very good signal you should take this into account and let the sensor warm up before calculating the offset.
Some gyro sensors, like the Hitechnic gyro for example,  are seriously affected by changes in input voltage. As the NXT is battery powered this is a serious problem. Starting the motors of the NXT will result in a power drop and thus in a change in offset. There is a trick to avoid this if you have a sensor mux with an external power supply. Like this one from Mindsensors. In general I advise you to choose another gyro.
Even when temperature and power are constant the offset of a gyro will still vary a bit over time. This variation is called random walk.

There is a very elegant technique to deal with sensor drift. This is to constantly but slowly update the offset error of the sensor. Instead of treating the offset as a constant you  treat it as a value that can change over time. To calculate the offset you now use the moving average of the most recent samples as the offset. Then you always have an up-to-date offset. Calculating the moving average however is CPU intensive and you also need a lot of memory to store the samples. Therefore it is better to use a low-pass filter instead of a moving average. This does not use extra memory and little computing power but the effect is the same.
This technique will work very well when the sensor is at stand still. But will it work well when it is rotating? Any rotation will  influence the calculated offset. So of possible one should pause updating the offset while the sensor rotates? Sometimes another sensor can help to detect a rotation. A compass, an accelerometer or  motor encounters can all be off help.
However, there is also another solution. This one is based on the notion that a rotation in one direction is very often compensated with a rotation in another direction. A balancing robot for example stays upright, so in the long term it does not rotate. It might lean forward and backward for short moments of time. But this forward and backward rotations cancel each other out on the long run. So in the long run the average signal of the sensor equals the offset of the sensor, even when there are rotations from time to time. This means that even under dynamic circumstances one can constantly update the sensor offset. One only has to make sure to use enough samples so that there is enough time for rotations to cancel each other out. This technique is useful for balancing robots where rotations are short. It is less useful for slow turning robots where rotations have a long duration.

Converting angular velocity to direction

There are situations where one needs to know the direction of the sensor. In navigation for example one needs to know in what direction a robot is heading. A gyro can provide this kind of information. There are some limitations though. So how do you transform angular velocity into direction? This is done by integration. This might seem difficult. But it really is not. If a robot rotates for 2 seconds at a speed of 90 degrees per second, it has rotated for 180 degrees. So integration is nothing more than time multiplied with speed. The graph below shows both the (offset corrected) angular velocity and the direction. During the test I rotated the sensor four times with 90 degrees clockwise, 360 degrees counter clockwise, 90 degrees counter clockwise and 90 degrees clockwise. After this the sensor was back at its start direction.

gyro-3The  line representing the direction, labeled Rotation 2, clearly shows the steps of 90 degrees that I made. Careful examination of the data shows that according to the sensor the robot rotated with -358.8 degrees at max, where as I tried to rotate it with -360 degrees. This makes the sensor, and the integration, pretty accurate. However, after turning the sensor back to its start direction the calculated direction is not zero as is to be expected. Instead, it is about 5.7 degrees. This is not so good. What makes it even worse, there is no way to correct this. At least, not without the aid of another sensor of user intervantion. This is the main drawback of integration. Over time small errors (in the offset corrected) signal build up to become a large error in the integrated data.

But integration can be very useful nevertheless. Suppose a robot that needs to make exact turns. Using integration you can do so. But you need to reset the initial direction to zero just before making the term. This way the error in each turn will only be equal to the integration error that built up during this turn. This is small as making a turn does not last that long. In other words, you can make your individual turns accurate but not the overall direction of the robot.

But wait, there is one more thing. Integration only gives a change in direction. To know the real direction one should also know the direction before the changes took place. Quite often this initial direction can be assumed to be zero. But this is arbitrary and does not relate to the world. Although it might be all you need. If you need to relate the orientation of your sensor to the real world you need to align the sensor with the real world (make it point north) or you need another sensor that can do this for you. This could be a compass sensor.

This weekend I was wondering if it is possible to use triangulation to pinpoint the position of an object using two ultrasonic sensors. So I made a simple robot and wrote a small program to find out. It proves this can be done.

Triangulation is a technique that uses angles or distances between points to calculate the position of these points. GPS localization for example uses triangulation. The technique is based on the law of cosines. This law allows you to calculate the angles of a triangle given the lengths of its sides. But also to calculate the length of one side given the length of the two other sides and the angle of the opposite corner. This might sound complicated, but they teach it in high school, so you might know it.

This means that a robot equipped with two US sensors can calculate the exact position of an object, provided it is in sight of these two sensors. Potentially, this could be a huge benefit. Because a single sensor has a wide (30 degrees) detection beam, giving much uncertainty about the exact location of an object. But a single sensor also deforms the apparent shape of an object. Practically this means that constructing maps using a single US sensor is impossible. Maybe, with two sensors and some triangulation a map can be constructed, as the position of the objects can be calculated.

Here is a picture of the setup I used to test if it is practically possible to do this with two Lego US sensors. The sensors can be rotated as they are mounted on the shaft of a NXT motor. The sensors are constantly pointing to the object in sight. If no object is in sight the sensors point straight ahead, waiting for an object. It is clear to see from the rotation of the sensors where they are “looking” at. The effectiveness of the setup is clear to everyone.

After some tests I am pleased with the effectiveness of this setup. It is accurate up to 2-4 cm. If this is enough to produce good maps I don’t know. But it is at least effective for a whole range of other tasks. I’m thinking of shooting cannons, search missions and object avoidance. The video gives you an impression.

The effective range of the setup is limited as the picture to the right shows. I put a piece of Lego on every spot I got a successful hit. (The eleven hole beam is there for reference). It is not as wide or long as you might think. I think there are two reasons for that. the main reason is because of the low resolution of the sensor,  just one cm. This causes numerical errors whose effects are  small along the forward pointing axis, but that get larger to the sides. Offset error of the sensors is the second cause that limit the sideways effectiveness. Scale errors of the sensors limit the length of the effective range.

You might wonder how good a rigid setup works. Then the sensors are not allowed to rotate and home in on the object. Well, obviously the field of view is much smaller, and the sensors should be placed facing inward to maximize field of view. But I noticed that the result is less accurate and stable.

The program I wrote is very simple. It is just these two forms of the law of cosines embedded in some control structures.

package lejos.nxt.sensor.example;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.SensorPort;
import lejos.nxt.sensor.filter.AdditionFilter;
import lejos.nxt.sensor.sensor.LcUltrasonic;
import lejos.util.Delay;

public class TwoUSSensors {

	static final int P=5;							// P component of the P-controller
	static final double MARGIN=0.8;		// Sets the margin of max distance for two measurements to assume a single object in sight
	static final float MAXRANGE=150;	// sets the macimum range of the sensor
	static final int MINANGLE=25;			// sets the minimum angle a sensor can rotate to(o means the two sensors are facing each other
	static final double b=20.5;				// sets the distance between the two sensors, measured from shaft to shaft
	static final float SENSOROFFSET=2.5f;

	static final LcUltrasonic sensorA=new LcUltrasonic(SensorPort.S1);
	static final LcUltrasonic sensorC=new LcUltrasonic(SensorPort.S4);
	static final AdditionFilter corrrectedA=new AdditionFilter(sensorA,SENSOROFFSET);  // to correct offset from shaft
	static final AdditionFilter corrrectedC=new AdditionFilter(sensorC,SENSOROFFSET);

	static final NXTRegulatedMotor C=Motor.C;
	static final NXTRegulatedMotor A=Motor.A;

	public static void main(String[] args) {

		Object run=new TwoUSSensors();


	public TwoUSSensors() {
		double targetC=0, targetA=0;
		double  a,c;

		C.rotateTo(90, true);
		A.rotateTo(90, false);

		while(!Button.ESCAPE.isDown()) {


			LCD.drawString("       A     C", 0, 0);
			LCD.drawString("dist", 0, 1);
			LCD.drawString("dist'", 0, 2);
			LCD.drawString("target", 0, 3);
			LCD.drawString("error", 0, 4);
			LCD.drawString("state", 0, 5);

			LCD.drawInt((int) c, 7, 1);
			LCD.drawInt((int) a, 12, 1);

			if (a>MAXRANGE && c> MAXRANGE) {
				// it is assumed that there are no objects in sight
				 targetA = Math.PI/2;
				 targetC = Math.PI/2;
					LCD.drawString("-", 7, 5);
					LCD.drawString("-", 12, 5);
			else {
				// it is assumed there are objects in sight
				if (Math.abs(a-c)>b*MARGIN) {
					// it is assumed that there are two different objects in sight
					if (a<c) {
						// it is assumed there is an object in front of sensorC
						LCD.drawString("-", 7, 5);
						LCD.drawString("+", 12, 5);
						targetC =Math.toRadians(C.getPosition());
						c=Math.sqrt(a*a+b*b-2*a*b*Math.cos(targetC)); // the distance between sensorA and object;
						targetA =	Math.acos((a*a-b*b-c*c)/(-2*c*b));
					else {
						// it is assumed there is an object in front of sensorA
						LCD.drawString("+", 7, 5);
						LCD.drawString("-", 12, 5);
						targetA =Math.toRadians(A.getPosition());
						a=Math.sqrt(b*b+c*c-2*b*c*Math.cos(targetA)); // the distance between sensorA and object;
						targetC =Math.acos((c*c-a*a-b*b)/(-2*a*b));
					LCD.drawInt((int) c, 7, 2);
					LCD.drawInt((int) a, 12, 2);
				else {
					LCD.drawString("+", 7, 5);
					LCD.drawString("+", 12, 5);
					// it is assumed that there is one object in sight
					targetC =Math.acos((c*c-a*a-b*b)/(-2*a*b));
					targetA =Math.acos((a*a-b*b-c*c)/(-2*c*b));

			LCD.drawInt((int) Math.toDegrees(targetA),7,3);
			LCD.drawInt((int) Math.toDegrees(targetC),12,3);

			// set speed
			LCD.drawInt(rotateTo(A, targetA),7,4);
			LCD.drawInt(rotateTo(C, targetC),12,4);

		// return to start position
		C.rotateTo(0, true);
		A.rotateTo(0, false);

int rotateTo(NXTRegulatedMotor motor,double target){
	int targetDeg=(int) Math.toDegrees(target);

	if (targetDeg<MINANGLE) target=MINANGLE;
	int error=targetDeg-motor.getPosition();
	if (Math.abs(error)<=1) return error;
	motor.rotateTo(targetDeg, true);
	return error;


Currently I am developing a new sensor that will be on the market this summer. Although I do not want to give away too much details I would like to share some insight in the development process.

Few months ago I got this idea for a new sensor. Something unlike all the sensors that are available today. I think you’ll want it when you see it. I discussed this idea with John from Dexter Industries and we decided to develop the sensor together. As I had a pretty good idea of the functionality of this sensor we could start selecting parts right away. Once we got the part list together John started drawing a schematic for it. From this we ordered some prototype boards. John soldered all the parts on it. I’m still amazed how he did that. Man, these parts are small!

A prototype serves as a proof of concept and also to find any design errors. There are always design errors. In this case we forgot to wire the power line. As a result the prototype was dead. Luckily John managed to add the wiring for the power line and the sensor came to life. It immediately was clear to us that the sensor was what we expected it to be. I wrote a driver for it and started testing. How very nice! My 10 year old son liked it very much, he even built a test device for me.

There are some other small errors on the prototype that are being corrected at the moment. When these are fixed we can start thinking of a first production run. Once this is on its way I will show the sensor to you and organize another giveaway 🙂

I’m pretty sure you’ll like our sensor, so stay tuned.


It is time to announce the winner of the dCompass from Dexter Industries. It is Brian L from Livingston, NJ. He is using IMU’s on holonomic NXTs. No wonder he found my blog! I think the compass is in good hands with Brian.
For the others. Stay tuned, there might be future giveaways. There surely will be interesting posts! Thanks for participating.

Few weeks ago Dexter Industries sent me a prototype of the dCompass. I was really pleased with the sensor as this compass proved to be a really good one. It was a nice addition to my IMU.

But a compass sensor can be used for other things than using it as a compass. With a little imagination you can make very cool robots with it. In this post I’ll give a few suggestions. Also, you have the chance to win one.

Using the dCompass as a compass is the obvious thing to do. Still I want to give you some hints regarding this.
The compass measures the magnetic field over three axes. The direction of the sensor is calculated from the strength of the magnetic field over the X and Y axis using the atan2 function. Luckily, most drivers will do this for you. When using the dCompass as a compass you’ll have to make sure it is level and kept away from magnetic disturbances. Keep it 10 to 15 cm from the NXT brick and (heart of) the motors for best results.

The fact that the dCompass detects all magnetic fields and not just the earth magnetic field is the basis for many other uses of this sensor. But before I dive into this some more about the earth magnetic field. One thing that might surprise you is that the magnetic north is not somewhere on the ground. If you live on the northern hemisphere it is somewhere underground. If you live on the southern hemisphere it is somewhere in the sky! The earth magnetic field does not change. It always points at the same direction and it always is of the same strength. Only when you go to another place it’s direction and strength changes. Just as you can calculate the direction of the magnetic field you can calculate its strength (emf). Pythagoras tells us how. emf=sqrt(x^2 + y^2 + z^2)
Once you know the strength of the earth magnetic field you can use this to calculate the strength of other magnetic fields (omf) near the compass. This of course is the difference between the measured field strength and the earth magnetic field.
omf=sqrt(x^2 + y^2 + z^2) - emf
The resulting value is the gateway to other uses of the dCompass. If omf equals zero there are no disturbances around. Any other value is an indication of something magnetic in the vicinity.

So what exactly can you do with this information? Here are some ideas:

  • make a traffic counter that counts the number of vehicles that pass by. You might even be able to deduce the kind of vehicle from the field strength as well as its direction.
  • make an magnetic key with a motorized lock that only opens when the right field strength is measured.
  • make a rotation counter that counts the number of revolutions of something turning.
  • you can expand the previous idea to make something that shows you how fast your bike is going.
  • make a device that tells you where in the wall the electric wires run.

Well, these are just a few ideas to trigger your imagination.

If you have a good idea you can find more info in the compass or buy one at Dexter Industries. Or, if you are really lucky, you can win one here. All you have to do is to subscribe to my blog. I will randomly draw a winner from all my subscribers on 15 march 2012.

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.