Interesting. That fits with what I saw when testing the sensor, rapid drift to begin with then much slower. But I didn't run the test for a long time. I intended to and then got side tracked into looking at some other stuff.
I stumbled over this while looking for some other information:
http://web.mit.edu/first/segway/#software
The pdf (in one of the links), is interesting it talks about combining a gyro and accelerometer using a complementary filter...
Experimental Gyro Compass (LeJOS)
Re: Experimental Gyro Compass (LeJOS)
Well, the plot thickens...
I've done some more tests, and I'm not sure if I'm any wiser. This one is really puzzling!
During the rework of the program, I saw some indications that the gyro sensor kind of "settles in". I'm not sure if this is after a turn, after it is switched on or what, but I'm pretty that there is some kind of stabilization period...
Using the he battery level as part of the bias calculation appears to work pretty well, and the sensor value fluctuates much less. Unfortunately the integration part is now very inaccurate! During the rework of the program, I changed the calculations to be made with floating point numbers instead of using fixed point numbers (integers). and I suspect that it has something to do with the inaccuracy, but I'm really puzzled!
The program is listed below. Comments and help would be greatly appreciated!
Kindly,
Povl
I've done some more tests, and I'm not sure if I'm any wiser. This one is really puzzling!
During the rework of the program, I saw some indications that the gyro sensor kind of "settles in". I'm not sure if this is after a turn, after it is switched on or what, but I'm pretty that there is some kind of stabilization period...
Using the he battery level as part of the bias calculation appears to work pretty well, and the sensor value fluctuates much less. Unfortunately the integration part is now very inaccurate! During the rework of the program, I changed the calculations to be made with floating point numbers instead of using fixed point numbers (integers). and I suspect that it has something to do with the inaccuracy, but I'm really puzzled!
The program is listed below. Comments and help would be greatly appreciated!
Kindly,
Povl
Code: Select all
import lejos.nxt.ADSensorPort;
import lejos.nxt.Battery;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import static java.lang.Math.abs;
/**
* This is an experimental test class to study the HiTechnic Gyro Sensor for use
* as a gyro compass..
*
* The class is not in a finished state, and is only meant for experiments.
*
* @author kvols
*
*/
public class GyroCompassTest implements Runnable {
public static final int SAMPLE_RATE = 3;
public static final float BATT_ALPHA = 597.20500f;
public static final float BATT_BETA = 1.111321f;
private static final int INITIAL_READINGS = 100;
private float offsetAdj = 0;
private final ADSensorPort port;
private float heading = 0;
private boolean adjust=true;
public GyroCompassTest(ADSensorPort port) throws InterruptedException {
this.port = port;
port.setType(SensorPort.POWER_9V);
Thread.sleep(100);
for (int i = 0; i < INITIAL_READINGS; i++) {
port.readValue();
}
for (int i = 0; i < INITIAL_READINGS; i++) {
float o = port.readValue();
o = o - (Battery.getVoltage() * BATT_BETA + BATT_ALPHA);
offsetAdj += o;
Thread.sleep(3);
}
offsetAdj /= INITIAL_READINGS;
// Start the sensor sampler
Thread t = new Thread(this);
t.setDaemon(true);
t.start();
}
/**
* @throws InterruptedException
*/
public static void main(String[] args) throws InterruptedException {
GyroCompassTest gt = new GyroCompassTest(SensorPort.S1);
while (!Button.ESCAPE.isPressed()) {
if (Button.ENTER.isPressed()) // Set to zero
gt.heading = 0;
if(Button.RIGHT.isPressed())
gt.adjust=true;
if(Button.LEFT.isPressed())
gt.adjust=false;
Thread.sleep(25);
LCD.clear();
int v = Math
.round(1000 * (Battery.getVoltage() * BATT_BETA + BATT_ALPHA));
LCD.drawInt(v, 5, 6);
// Display the heading rounded to nearest degree:
LCD.drawInt(Math.round(gt.offsetAdj * 1000), 5, 7);
// Display the current estimated sensor offset (1000nds):
LCD.drawInt(Math.round(gt.heading * 1000), 5, 3);
}
}
@Override
public void run() {
try {
float av = 0;
float oldAv;
float err = 0;
int count = 0;
long time = System.currentTimeMillis();
for (;;) {
Thread.sleep(SAMPLE_RATE);
oldAv = av;
av = port.readValue()
- (Battery.getVoltage() * BATT_BETA + BATT_ALPHA)
- offsetAdj;
// Detect no movement and fine tune the offset
if (abs(av) < 2f && adjust) {
err += av;
// If the sensor has not moved for a while, adjust the offset a bit:
if (++count == 10) {
offsetAdj += err / (count*100);
count = 0;
err = 0;
}
} else {
count = 0;
err = 0;
}
// Calculate the absolute rotation
int dt = (int) (System.currentTimeMillis() - time);
time += dt;
heading += dt * (oldAv + av) / 2000f;
}
} catch (InterruptedException e) { // Just stop if interrupted!
}
}
}Re: Experimental Gyro Compass (LeJOS)
Povl,
As far as I can see (I don't know java) there are three potential problems with the code.
1 I find it difficult to understand the code that updates the adjust value. your err variable, to adjust the offset, is based on 10 readings, yet you device it by 1000. Why is this?
You need something like this (not java code)
2 you update the offset when the robot is motionless. This can be a good thing, but you cannot conclude that the robot is motionless based on the gyro. You will have to use another source of information to base this conclusion on. This might be a compass or motor information. Even then, this is very tricky.
As it is now a movement of the robot that is smaller than the threshold (2 in your program) will corrupt the offset. This is likely to happen at the beginning of a movement when coming from a stationary situation. Once the offset is corrupted it is unlikely to be corrected because the program thinks (based on corrupted gyro readings) that the robot is moving when it actually is motionless. This is why you need another source of information to detect a sate of stand still.
3 you use a sample rate of 3 msec. The update frequency is between 3 and 4 msec. Using a rate of 3 msec means that you read some samples twice. This messes up your statistics as the samples are not independent. Try 4 msec instead.
So, for a start I would disable the offset adjustment and see how this works.
As far as I can see (I don't know java) there are three potential problems with the code.
1 I find it difficult to understand the code that updates the adjust value. your err variable, to adjust the offset, is based on 10 readings, yet you device it by 1000. Why is this?
You need something like this (not java code)
Code: Select all
If (absolutely_sure_that_the_robot_is_motionless)
Adjust = adjust * (sample size-1) / sample size + av / sample size
As it is now a movement of the robot that is smaller than the threshold (2 in your program) will corrupt the offset. This is likely to happen at the beginning of a movement when coming from a stationary situation. Once the offset is corrupted it is unlikely to be corrected because the program thinks (based on corrupted gyro readings) that the robot is moving when it actually is motionless. This is why you need another source of information to detect a sate of stand still.
3 you use a sample rate of 3 msec. The update frequency is between 3 and 4 msec. Using a rate of 3 msec means that you read some samples twice. This messes up your statistics as the samples are not independent. Try 4 msec instead.
So, for a start I would disable the offset adjustment and see how this works.
My blog: nxttime.wordpress.com
Re: Experimental Gyro Compass (LeJOS)
Thank you for your comments, Aswin!
I definitely see the problem in the algorithm self-calibrating - there is a real danger that the sensor would either interpret slow movement as no movement, and thereby making the compass slowly drift, or (as I suspect is happening in my case) only see the small spikes as no movement, and interpret the larger spikes as movement. Both types of errors will disturb the interpretation of the readings.
The formula I'm using, should make the internal offset value slowly converge to the observed offset. My intention is to make it converge slowly, hence the division by the larger number.
But it turns out that my real problem is, that the sensor is a bit more noisy that first thought. I created a program to harvest some samples, to see what I'm actually dealing with, and partly because I also saw some strange instability immediately after startup.
The program takes 2000 samples at a rate of 4 ms between samples, then takes a 2 minute pause (still sampling from the gyro). The process is repeated 5 times (due to space limitations in the brick - each round of samples is 16000 bytes of data)
A small program reads the data and converts to a CSV file:
The results are somewhat surprising. the sensor returned data in the range 599-608. In the beginning, data we a bit more spread than at the end of the run 8 minutes later. The samples below are sorted to show the frequency of the sampled data. Needles to say, the sensor was kept absolutely motionless under the test!
Right after start:
600: 2
601: 1
602: 10
603: 421
604: 932
605: 380
606: 2
607: 250
608: 2
After 2 minutes:
600: 1
601: 4
602: 60
603: 977
604: 757
605: 144
606: 3
607: 54
After 4 minutes:
601: 1
602: 169
603: 1299
604: 466
605: 50
607: 15
After 6 minutes:
599: 1
600: 1
601: 9
602: 326
603: 1300
604: 327
605: 29
606: 2
607: 5
After 8 minutes:
600: 4
601: 6
602: 303
603: 1277
604: 378
605: 18
607: 13
608: 1
I definitely see the problem in the algorithm self-calibrating - there is a real danger that the sensor would either interpret slow movement as no movement, and thereby making the compass slowly drift, or (as I suspect is happening in my case) only see the small spikes as no movement, and interpret the larger spikes as movement. Both types of errors will disturb the interpretation of the readings.
The formula I'm using, should make the internal offset value slowly converge to the observed offset. My intention is to make it converge slowly, hence the division by the larger number.
But it turns out that my real problem is, that the sensor is a bit more noisy that first thought. I created a program to harvest some samples, to see what I'm actually dealing with, and partly because I also saw some strange instability immediately after startup.
The program takes 2000 samples at a rate of 4 ms between samples, then takes a 2 minute pause (still sampling from the gyro). The process is repeated 5 times (due to space limitations in the brick - each round of samples is 16000 bytes of data)
Code: Select all
import java.io.DataOutputStream;
import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;
import lejos.nxt.ADSensorPort;
import lejos.nxt.Battery;
import lejos.nxt.SensorPort;
public class GyroSampler {
private static final int SAMPLES = 5;
private static final long SAMPLE_RATE = 4;
private static final long WAIT_TIME = 120000;
/**
* Sample gyro and battery data.
*
* @param args
* @throws IOException
* @throws InterruptedException
*/
public static void main(String[] args) throws IOException,
InterruptedException {
long startTime = System.currentTimeMillis();
final class Data {
int time;
short gyro;
short batt;
}
;
final Data[] data = new Data[2000];
for (int i = 0; i < data.length; i++) {
data[i]= new Data();
}
ADSensorPort gyro = SensorPort.S1;
gyro.setType(SensorPort.POWER_9V);
long time = System.currentTimeMillis();
for (int sample = 1; sample <= SAMPLES; sample++) {
System.out.println("Sample " + sample + " of " + SAMPLES);
File f = new File("sample" + sample + ".dat");
if(!f.createNewFile()) break;
DataOutputStream dataOut = new DataOutputStream(new FileOutputStream(f));
time = System.currentTimeMillis()+5;
for (int i = 0; i < data.length; i++) {
long wait = time - System.currentTimeMillis();
if (wait > 0)
Thread.sleep(wait);
data[i].time = (int) (System.currentTimeMillis() - startTime);
data[i].gyro = (short) gyro.readValue();
data[i].batt = (short) Battery.getVoltageMilliVolt();
time += SAMPLE_RATE;
}
// Save the data in the file:
for (int i = 0; i < data.length; i++) {
dataOut.writeInt(data[i].time);
dataOut.writeShort(data[i].gyro);
dataOut.writeShort(data[i].batt);
}
dataOut.close();
if (sample != SAMPLES) {
// Pass some time
time += WAIT_TIME;
while (System.currentTimeMillis() < time) {
gyro.readValue();
Battery.getVoltageMilliVolt();
Thread.sleep(SAMPLE_RATE);
}
}
}
}
}Code: Select all
import java.io.DataInputStream;
import java.io.IOException;
import java.util.HashMap;
import java.util.SortedSet;
import java.util.TreeSet;
public class ConvertData {
/**
* @param args
* @throws IOException
*/
public static void main(String[] args) throws IOException {
HashMap<Integer, Integer> stat= new HashMap<Integer, Integer>();
DataInputStream in= new DataInputStream(System.in);
while(in.available()>0) {
int time= in.readInt();
int gyro= in.readShort();
int batt= in.readShort();
int count=0;
if(stat.containsKey(gyro)) count= stat.get(gyro)+1;
else count=1;
stat.put(gyro,count);
System.out.println(time+", "+gyro+", "+batt);
}
System.out.println();
for (Integer g : new TreeSet<Integer>(stat.keySet())) {
System.out.println(g+": "+stat.get(g));
}
}
}
Right after start:
600: 2
601: 1
602: 10
603: 421
604: 932
605: 380
606: 2
607: 250
608: 2
After 2 minutes:
600: 1
601: 4
602: 60
603: 977
604: 757
605: 144
606: 3
607: 54
After 4 minutes:
601: 1
602: 169
603: 1299
604: 466
605: 50
607: 15
After 6 minutes:
599: 1
600: 1
601: 9
602: 326
603: 1300
604: 327
605: 29
606: 2
607: 5
After 8 minutes:
600: 4
601: 6
602: 303
603: 1277
604: 378
605: 18
607: 13
608: 1
Re: Experimental Gyro Compass (LeJOS)
Almost forgot: I saw you link Andy, Thanks! Definitely an interesting project!
It's a pretty neat idea to combine an accelerometer and a gyro for that purpose: The accelerometer has the absolute reference, and the gyro can react to rotation. I guess it's a bit like Aswins combination of compass and gyro...
In my current project I was going to use a (gyro) compass to keep track of the heading of the robot, if it somehow was stuck. But compass sensor was to susceptible to errors when the robot wasn't level, and it's a bit hard to place on a robot with magnetic motors and magnetic batteries. The gyro seemed perfect...
Anyway - i followed you idea and took some samples over some more time. See above...
It's a pretty neat idea to combine an accelerometer and a gyro for that purpose: The accelerometer has the absolute reference, and the gyro can react to rotation. I guess it's a bit like Aswins combination of compass and gyro...
In my current project I was going to use a (gyro) compass to keep track of the heading of the robot, if it somehow was stuck. But compass sensor was to susceptible to errors when the robot wasn't level, and it's a bit hard to place on a robot with magnetic motors and magnetic batteries. The gyro seemed perfect...
Anyway - i followed you idea and took some samples over some more time. See above...
Re: Experimental Gyro Compass (LeJOS)
Almost forgot... The battery levels from the same samples are equally interesting. The strange jumps seen in the gyro readings are not seen in the battery levels. In other words: It's probably reasonable to conclude that most of the noise seen in the gyro readings are from the gyro sensor itself. I guess that I have to figure out how to do some filtering of the input...
Btw: Note the resolution of the battery level. It appears to read values in 13 or 14 mV steps.
Povl
First 8 seconds:
7270: 1
7283: 6
7297: 11
7311: 274
7325: 457
7339: 378
7353: 571
7366: 280
7380: 21
7394: 1
After 2 minutes:
7270: 2
7283: 6
7297: 77
7311: 401
7325: 510
7339: 439
7353: 472
7366: 90
7380: 2
7394: 1
After 4 minutes:
7270: 2
7283: 7
7297: 108
7311: 438
7325: 454
7339: 473
7353: 411
7366: 105
7380: 1
7394: 1
After 6 minutes:
7270: 5
7283: 9
7297: 116
7311: 441
7325: 430
7339: 470
7353: 445
7366: 82
7380: 2
After 8 minutes:
7270: 1
7283: 10
7297: 162
7311: 438
7325: 461
7339: 464
7353: 410
7366: 52
7380: 2
Btw: Note the resolution of the battery level. It appears to read values in 13 or 14 mV steps.
Povl
First 8 seconds:
7270: 1
7283: 6
7297: 11
7311: 274
7325: 457
7339: 378
7353: 571
7366: 280
7380: 21
7394: 1
After 2 minutes:
7270: 2
7283: 6
7297: 77
7311: 401
7325: 510
7339: 439
7353: 472
7366: 90
7380: 2
7394: 1
After 4 minutes:
7270: 2
7283: 7
7297: 108
7311: 438
7325: 454
7339: 473
7353: 411
7366: 105
7380: 1
7394: 1
After 6 minutes:
7270: 5
7283: 9
7297: 116
7311: 441
7325: 430
7339: 470
7353: 445
7366: 82
7380: 2
After 8 minutes:
7270: 1
7283: 10
7297: 162
7311: 438
7325: 461
7339: 464
7353: 410
7366: 52
7380: 2
Re: Experimental Gyro Compass (LeJOS)
Sorry, I don't understand the meaning of the 1st row nd the 2nd row and it's dependencies from each other:
can you please explain it to me?After 8 minutes:
600: 4
601: 6
602: 303
603: 1277
604: 378
605: 18
607: 13
608: 1
what's the largest measurable voltage? maybe 14V? displayed as a 10 bit value? 14000/1024=13.7 mV steps?kvols wrote:Btw: Note the resolution of the battery level. It appears to read values in 13 or 14 mV steps.
Re: Experimental Gyro Compass (LeJOS)
Sure!
Every two minutes I took 2000 samples of battery voltage and gyro readout. The number you see are the number of times the gyro value was seen. In your quote, the value of 600 was seen 4 times during the 2000 reads, 601 was seen 6 times, etc. If you add 4+6+303+1277+378+18+13+1 you get 2000.
The interesting part is that in the start the values are quite messy, and does not appear to be normal distributed. The battery readings are nicely distributed all the time, but the gyro sensor isn't until after some minutes.
Yes, a 10 bit AD converter for the battery would also be my guess - that would nicely match the other AD converters in the brick.
Povl
Every two minutes I took 2000 samples of battery voltage and gyro readout. The number you see are the number of times the gyro value was seen. In your quote, the value of 600 was seen 4 times during the 2000 reads, 601 was seen 6 times, etc. If you add 4+6+303+1277+378+18+13+1 you get 2000.
The interesting part is that in the start the values are quite messy, and does not appear to be normal distributed. The battery readings are nicely distributed all the time, but the gyro sensor isn't until after some minutes.
Yes, a 10 bit AD converter for the battery would also be my guess - that would nicely match the other AD converters in the brick.
Povl
Re: Experimental Gyro Compass (LeJOS)
would it be a big effort for you to make a chart for each of the 5 time windows and plot both the gyro and the voltage bell shaped curves projected over each other?
I'm not good in Excel
(incidence on the ordinate related to the normalized 7270 -7400 /599-608 abszisse interval)
I'm not good in Excel
(incidence on the ordinate related to the normalized 7270 -7400 /599-608 abszisse interval)
Re: Experimental Gyro Compass (LeJOS)
No problem, if I may use LibraOffice and GIMP...
- 0.gif (11.78 KiB) Viewed 10330 times
- 2.gif (11.79 KiB) Viewed 10330 times
- 4.gif (11.76 KiB) Viewed 10330 times
Who is online
Users browsing this forum: No registered users and 1 guest