Hi,
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:
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.
Last post I showed that Sidbot is capable of attitude calculation. It is however prone to drift. After a while the attitude calculated by the NXT drifts from its real attitude. There are two factors contributing to this. The first is offset drift in the gyro. I dealt with this, this post describes how. The second factor is random errors. I’ll deal with that later.
The offset of a gyro is the value it returns when being inert. The offset has to be distracted from the gyro readings to get the rate of turn. It is however not a constant, it’s value changes over time. One has to find a way to update its value, even when the sensor is not inert. You can do so by first recognizing that in the long run the robot is standing still (on average). If you average the readings over some time then the result is equal to the offset. This is exactly how you update the offset, you calculate the average reading over some time. In code it could look like this
Offset=((N-1)*offset)+gyroValue)/N
Where N represents an amount of time.
N is subject to tweaking. If its value is to low the offset will not be stable as it has a lot of “movement” in it. When it is too high the calculated offset cannot keep track of real offset changes. It then has too much “history” in it. I used Excel to experiment with different values of N. To do so I had to export raw gyro readings to excel and add the formula above. For the moment I have chosen a value of 200 while the sample rate is 50 hertz. To make the effect of the formula constant with different sample rates I define N as 4*sample_rate.
This post presents the robotC code for the Kalman filter.
Now the Kalman filter is working I made the code suitable for use in robotC progams. The code can be freely used by you. Download the code
To use the filter include heading.c into your program. This will create a global variable called heading that contains the filtered heading. The variable is updated every 20 mSec.
When you include the code into your program a task will be started automaticly once your program starts. This task will first search for both a compass and a gyro sensor. Once they are found the filter starts calculating the value of heading. The program can find the sensors as long as they are defined with robotC’s motors and sensors setup.
Check the readme and example program called testHeading.c for additional information.
The code probably will not work when the sensors are connected through the sensor multiplexer becuse the function that searches for the sensors only scans the first four sensor ports. But in case you have a multiplexer it should be easy to modify the program.
Initially I used vector math to implement the filter in robotC. For this version however I got rid of vector math and switched to normal type of (calculus) functions. This makes the code (and the filter) so much more easy to understand.
Here is the code, in case you just want to look at it.
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*************************************************************************************************\
* *
* PROGRAM: Heading *
* VERSION: 0 *
* PURPOSE: This program integrates gyro and compass readings using a Kalman filter to get a more *
* accurate value for heading *
* AUTHOR: Aswin Bouwmeester (aswin.bouwmeester@gmail.com) *
* DATE: okt 10, 2010 *
* *
\*************************************************************************************************/
/*
variance for the gyro sensor for different sample sizes
N T var
1 5 0,222
2 10 0,144
3 15 0,123
4 20 0,119
5 25 0,114
6 30 0,088
8 40 0,093
10 50 0,078
*/
#pragma systemFile
#pragma autoStartTasks
// MOUNTING can be set to - if the compass or the gyro is mouned upside down
#ifndef MOUNTING
#define MOUNTING
#endif
float meanSensorValue(tSensors s, int interval, int N);
tSensors findSensor(TSensorTypes type);
float heading=-1;
task getheading()
{
float
var_compass = 0.9,
var_gyro=0.119,
var_filter_predicted,
var_filter_updated,
kalman_gain,
compass_measured,
compass_predicted,
compass_updated,
gyro_measured,
time_span,
offset;
;
long
time_start,
time_end
;
bool
disturbed
;
tSensors
_compass,
_gyro
;
// find the compass and gyro sensors;
_compass=findSensor(sensorI2CMindsensorsCompass);
if (_compass==S5)
_compass=findSensor(sensorI2CHiTechnicCompass);
_gyro=findSensor(sensorI2CHiTechnicGyro);
// Display error message if the sensors are not found
if (_compass==S5) nxtDisplayTextLine(1,"No compass found");
if (_gyro==S5) nxtDisplayTextLine(2,"No _gyro found");
if (_compass==S5 || _gyro==S5)
{
PlaySound(soundException);
while(nNxtButtonPressed==-1);
}
else
{
// get the gyro offset
offset=meanSensorValue(_gyro,5,100);
// initialise the filter;
compass_updated=SensorValue(_compass);
var_filter_updated=0;
// Run the filter forever;
while (true)
{
// get time span;
time_end=nPgmTime;
time_span=((float)(time_end-time_start))/1000.0;
if (time_span<=0) time_span=0.02; // this is to compensate for wrapping around the nPgmtime variable;
time_start=nPgmTime;
// get measurements from sensors
// (when changing the sample size of the gyro, one must also change the variance)
compass_measured=(float)SensorValue(_compass);
gyro_measured=MOUNTING (meanSensorValue(_gyro,5,4)-offset);
// predict;
compass_predicted=compass_updated+time_span*gyro_measured;
var_filter_predicted=var_filter_updated+var_gyro;
// heading must be between 0 and 359
if (compass_predicted<0) compass_predicted+=360;
if (compass_predicted>=360) compass_predicted-=360;
// Detect _compass disturbance;
if (abs(compass_predicted-compass_measured)> 2 * sqrt(var_filter_predicted))
disturbed=true;
else
disturbed=false;
// get Kalman gain;
if (disturbed)
kalman_gain=0;
else
kalman_gain=var_filter_predicted/(var_filter_predicted+var_compass);
// update;
compass_updated=compass_predicted+kalman_gain*(compass_measured-compass_predicted);
var_filter_updated=var_filter_predicted+kalman_gain*(var_compass-var_filter_predicted);
// make result available gobally
heading=compass_updated;
// display informatin about filter
#ifdef HEADING_DISPLAY
nxtDisplayTextLine(0,"Heading filter");
nxtDisplayTextLine(1,"Heading : %3.0f",compass_updated);
nxtDisplayTextLine(2,"Compass : %3.0f",compass_measured);
nxtDisplayTextLine(3,"Variance : %6f",var_filter_updated);
nxtDisplayTextLine(4,"Disturbed: %1d ",disturbed);
#endif
// wait for next iteration;
wait1Msec(0);
}
}
}
// function used to automaticly find sensor out in what port a sensor is attached (returns S5 when sensor is not found)
tSensors findSensor(TSensorTypes type)
{
tSensors port;
for (port=S1;port<=S4;port++)
if (SensorType[port]==type) return port;
return S5;
}
// get a mean sensor value from specified number of samples
float meanSensorValue(tSensors s, int interval, int N)
{
float som=0;
for(int i=0;i<N;i++)
{
som+=SensorValue[s];
wait1Msec(interval);
}
som=som/N;
return som;
}
Last post has been months ago. I finally got the courage to continue my work on the Kalman filter. I can report some progress and another, hopefully small, draw back.
Remember that my filter didn’t work because of the compass disturbances made by metal and magnets. Last nights I’ve been working on a solution for this. In essence I want to ignore compass readings when there is a disturbance somewhere. But how do I detect disturbances? A little work I did for Xanders omniwheel robot pointed me in the right direction.
My Kalman filter not only gives predicted values for the heading but it also knows how accurate this prediction is. Knowing these two things one can use statistics to calculate how likely a compass reading fits in the prediction. Let me explain this using an example. Suppose the filter predicts a heading of 90 degrees (West). At the same time it also knows that the true heading is for 95% certain between 87 and 93 degrees. In this case you can trust a compass reading of 92 degrees, but you wouldn’t trust a compass reading of 97 degrees. Based on this technique I added a switch to my filter. If compass readings are to be trusted the filter performs normally. But if it does not trust a compass reading then it doesn’t take this reading into account. It then skips the update phase and continues straight to the next iteration making the next prediction. This seems to work.
Here is the first graph of my filter in action. 
The red and purple lines represents the orientation of my robot. In red is what my compass reports, in purple is what the filter reports. The horizontal axis is the time axis (in msec). Let me explain what is happening in real life and how you can see the effects in the graph.
The robot is standing on an enforced concrete floor with its side against the wall. The iron in the concrete gives nasty disturbances to the compass. Initially the robot is pointing almost east (80 degrees). After some time I lift the robot and turn it half a circle and put it back against the wall with its other side. By using the wall I know that the robot made a turn of 180 degrees. Turning the robot shows up in the graph as rising (or falling) of the red and purple lines. Once the robot starts turning we can see both lines getting away from each other. This is because the compass is affected by the iron and the gyro is not. The filter detects this and start ignoring the compass readings. This is shown by the green line, when it is up the filter ignores the compass. As you can see from the red line the filter is very right to do so. We would expect it to rise to 80+180=260 degrees, but it reports around 240 degrees. This is obviously very wrong. The result of the filter on the other hand seems quite accurate, the purple line rises to the expected value of 260 degrees. Once I turn the robot back the compass readings come back in line with the filter readings. The filter then starts taking compass readings back into account. From that moment the filter can use compass readings again to filter out any accumulated errors in the filter heading.
Does this mean my filter is ready? No, There still is one problem to tackle. But I’ll explain that problem in the next episode of my ever lasting quest for better sensor readings.
Hi,
It as been some time since I posted my last blog. Work kept me busy for most of the time. But I also experienced some difficulties that affected my motivation. However, I’m not willing to surrender yet. I will discuss the difficulties I experienced, maybe this will help me to find solutions for them.
First, I thought I had improved the position of the compass sensor when I replaced it last time. There were no more disturbances from the motors. But during testing I found out why. The brick was influencing the compass sensor in such a strong way that the influence of the motors was pushed to the background. But still the sensor didn’t give me good readings. I moved the compass sensor to the top of a pole I placed on my robot. This is very ugly but it does the trick (I hope).
Second, my son needed some of the parts back that I borrowed from him. FreeRover was destroyed while he removed them. Luckily there is BrickLick. I put my son on sale on BrickLick, so that problem is solved as well 😉
The third problem is in compass disturbances. I anticipated this problem but I still did not solve it. What happens is this. Suppose a situation were there are no disturbances to the compass. Its reading is more or less accurate. When I start moving the robot the combined output of the gyro and compass sensors by the Kalman filter is better than the output of the sensors on their own. But when I move the robot to a place where there is a magnetical disturbance and hold the robot there the Kalman output is good in the beginning but then it drifts of to the disturbed compass reading. I do understand why, the filter knows both the compass and the gyro cannot be trusted totally, also it knows there is some process noise (ie the position can change due to movements of the robot). So when it gets a false compass reading it still takes this reading into account because it cannot know were the errors came from, it might as well be a false gyro reading or a movement of the robot. I hoped to make the filter smarter and to detect false compass readings, when there is a large change in compass reading that is not in line with the gyro reading I ignored the compass reading. However this proved to be more difficult than I thought. As sensor readings are frequent, disturbances do not show as large changes, they come in slowly and cannot be detected because they are within the range of sensor noise. I do not have a solution for this yet.
In my last post I discussed how I improved the accuracy of my robots sensors. This was done by trying out different sensor locations and measuring the variance of the sensors at different locations. This time I’ll describe how I measured the variance for both the compass and gyro sensor.
Earlier I found out that the NXT motors influence the accuracy of both sensors in two ways. First, the magnetic field generated by the NXT motors influence the compass sensor. I minimized this effect by using the brick as a shield. For this I placed the compass sensor at the top rear of the brick. The NXT motors are underneath and in front of the brick. Second, changes in motor power seem to affect the voltage level on the sensor ports for a short period of time. I’m still figuring out a good method to minimise this effect. For measuring the variance of the sensors I got rid of this effect by pausing sensor readings for a short time after changes in motor power.
For my final test I wrote a program that set motor power to a random level between -100 and 100 every 0,5 seconds. At the end of this time period, just before applying a new value for motor power, I measured the value of both sensors. I did this 300 times, taking two and a half minutes. The motor power was applied to both driving motors of FreeRover, but not to the steering motor. FreeRover was placed on a cardboard box to get the wheels free from the ground and thus to prevent FreeRover from moving. The environment was as stable as could be whilst at the same time the system could vibrate (although not as much as it would when FreeRover really moves). Below is a graph of the measurements.
The graph shows the readings of the compass sensor in red. It is pretty stable, even with the motors running. There are a few readings of the mean giving it a variance of 0.03544. The Gyro readings are in blue, it shows more variation in the readings. The variance of the Gyro is 0,93311 in this test. I did repeat the test several times at different headings and locations. The results of the tests were not always the same but the overall image is consistent with this example.
I want to finish this post with a remarks. This isn’t a test that satisfies scientific standards, but hey, the NXT is a toy after all.
A first test of the accuracy of my compass and gyro sensors proved that sensors are heavily affected by the robot itself. Especially the motors affect the readings of the compass sensor.
With this knowledge In mind I wanted to make my robot more accurate. This is what I mean by system accuracy.
First I looked at the gyro. Both its average reading as the spread around the average seem to be affected by running motors. First I tried to put the sensor away from the motors by mounting it on top of the robot. This didn’t improve readings. The mean was still affected and spread got worse. The latter, I concluded, must be caused by vibrations. These are worse on top of the robot just as the tip of a mast swings further than the boat itself. I put the sensor back to its original position. The drop in mean still puzzled me, but I already concluded I should get the base level of the sensor with running motors, as this is the normal state of my robot. After I changed batteries I noticed that the drop in mean due to the motors was smaller. Maybe the motors cause a drop in voltage on the sensor port? Maybe this causes the drop in mean? Maybe this is also the cause of the spike errors just after changing power levels? I don’t know for sure. But this is more likely to be the cause than electro magnetical disturbances, at least for this sensor.
The compass sensor on the other hand suffered heavily from electro magnetical disturbances during my first test. I tried several alternative positions with this sensor but I ended with mounting it on top of the brick. The brick shields the sensor from the motors this way. This place proved to give the least disturbances. The sensor is still off-balance a short time after applying a change in power sent to the motors, but it recovers within 0.1 seconds. This effect I contribute to a temporarily drop in voltage on the sensor ports. I really should include voltage levels in my readings when testing again.
With the new set up I think the sensor placement is as good as it can be. I’ll have to keep in mind though that sensor readings seem to be affected by drops in power levels. I think I will overcome these effects by not feeding the Kalman filter observations when this happens. This is a valid technique for the filter. One just has to skip the update phase for a few iterations.
In the next post I will present the results of my final test and determine the variance of both sensors within my system.
NXT time 