angular position from gyroscope

Hello,
I am having a bit of difficulty in using the IDG500 gyroscope (its was part of a 5 dof from sparkfun). I have used the code below

#define PIN_A3 3 //analog pin 3
#define PIN_A5 5 //analog pin 5
#define PIN_A0 0 //analog pin 0
#define PIN_A1 1 //analog pin 1

int accel_y_raw = 0; int accel_z_raw = 0; int vref = 0;
int gyro_x_raw = 0;
float accel_y = 0.0; float gyro_x = 0.0; float accel_z = 0.0;
void setup()
{
  Serial.begin(9600);
}

void loop()
{
  accel_y_raw = analogRead(PIN_A3);
  accel_z_raw = analogRead(PIN_A0);
  gyro_x_raw = analogRead(PIN_A5);
  vref = analogRead(PIN_A1); //this is vref on the idg500
  
  accel_z_raw = (accel_z_raw - 341); //remove the 0 g bias
  accel_y_raw = (accel_y_raw - 341); //remove the 0 g bias
  gyro_x_raw = (gyro_x_raw - vref); //remove the 0 rate bias
  
  Serial.print("\n");
  Serial.print(accel_y_raw);
  Serial.print(" ");
  Serial.print(accel_z_raw);
  Serial.print(" ");
  Serial.print(gyro_x_raw);
  delay(100);
}

to collect data from the gyroscope and accelerometer (a ADXL335). I rotated the IMU 900 along 1 axis and collected the data. I then put this through a script I wrote in matlab to obtain the angular position of the IMU over time from the accelerometer and the gyroscope. The script is below

accel_y_raw = kalman_raw_data(:, 1); %obtain force in y axis
accel_z_raw = kalman_raw_data(:, 2); %obtain force in z axis
gyro_x_raw = kalman_raw_data(:, 3); %obtain the gyro data
gyro_angle = zeros(length(gyro_y_raw), 1);

%scale the raw values
accel_y = accel_y_raw*5/(1024*0.3336); %convert to g's
accel_z = accel_z_raw*5/(1024*0.3336); %convert to g's
gyro_x = gyro_x_raw*5/(1024*0.002); %convert to angular velocity

accel_angle = atan2(accel_z, accel_y)*180/pi; %obtain the angle from the g's
plot(accel_angle);

%calculate angle using gyro data
%this is done by intergrating and accounting for the 100ms between samples
total = 0;
for i =1:1:length(gyro_x)
    gyro_x(i)
    total = total + gyro_x(i)*0.1;%do the integration
    gyro_angle(i) = total;%this will store the change in angle from some origin
end

figure, plot(gyro_angle)

The angular position from the accelerometer is as it should be, however the angular position from the gyroscope does not make much sense. I have attached these plots as an attachment.
It would be appreciated if someone could help me out.

It's not clear from the description how the data you got differed from what you expected.

The delay of 100 milliseconds combined with the time needed to read each of the analog inputs gives you an interval slightly longer than 100 milliseconds. This will screw with the rate gyro calculations because each rate will apply for slightly more than the 100 milliseconds calculated. The error accumulates.

The delay of 100 milliseconds combined with the time needed to read each of the analog inputs gives you an interval slightly longer than 100 milliseconds

...plus the print time of slightly greater than 1ms per character.

my attempt to attach the plots of the angular position over time from the accelerometer and gyroscope seemts to have failed, so i will describe what happened.
For the accelerometer, the angle i calculated varied from an initial value of 900 to 0 then back to 900, which is expected because i moved the accelerometer 900 from an initial position then back to that position.
For the gyroscope, the angle i calculated varied from an intial value of 00 to -350 then back to 00.

I also timed the duration of a loop (including digitising the analog signal and serial port communication) and found that the loop took 110ms, but that still did not explain the angle I was calculating from the gyroscope output.

Also thanks for your time, it is really appreciated.

I don't see any obvious problems in the math, assuming the analog reference voltage is the default 5v.

Does it always get -35 instead of -90? Have you tried turning the gyro at different rates to see if the error is rate-sensitive?

The voltages you are measuring are quite low so it might be a rounding error in the A/D.

Without seeing the raw data its not easy to work out if the problem is with the data or with the script - I'd suspect the later since its easy to get a constant factor wrong.

I would suggest first doing a simple test to convince yourself that the output from the gyro is what you expect for a given rate of turn.

Secondly to get reliable timing from the sensing code don't use delay(), use millis() and waiting code something like:

long next_tick = millis () + 100 ;

void wait_next_tick ()
{
  while (millis () < next_tick)
  {}
  next_tick += 100 ;
}

