Now that I’ve finally gotten some time to discover sensor filtering, I have a few questions regarding gyroscope filtering. I’ll post them sort of one at a time because one has to be answered before the next one makes any sense. I’m experimenting with @jmmckinney 's code currently. My first question is, when I set up the gyroscope, should I be setting up as a gyroscope or as an analog sensor as mentioned in this thread. As far as I can tell, there isn’t any conversion to convert the values to degrees per second in his code, so it seems that I would need to set it up as a gyroscope. However, if I use SensorValue[gyroscope] for example, with gyroscope being set up as a gyro sensor in Motors and Sensors Setup, it would be getting the inbuilt filtered value from RobotC. That doesn’t make any sense based on the logic of the gyro_calibrate() function in his code. What should this be set up as?
Secondly, when a gyro is configured as an analog sensor, what is being returned? The range is from 0 to 4095, but what do these values represent? Can they be converted into degrees per second?
Haven’t been on in a bit, but I know quite a lot about the gyro.
For your own (or @jmmckinney’s or @jpearman’s) filtering, you have to use it as an analog sensor.
As to your second point, anytime you read the port of the gyro directly, what you get is an unfiltered integer that represents the instantaneous rate at which the gyro is turning, with the sign of the value indicating the turn direction. I don’t remember the scale of the value, but it’s pretty clear for the datasheet for thegyro device or from@jmmckinney’s code what the scale is. You’d only read it that way if you wanted to do your own filtering.
@jmmckinney provides a function which returns the filtered turn rate. You have to read that rate over time, noting how long it has been since you last read the turn info, and use the time and rate to calculate a turn value. It’s very straightforward, once you understand it.
Ok, so this makes some sense. I did whip this together. Would you be able to take a look at it and check my logic? It seems to be kind of working, where it measures an actual 90 degree turn to be around 67 degrees.
To be clear about my implementation, I’m actually not doing any filtering on gyro noise while a robot is moving. My code just has an effective model for calibration and preventing gyro drift (with a configurable parameter). Here’s a summarized version of my approach while working with the gyro sensor which should explain how everything works.
There’s a few things to know about the chip and the breakout board that VEX made for the gyro sensor before you can jump in and build a filter for it. Have a look at the datasheet for this chip, it’s important for understanding the sensor scaling: https://content.vexrobotics.com/docs/276-2333-Datasheet-1011.pdf
1. The analog inputs on the Cortex do their ADC on a 5v scale but according to the datasheet of the gyro chip the sensor itself is 3.3v scale. When I was doing the initial work on my filter implementation I determined that there must be some sort of boost circuit on the VEX gyro sensor itself that boosts the 3.3v signal up to 5v for use with the Cortex. Becuase of this boost, you need to scale all your measurements up by a factor of 5v / 3.3v which is about 1.515.
2. There’s two key values we need to look at on the gyro datasheet in order to be able to measure angular rate: the zero rate voltage (that is, what voltage it spits out when there’s no movement) and the sensitivity (that is, the change in volts per degree per second of rotation of the sensor). Remember, these values need to be scaled by our “boost factor” of 1.515. The boosted values should be a zero rate voltage of 1.5v * 1.515 = 2.2725v and a sensitivity (volts per degree per second) of 0.0011v/dps * 1.515 = 0.001667v/dps.
3. We need to convert those voltages from above into ADC values for the Cortex. This is a simple linear interpolation from 0v-5v to 0-4095. So the zero rate value is 2.2725v / 5v * 4095 = about 1861, and the sensitivity in “ticks” per degrees per second is 0.001667(v/dps) / 5v * 4095 = about 1.36527 ticks/dps. I assume you’re able to piece together the rest, but basically a sensor value of 1900 based on these calculations means that the sensor is rotating at a rate of (1900-1861)/1.36527 = about 28.566 degrees per second. You’d then be able to integrate this over time in a loop and get an angle measurement.
4. Now if we measure our raw analog sensor values from the gyro at zero rate, we can find some noise and possibly a different zero rate value that what we calculated. This is why we need to calibrate the sensor. The way I do calibration is by measuring the sensor at rest for a few seconds to get the average value, and set the zero rate value to that average value. We assume with good faith that our sensitivity is constant, because calibrating that would be a difficult task.
5. There’s still the issue of gyro drift. There’s a ton of ways to deal with this, and I happened to choose one that I thought might work well when working on this for my World Championship bots from 2015 and stuck with it, but don’t assume that this is the optimal approach for this sensor, I assure you that it probably isn’t. My approach uses an assumption that the noise is gaussian (that is, you can model the noise distribution with a gaussian, or “normal” model). This assumption allows us to build a model that we can use to determine (with a configurable level of confidence) that any given measurement is noise and should be determined as no movement, or if it is actually movement that should be measured. While calibrating the sensor, we can use the data points that we collected to estimate the standard deviation of the zero-rate measurements by taking the average of their distances from the zero-rate average value that we computed before (read this sentence a few times if it doesn’t make sense right away). For those who have a bit of a statistics background, you know that gaussian distributions can build confidence intervals by using the standard deviation. We can configure a confidence band of n standard deviations around the zero rate average we computed and saved in our calibration routine. Anything in this band is assumed to be noisy zero rate readings, so we can just set them all to the zero rate value, thus reducing or removing gyro drift while not moving.
If you’re just interested in using my implementation, I can make a post that demonstrates what I believe to be the proper way to measure the gyro data using my lib. I’d suggest putting the measurements in a task that runs as fast as possible basically.
Okay, so I think I’m understanding what you are talking about. That really helped me understand what your code is doing and the logic behind it. I’m going to work on something and then report back the results.
Okay, so it’s been quite a while, but I have been trying a bunch of different things. The code I’m using for the filtering isn’t much different from what I had previously other than now I’m only waiting for 1 mSec between measurements. It still only reads about 65 ish degrees when it has turned 90 in reality. I was doing some further experimentation and found that the error is linear. What I mean is that if I turn it 90 degrees and find what percentage of 90 that reading is, then I do that for 180 degrees, and then for 270 degrees, that constant is always very close. As a result, I’m thinking of setting my the gyro value that I will use to the gyroVal * experimentally_determined_constant. Does this sound like something that is okay to do, or is this fundamentally flawed?
Nope, this is common. If you are using RobotC, you can use SensorScale[gyroName] = x; to adjust the scale to match sensor values to actual values. Multiplying your values by a constant is the same.
Okay thanks. I wasn’t sure if that was okay because here we are obtaining the values by reading and interpreting the raw values. Now that I think about it though, it is probably something to do with the variances in manufacturing of these sensors. Thanks a ton!
You’re exactly right; it has to do with the sensors as built, not with how you’re reading them. All sensors have some amount of variation to the stimuli they are designed to measure. Sometimes the manufacturer provides compensation or calibration to get the measurement into a standard range. (temperature sensing diodes, resistive heaters for temperature stabilization and laser-trimmed resistors and capacitors are all typical ways that sensors response is standardized. With this particular sensor board, they didn’t do any on-board compensation or calibration; they’re counting on the user to do that. The variance for this sensor is linear( or close to, within 1 bit of measurement) so that a simple scaling of the output is all you need. What you’ve worked out on your own is pretty much what you need to do.
For a nice description of what’s needed, I usually advise people to read this article:
In particular, read the section labeled “Scaling.”
Thanks for that. I read the whole article and it was very useful. Thank you to Renegade Robotics for writing that as well.
I have updated my program, and it seems pretty accurate now. However, I now get extensive drift again. I’m not sure if that has to do with this constant that I’m multiplying by slowly increasing in error as I turn, or if it’s something else. I will do further experimentation and then post my code. Thanks to everyone that has helped me in this new venture. I couldn’t have done it without you all.
Everything else is identical to the way that @jmmckinney had it in his online repository. Any suggestions on how to fix this? As far as I can tell, the constant I have is pretty accurate.
This is somewhat difficult to follow without the context of the rest of your code. What are the types for your variables in this task? Do you make sure to run the calibrate routine before you use the sensor? There are pieces of code missing that could be contributing to your issue that would be helpful to see in order to help diagnose the issue.
Also, have you tried testing your sensor on someone else’s code, or have you tried testing your code with another sensor? That could help determine if the issue is programmer error or a bug vs an issue with the hardware itself.
Sorry about that. I do run the calibration routine and it works as expected - I can see the values in the debugger after its completed calibration.
Here’s the complete code:
“GyroFiltering.h”:
#ifndef NERD_GYRO
#define NERD_GYRO
float gyroValue = 0;
float finalGyroValue = 0; //value of the gyro after being multiplied by the constant
int gyroscopeValue;
struct gyro_config{
float std_deviation;
float avg;
float volts_per_degree_per_second;
char gyro_flipped;
};
typedef struct {
struct gyro_config config;
int port_number;
float gyro_offset_constant;
} Gyro;
Gyro mainGyro;
//ignore data within n standard deviations of no motion average
#define GYRO_STD_DEVS 10
#define GYRO_OVERSAMPLE 2
//points or time in mSec that the gyro calibrates for
#define GYRO_CALIBRATION_POINTS 1500
float calibration_buffer [GYRO_CALIBRATION_POINTS];
float gyro_get_rate (Gyro gyro);
/**
* generate calibration data for the gyro by collecting
* zero movement data for reference when reading data later
*
* @param gyro instance of gyro structure
*/
void gyro_calibrate (Gyro gyro){
float raw_average = 0.0;
float std_deviation = 0.0;
//calculate average gyro reading with no motion
for(int i = 0; i < GYRO_CALIBRATION_POINTS; ++i){
float raw = SensorValue [in1];
raw_average += raw;
calibration_buffer * = raw;
delay (1);
}
raw_average /= GYRO_CALIBRATION_POINTS;
gyro.config.avg = raw_average;
//calcuate the standard devation, or the average distance
//from the average on the data read
for (int i = 0; i < GYRO_CALIBRATION_POINTS; ++i)
std_deviation += fabs (raw_average - calibration_buffer *);
std_deviation /= (float) GYRO_CALIBRATION_POINTS;
gyro.config.std_deviation = std_deviation;
/*
* Datasheet from VEX indicates that the sensitivity of the gyro is 1.1mV/dps
* and the cortex ADC for raw analog reads ranges from 0-4095 for 0v-5v
* readings. The gyro is scaled from the nominal 2.7v-3.6v operating range
* that the actual chip has to work on the cortex's 5v logic voltage. The scale multiplier
* value is in the ballpark of 1.515.
*/
gyro.config.volts_per_degree_per_second = 0.0011 * 1.515;
writeDebugStreamLine("Calibration complete");
}
/**
* initialize gyro and run the calibration subroutine
*
* @param gyro instance of gyro structure
* @param port_number the port number of the gyro
*/
void gyro_init (Gyro gyro, int port_number, char gyro_flipped, float gyro_offset_constant) {
gyro.port_number = port_number;
gyro.config.gyro_flipped = gyro_flipped;
gyro_calibrate (gyro);
gyro.gyro_offset_constant = gyro_offset_constant;
}
/**
* calculate filtered gyro rate data, ignoring anything within
* GYRO_STD_DEVS standard deviations of the average gyro
* rate value at zero motion
*`
* @param gyro instance of gyro structure
*
* @return gyro rate, in degrees per second
*/
float gyro_read = 0.0;
int sample_sum = 0;
float gyro_get_rate (Gyro gyro){
sample_sum = 0;
gyro_read = 0.0;
#if defined (GYRO_OVERSAMPLE)
if (GYRO_OVERSAMPLE > 0) {
int n_samples = pow (4, GYRO_OVERSAMPLE);
for (int i = 0; i < n_samples; ++i)
sample_sum += SensorValue[gyroscope];
gyro_read = (float) sample_sum / (float) n_samples;
}
else
gyro_read = SensorValue[gyroscope];
#else
gyro_read = SensorValue[gyroscope];
#endif
//Difference from zero-rate value or the average calibration read
float difference = gyro_read - gyro.config.avg;
//Difference fro zero-rate value, in volts
float gyro_voltage = difference * 5.0 / 4095.0;
if (fabs (difference) > GYRO_STD_DEVS * gyro.config.std_deviation)
if (gyro.config.gyro_flipped)
return -1 * gyro_voltage / gyro.config.volts_per_degree_per_second;
else
return gyro_voltage / gyro.config.volts_per_degree_per_second;
return 0;
}
#endif
/**
*This task will keep track of the gyroscope value using gyro_get_rate()
*/
float prevTime;
float currentTime;
float gyroValDiff;
float gyroRate;
float timeDiff;
float timeDiffSeconds;
task gyroValCalc {
prevTime = time1[T1];
wait1Msec(1);
currentTime = time1[T1];
gyroValDiff = 0;
gyroRate = gyro_get_rate(mainGyro);
timeDiff = currentTime - prevTime;
timeDiffSeconds = timeDiff / 1000;
while(true) {
//writeDebugStreamLine("%d", gyro_get_rate(mainGyro) );
gyroscopeValue = SensorValue[in1];
currentTime = time1[T1];
gyroRate = gyro_get_rate(mainGyro);
timeDiff = currentTime - prevTime;
timeDiffSeconds = timeDiff / 1000;
gyroValDiff = gyroRate * timeDiffSeconds;
gyroValue += gyroValDiff;
finalGyroValue = gyroValue * mainGyro.gyro_offset_constant;
if(finalGyroValue < 0) {
finalGyroValue = 360 + finalGyroValue; //add because gyroVal is currently negative to reverse that value
gyroValue = finalGyroValue / mainGyro.gyro_offset_constant;
}
else if(finalGyroValue >= 360) {
finalGyroValue = 360 - finalGyroValue;
gyroValue = finalGyroValue / mainGyro.gyro_offset_constant;
}
prevTime = time1[T1];
wait1Msec(1);
//writeDebugStreamLine("%d", sample_sum);
}
}
I include the preceding file in the following file - “Gyroscope Filtering.c”:
is initialized and calibrated before anything begins.
I have not attempted to use this sensor with anyone else’s code, nor have I tried to use a different gyro with this code, but I believe that this sensor is fine because it worked fine when using RobotC’s inbuilt gyro code when I set it up as a gyroSensor. However, I will try using a different sensor later today. **
I just tried running you code, I’m finding no drift really at all if what you mean by drift is change of gyro angle when it’s not moving.
I added this to the code to debug
and changed this code as it didn’t make much sense to me to multiply gyroValue and then divide it again.
finalGyroValue = gyroValue * mainGyro.gyro_offset_constant;
if(finalGyroValue < 0) {
finalGyroValue = 360 + finalGyroValue; //add because gyroVal is currently negative to reverse that value
// gyroValue = finalGyroValue / mainGyro.gyro_offset_constant;
}
else if(finalGyroValue >= 360) {
finalGyroValue = 360 - finalGyroValue;
// gyroValue = finalGyroValue / mainGyro.gyro_offset_constant;
}
and I also had to change the value of mainGyro.gyro_offset_constant because it was creating an incorrect value of finalGyroValue for me, I found that gyroValue was almost correct without any correction. I just plugged in a value of 1.04 without really figuring out where your value came from.
So after doing that, this is what I see 10 minutes after calibration
drifted less than 1 deg
and if I rotate 90 degrees
seems pretty good, value on the right is finalGyroValue which I assume is your output.
Thank you so much for taking your time to actually test my code.
I probably should have specified though, because when I was saying drift I meant over time of driving it around. When I leave it there, the drift is minimal even over long periods of time. The problem is when I drive it around and turn it in circles several times. That’s what I’m trying to reduce because I’m working on position tracking code. This needs to be very accurate for my purposes.
One of my ideas is that since the constant I multiply by isn’t perfect, it might read 179.5 degrees on a 180 degree turn. Then, if you turn the robot completely around 10 times, it would be 5 degrees off. Keep going and the error would keep increasing. My plan is to try to perfect the constant, but if anyone else has any ideas, I would be very grateful.
For platform tracking/positional awareness determination, you would normally either use more than one sensor system or perform a synchronization maneuver to synchronize and update your directional heading over time. It would be very unusual to use a single sensor system for that purpose for exactly what you’re seeing here: errors accumulate. For VEX you can sometimes use line sensing or ultrasonic sensors as another sensor system and perform sensor fusion to improve your positional awareness. However, aligning yourself with a synchronization maneuver is easier. One very simple such maneuver in VEX is driving in to a wall to absolutely square the sensor with the world. Then re-zero.
This sensor may never provide the accuracy you’d want for absolute positioning over two minutes while driving around in normal field conditions. This is not a complaint about the sensor; it really does a fine job. You just have to use it cleverly to wring a lot out of it.
It might be worth finding out how close you can get the constant if you believe it is contributing reporting error. That’s easily done. Zero the gyro. Pick up the robot and while holding it gently spin in place. Do this for at least five rotations without bumping or shaking the gyro. Ten would be even better. Gently put the robot down and square it up with its original position. Take the gyro reading. Use the difference in readings to determine whether your constant is correct. If it isn’t, calculate the updated constant using the data you just created.
Note that if you are using the standard gyro stuff in RobotC (I know you aren’t; other people will be) that it is helpful to set SensorFullScale to allow wrapping 12 or so times without sensor rollover.
You shouldn’t drive the robot in a circle to do this; the point is to measure the sensor’s sensitivity to rotation, not how it is affected by mechanical and electrical noise when driving.
And on that point, sometimes it is mechanical and electrical noise which are the greatest contributors to inaccuracy. That’s why @jpearman is asking how you have the sensor mounted. You can work on improving that and get lots of gain in accuracy.
Following that, the biggest gains come from occasionally re-zeroing when you know which way you’re pointing. Then, even a large measurement error can give acceptable results.