Also you can do the angle integration in the Arduino code where you have the rate values and the time values to hand - quicker and easier to test that its working if the Arduino reports back immediately rather than having to upload to a script.

1.) Neither of those devices is a 5v device.

2.) vref from the IDG500 is NOT the zero rate bias. The zero rate bias will be somewhere around half of vref.

I would be interested in seeing how you have these sensors wired up.

jraskell:
2.) vref from the IDG500 is NOT the zero rate bias. The zero rate bias will be somewhere around half of vref.

Are you sure? The VRef is 1.3v and the signal can go +/- 1v (500°/s at 2mv/°/s). If the 'zero' point was 0.65v (1/2 of 1.3v) the signal could go to negative voltages and that doesn't sound right. I think that if VRef wasn't the zero reference the chart would have shown the gyro as rotating constantly.

johnwasser:

jraskell:
2.) vref from the IDG500 is NOT the zero rate bias. The zero rate bias will be somewhere around half of vref.

Are you sure? The VRef is 1.3v and the signal can go +/- 1v (500°/s at 2mv/°/s). If the 'zero' point was 0.65v (1/2 of 1.3v) the signal could go to negative voltages and that doesn't sound right. I think that if VRef wasn't the zero reference the chart would have shown the gyro as rotating constantly.

You are correct. I made the assumption that it was a standard vref out, and it isn't. In fact, I think it's rather misleading to call it a vref at all when it is, in fact, more of a zref. My bad either way though as the datasheet clearly explains what the output is and does.

Ive modified my test program as per suggestions.
1)I used the 3.3v reference for the ADC which should help combat rounding error
2)to get reliably timing for the integration I measure the time it takes to execute each loop continuosly and log it for later calculation. I still retain a small delay.

the modified arduino code is as follows

#define PIN_A0 0 //analog pin 0
#define PIN_A1 1 //analog pin 3
#define PIN_A5 5 //analog pin 5

int x, ref;
unsigned long time1, time2, deltatime; //used to work out duration
void setup()
{
  Serial.begin(9600);
  analogReference(EXTERNAL); //use the 3.3v reference for the ADC
}

void loop()
{
  time1 = millis(); //get start time
  x = analogRead(PIN_A5); //digitise gyro output
  ref = analogRead(PIN_A1); //digitise reference
  
  Serial.print(x); Serial.print(" ");
  Serial.print(ref); Serial.print(" ");
  delay(20);
  time2 = millis(); //get end time
  Serial.print(time2-time1); Serial.print("\n");
  //time1 = millis();
  //Serial.print(time1-time2); Serial.print("\n"); //about 1ms to do this command
}

I then process this in matlab (i like matlab because of its plotting function) with the following code

gyro_raw = input(:,1); %get the raw gyro data
ref = input(:, 2); %get the raw vref data
time = input(:, 3); %get the time (ms) between samples
time = time + 1; %add a 1ms to account for the final serial print
time = time/1000; %convert to seconds
gyro_angle = zeros(length(gyro_raw), 1); %create a matrix to store the angular position

angular_rate = ((gyro_raw - ref)*3.3/1024)/0.002;%get the angular rate

%get the angular position
%this is done by integrating and accounting for the time between samples
total = 0; %stores that angular position
for i =1:1:length(gyro_raw)
    total = total + angular_rate(i)*time(i);%do the integration
    gyro_angle(i) = total;%this will store the change in angle from some origin
end
plot(angular_rate);
figure, plot(gyro_angle);

What I did was rotate the gyroscope and plot the angular position over time. I still have the same problem where the angular position I measure from the gyroscope does not reflect the amount I rotate the gyroscope. For example I rotate the gyro past 900 but I measure a rotation less than 900, and this happens consistently. However if I change the sensitivity from 2mv/0/s (as per datasheet) to say 1mv/0/s I get better results. These are pretty rough experiments because I do the rotations by hand and observations by eye. I would attach the raw data I have collected and the plots I made, but I don't know how.

I have connected the IMU and arduino as follows
IMU board side connected to Arduino Side
pin5 (vref) analog in 1
pin7 (xrate) analog in 5
pin8 (gnd) the gnd pin below aref
pin9 (3.3v) 3.3v power output

At this point I think I am inclined to blame the hardware (I am aware this is rather poor form), and try a new gyroscope and hopefully that will solve everything

You are using a baud rate of 9600, it will take vastly more than 1ms to output all those Serial bytes!!!

You must measure the true time between samples:

long prev_time = millis () ;

void loop ()
{
  // blah blah read the values, etc.
  long time = millis () ;
  Serial.println (time-prev_time) ;
  prev_time = time ;
}

This way you measure the time from one execution of loop to the next at the same point. Without this your integration is meaningless. You were measuring the time to call delay() and then hoping those Serial.print statements were running at much more than 9600 baud. (Each byte transmitted will take over 1ms at 9600).

Also I'd recommend doing the integration on the Arduino at a sampling rate of 300Hz or so (more than twice the bandwidth of the LPF) to reduce quantization error.

You are using a baud rate of 9600, it will take vastly more than 1ms to output all those Serial bytes

...plus the print time of slightly greater than 1ms per character.

MarkT:
You are using a baud rate of 9600, it will take vastly more than 1ms to output all those Serial bytes!!!

I think you may have misunderstood my comments. What I meant was the single Serial.print() command takes 1ms. In my code I measured the duration of a loop to be approximately 30ms which was what I used for my integration. I used that extra 1ms to compensate for that final serial.print() that was not included in the measured time.

MarkT:
Also I'd recommend doing the integration on the Arduino at a sampling rate of 300Hz or so

I modified my code as per your suggestions so the sampling is done as fast as the micro can do it

#define PIN_A0 0 //analog pin 0
#define PIN_A1 1 //analog pin 3
#define PIN_A5 5 //analog pin 5
int x, ref;
float angular_rate;
float angular_position = 0.0;
unsigned long prev_time, time; //used to work out duration
float delta_time;

void setup()
{
  Serial.begin(9600);
  analogReference(EXTERNAL); //use the 3.3v reference for the ADC
  prev_time = millis();
}

void loop()
{
  x = analogRead(PIN_A5); //digitise gyro output
  ref = analogRead(PIN_A1); //digitise reference
  //Serial.print(x); Serial.print(" "); 
  //Serial.print(ref); Serial.print(" ");
  
  angular_rate = ((float)(x - ref))*3.3/(1024.0*0.002); //get the angular rate
  time = millis();
  delta_time = (float)(time-prev_time);
  angular_position = angular_position + angular_rate*time/1000.0;
  //Serial.print(angular_rate, 4); Serial.print(" ");
  Serial.print(angular_position, 4); Serial.print("\n");
  //Serial.print(delta_time, 4); Serial.print("\n");
  prev_time = time;
  //time1 = millis();
}

I have attached the angular position data to this post in .txt format. For this test I left the gyro flat on a level surface. What concerns me here is that not only does the angular position run away, it seems to be doing so at an increasing rate. Not sure what to do here.

data.txt (3.46 KB)

What I meant was the single Serial.print() command takes 1ms

No, it takes over 1ms per character printed, including CR/LF.
(the last character of any run of data sent will allow Serial.write to return as soon as it is written to the Tx register, so unless the next character to be written comes within another millisecond, the last character comes for "free")

What concerns me here is that not only does the angular position run away, it seems to be doing so at an increasing rate. Not sure what to do here.

Yes it will drift, its a rate gyro, not an absolute one. You can improve things by oversampling the zero point to more bits so that the systematic drift is reduced, but the random component is inescapable. Typically you ensure there is enough noise (off the order of one LSB) in the system and take 2^n readings of the 'zero' value - the average of these is an estimate of the true zero with up to n/2 bits more accuracy. You only get n/2 bits more because of the noise.

This reference might be useful: http://www.atmel.com/dyn/resources/prod_documents/doc8003.pdf

You may also find that the zero point itself tends to drift with time and temperature and needs regular recalibration, but the best thing is to use another reference for low-frequency angular reference and Kalman filtering to stitch them together.

Hi,

I would like to copy the pitch and roll of a platform but don't know what I need for it.
So basically I want to reproduce the pitch and roll angle on another platform so that the two platform are 'pitched' and 'rolled' the same way (so I can give the occupants on platform A the same feeling as if they where on platform B). That is why I post this message in this topic (I need to know the angular position after all).
Do I need accelerometers, gyros, compasses or a combination of them? It looks like I need a balancing robot kind of setup which uses a combination of gyros and accelerometers (?).
I only know that gyros measure angular velocity and accelerometers measure acceleration and that an iphone uses accelerometers...

Thanks,
Fons

Hey
You should have a look at my guide. I am pretty sure that it will answer some of your problems.
http://arduino.cc/forum/index.php/topic,58048.0.html

  • Lauszus

In my code I measured the duration of a loop to be approximately 30ms which was what I used for my integration.

With one very simple modification of your code, you can go from 'approximately' to exactly.
Move your assignment of time to prev_time from after your serial outputs to immediately after your calculation of delta_time. By making that one change, your delta_time value will be exactly the duration of THAT specific loop. No appoximation of loop time necessary. One less source of integration error.