Got Gyro drift? Try Sensor Fusion !

[Richard100]

The RoboBees release a simple Inertial Measurement Unit (IMU) based Attitude and Heading Reference System (AHRS), written in LabVIEW.

There are several commercial, automotive grade devices available that are IMU-based and maintain good quality orientation (Roll, Pitch, and Yaw) information over time. Some are within reach of well-financed FRC teams. Here’s some free code that lowers the entry cost for teams that would like to eliminate the drift associated with KoP Rate Gyroscope sensors and try their hand at integrating a basic AHRS into their next robot design. This code is compatible with the AdaFruit.com 9-DoF IMU, product ID 1714, which runs about $20 (no affiliation). This breakout board contains a triaxial Accelerometer, Rate Gyroscope, and Magnetometer, powered by 5 VDC and interfaced over the I2C bus (cRIO and RoboRIO compatible).

The Magnetometer sensor is likely to be new to many teams, and challenges remain in successfully integrating it on an FRC bot, but its contribution is critical to stabilize what is arguably a robot’s most important rotation - Yaw. The simple Complementary Filter is applied to fuse the three sensors into a single composite sensor which provides accurate and stable rotation indications in all 3 dimensions (Roll, Pitch, and Yaw). This sensor fusion approach effectively combines the individual sensor's best respective properties while mitigating their individual shortfalls. Code runs on the cRIO with last year’s version of LabVIEW, and should easily port once FIRST releases this season’s LabVIEW version for the new RoboRIO.

IMU calibration can be involved and a bit esoteric … the code guides you through a simplified calibration process to help you get the most out of your sensor. The various IMU code modules are patterned after the familiar WPI sensor VIs. There’s no coprocessor to integrate or soldering/assembly. The concept behind the released demonstration project is to have you up and running by simply plugging in the IMU and supplying your FRC Team number to the LabVIEW Project file.

The files have a bunch of comments for how the code works, equation references, and implementation considerations to promote learning about this powerful technology. Bug reports are welcome!

Best wishes to all FRC teams this season, the RoboBees are excited about seeing you out on the field!

The RoboBees FRC Team 836

[Edit:Code updated 1 Jan 15 to include all required dependencies.]

[Richard100]

For those who want quick access to some additional detail ...

Description:
This series of Inertial Measurement Unit (IMU) subVIs is patterned after the WPI series for reading sensors, and includes several additional IMU Application subVIs. These application subVIs do the math required for sensor fusion, forming a basic Attitude and Heading Reference System (AHRS), with drift-free Roll, Pitch and Yaw outputs. An IMU typically consists of a 3-axis Accelerometer, Rate Gyroscope, and Magnetometer (sometimes the Mag is absent, resulting in reduced capability). Although technically incorrect, these devices are often referred to as 9 Degrees-of-Freedom (DoF) sensors (3 sensors with each 3 axes in XYZ = 9 DoF). The RoboBees IMU Series subVIs are:

OPEN - Performs IMU initialization, setup, and calibration of each axis of Rate Gyroscope, Accelerometer, and Magnetometer.
IMU RefNum Registry Set / Get - These are standard WPI-type functional globals used to pass IMU configuration data.
READ - Reads IMU sensors, generates and applies calibration values. Gyro output provides XYZ rates as well as integrating these rates for degree displacements. Accelerometer output provides XYZ acceleration and indication of motion and freefall.
RESET - Resets Gyro degree displacement to zero for all axes.
COMPLE FILTER - Performs sensor fusion for basic AHRS using a complementary filter design.
TILT - Calculate Roll and Pitch from gravity vector and implements an optionally tilt-compensated Earth compass for Yaw.

These IMU subVIs are designed to be used with the Adafruit 9-DoF IMU Breakout - containing L3GD20 (Gyro) + LSM303 (Accelerometer and Magnetometer) chips (Product ID: 1714), or the Adafruit 10-DoF IMU Breakout - which adds the BMP180 chip (Product ID: 1604). The 10-DoF IMU adds a Barometer for air temperature and pressure measurements (for indicating altitude, not attitude), and is not currently implemented. The two IMU Application subVIs, COMPLE FILTER and TILT, are agnostic of IMU model, and compatible with separate sensors, as long as all sensor axes are aligned properly.

The motivation to fusing these sensors is to combine their best properties and mitigate the worst properties of each. The overall sensor fusion design is provided in block diagram form (attached). A major benefit is effective elimination of the drift inherent in the Rate Gyro when used alone.

Design:
The IMU OPEN subVI sets up each sensor based on the respective Set Cluster settings. This includes setting the sensor range, output data rate, enabling the High Pass Filter (Gyro and Accelerometer only), and selecting a High Pass Cut off Frequency. Each sensor can also be selectively enabled/disabled. All sensors (except the Barometer), need to be enabled for a fully functional AHRS. The IMU breakout board from Adafruit.com places all sensors on a common I2C bus for communication. The sensor chip's I2C Addresses (in hex) and respective data sheets for I2C register references are:

Chip - Sensor - I2C Address - Data Sheet Reference
LSM303DLHC - Accelerometer - 0x19 - http://www.adafruit.com/datasheets/LSM303DLHC.PDF
LSM303DLHC - Magnetometer - 0x1E - see above
L3GD20(H) - Rate Gyroscope - 0x6B - http://www.st.com/st-web-ui/static/a...DM00036465.pdf
BMP180 - Barometer - 0x77 - http://www.adafruit.com/datasheets/B...0-DS000-09.pdf

Note: Adafruit modified both breakout board configurations in Oct of 2014 to upgrade the Gyro sensor from L3GD20 to L3GD20H (improved performance specifications). This code should be compatible with each Gyro variant. Guidance on the Gyro startup sequence is given in section 3.1 of ST MicroElectronics App Note AN4506 for L3GD20H :

http://www.st.com/web/en/resource/te...DM00119037.pdf

After the sensors are set up, enabled sensors are turned on, IMU configuration data is loaded into an IMU Cluster, and sensor calibration begins.

Calibration:
Raw readings from an IMU are not very useful until they are conditioned by carefully determined calibration parameters. Since reading the digital sensor values is a bit complicated, the IMU READ subVI is simply called by IMU OPEN to produce the raw measurement values used to characterize the sensor. The calibration is only good for the range selected during the cal process.

The calibration process used here is determining the values 'm' and 'b' for the assumed linear sensor model Y=mX+b for each parameter/axis pair, where 'X' is the raw sensor reading, 'm' and 'b' are cal parameters, and 'Y' is the corrected sensor reading. The sensor is 'exercised' eight times to allow the Gyro readings to reach steady-state, which improves the calibration. With the default cal sample size of 30, the Gyro/Accelerometer cal process takes about 2.5 seconds, which is about 0.5 seconds longer than the default cal time for the OPEN subVI for an analog Gyro. The Gyro and Accelerometer cal is performed first, and the robot needs to be completely still during this time (as is required with an analog Gyro). Since the cal is conducted within the OPEN subVI, and OPEN is generally placed in Begin (in FIRST Robotics) before the robot can move itself, this requirement is easily fulfilled. Care, however, must still be taken to ensure no external bumps occur during this cal. This code can detect if a physical disturbance occurred, and recalibrate automatically as mitigation (no extra charge).

Recalling the model Y=mX+b, for the IMU Gyro, 'm' is taken from the spec sheet as the gain (sensitivity) for each axis. Value 'b', which is bias from zero, is determined by taking the statistical mean of a sample of Gyro rate measurements while the sensor is still.

For the Accelerometer, 'm' should be determined off-robot, as it requires the sensor to be moved in all 3 dimensions. These values are either determined experimentally off-robot (to improve upon the standard sensitivity values given in the sensor spec sheet), or assumed to be a value of one for each axis. To determine experimentally, find the maximum and minimum values of each axis as they are rotated (slowly) through vertical. The axis sensitivity is half of the signed difference between max and min. Value 'b' is determined by taking the statistical mean of a sample of Accelerometer measurements while the sensor is still.

The fun really begins with the Magnetometer calibration. Proper Mag cal requires the sensor to be rotated in all 3 dimensions, but unlike the Accelerometer, the Mag must be rotated with the rest of the robot to account for how the robot itself disturbs the Earth's magnetic field (this is a lot easier when the Mag sensor is installed in a smart phone.). Since one typically would not have available a 3-axis non-ferrous robot gimbal, the cal process is designed to support a 2-D calibration in the X-Y (horizontal) plane only.

In another departure from the Gyro/Accel cal, sample data from the Mag must be taken as the robot is moving. This process is optional to allow the 'm' and 'b' Mag cal parameters to be determined during final testing, and then loaded into the Mag Set Cluster for operations (i.e. competition). If selected, the Mag cal process first instructs you to begin rotating the robot (manually) with a pop-up dialog box. Selecting OK initiates data collection, and provides a second dialog box to allow you to signal when you've completed a 360 degree rotation.

Once complete, the X and Y axis 'm' and 'b' parameters are calculated and displayed on the IMU OPEN Front Panel as 'scale' and 'zero', respectively. An XY graph of the data is also provided to allow visual inspection of the cal result - look for a nicely shaped circle (see example at left), if not, repeat the cal. If you like the plot, load the X and Y axis scale and zero parameters into the Mag Set Cluster, retain the default scale (1) and zero (0) values for the Mag Z axis, and turn off Mag cal by deasserting MagCal.

Magnetometer measurement error is generally classified into two categories, Hard Iron and Soft Iron effects. Only Hard Iron effects are compensated here, as they appear to dominate. Soft Iron effect compensation is a future improvement. Performance is adequate as long as sources of Soft Iron distortion (non-magnetized Iron, Nickle, and Cobalt) are kept away from sensor (or vice versa). If an ellipse is displayed, there is Soft Iron magnetic field interference. Ensure the cal plot has equally sized X and Y axis ranges prior to visually inspecting the cal plot and making an ellipse determination. If Soft Iron is detected, either relocate the IMU or discover and remove the offending material. Run all motors and other actuators to verify that the Yaw output of the Complementary Filter is not significantly affected. If so, it may be possible to relocate the IMU or shield the motor with mu-metal or similar. Do not shield the IMU!

Future Improvements:
Barometer, move freefall into interrupt/DIO, DCM/Kalman filter, Soft Iron Calibration, 3D cal, Auto ranging, FPGA integration, Fuse Mag-based Yaw with differential encoders to correct for external-to-robot ferrous sources, SPI interface for faster read response.

Implementation:
Code as per example on upper left. Experimentally determine values for Accelerometer scale as described in above section on calibration, and load into the Accel Set Cluster (or default to one). Set appropriate ranges, fastest data rates (if polling), and enable each sensor. Install sensor on robot and initially set MagCal to True. Determine Mag cal parameters as described above and load these values into Mag Set Cluster. Set MagCal to False to skip future Mag cals.

The Mag cal values are strongly a function of where the sensor is located on the robot, and what is installed nearby, so it's important to recalibrate fairly often, certainly after relocating the sensor, and again after the robot is completely assembled. Be sure you actuate all motors and mechanisms to test for interaction with the Yaw indication, which uses the Mag. Be aware that another robot that comes close to the Magnetometer might disturb the local magnetic field enough to cause error in Yaw – the solution to this difficulty is left to the enterprising student of sensor fusion.

Enabling the Gyro and Accelerometer High Pass Filters (HPF) can have a deleterious effect on dynamic response, as they can introduce lag. HPFs default to off, but advanced control system designers may find them useful. Experiment cautiously.

The Accelerometer senses both gravitation and movement (study Einstein's principle of Equivalence). Roll and Pitch will be affected by movement, as will Yaw should Tilt-Compensation be enabled (this is selected with the Comple Filter subVI).

Configuring these IMU subVIs per the example creates a simple Attitude and Heading Reference System (AHRS). The range of Pitch/Roll/Yaw values are constrained in range, however, as follows: Pitch range is -90<P<+90, Roll range is -180<R<+180, and Yaw range is -180<Y<+180. Although over-rotating Pitch and Yaw beyond their range will result in erroneous readings, this should be an adequate operating range for most robot body applications. These range constraints may or may not be adequate for an actuator or mechanism. Yaw is a bit different ... North is 0 degrees, and East/West is -90/+90 deg, respectively. South is both -180 and +180 deg, as these values are congruent. It is ok to over-rotate Yaw, as this portion of the Complementary Filter is designed with logic to smoothly transition across the +/-180 degree discontinuity (auto-wrap).

It is important to note that while the R/P/Y values are constrained, the Gyro displacement values in X/Y/Z are not. Gyro deg outputs from the IMU READ subVI will wind (not wrap) - i.e. they will continue to count above/below 360 degrees. Use the LabVIEW Quotient & Remainder (modulus) function to convert these outputs to a wrapping behavior, if desired.

Implementation considerations for IMU Read:
This design performs numerical integration off-FPGA for the Digital Gyro (unlike the Analog Gyro subVIs, which typically utilize the FPGA to perform accumulation for superior speed resulting in less integration error). Testing indicates this off-FPGA approach may be adequate for FRC application, however, it is important to ensure the code runs without any timing issues. This is why a Timed Loop is recommended for the IMU Read within Periodic Tasks (executes at a higher priority). Non-uniformity in read iteration is mitigated by design, as each dt is uniquely calculated by time stamping each read event. This means that while some variation in timing can be tolerated (the occasional late iteration), excessively long durations (periods) between successive reads will fail to capture the actual rate profile as it changes resulting in integration error.

In short, use a Timed Loop structure to iterate IMU READs, and set the loop period for a relatively fast iteration (<75 ms). Shorter periods provide improved accuracy of degree displacements, however setting this too short can impact the execution of your remaining code (especially when using Timed Loops). One way of determining an optimized period is to set it while monitoring the "Finished Late?" Loop parameter, ensuring this value stays False. It helps if the CPU Utilization remains low (<75%), which can be monitored with the RT Get CPU Loads subVI (Real Time menu). Study LabVIEW determinism for additional insight.

----- The RoboBees FRC Team 836! ----- You too can conquer inertia!! ... consider a career in STEM!

[Bpk9p4]

I was wondering if this will work with the new roborio. I was looking at the code and it looks like you have the open VI setup for the old system. How would you go about setting this up for the new system

[Richard100]

The posted code was developed with last year's 2013 LabVIEW (LV FRC 2014) and will not work with the new roboRIO until ported. I've had a chance to take a quick look at the LV FRC 2015 copy that we all received, and have noted that the WPI Library for I2C communication has been changed enough to require a bit of tinkering:

(a) The I2C addressing on the WPI I2C open VI now appears to be 7-bit based ... this means you no longer have to left shift the device address by one bit. (Reliable source : Joe Ross / Beachbots, thanx Joe)

(b) The I2C DevRef includes additional parameters to account for things like identifying which of the two roboRIO I2C buses the device is connected to, is now a TypeDef, and re-sequences the parameter order. This make it initially incompatible with my IMU READ Bundle Constants - but is an easy fix.

(c) The code would need to be moved into a project with a roboRIO Target, as the current Project File is cRIO-targeted. Note the I2C VIs change configuration internally as they are target-sensitive ... for example, you'll see the I2C compatibility mode connection on the I2C read VI disappear since I believe roboRIO I2C communication now auto-detects clock stretching.

(d) Also note that the on-line documentation for the I2C read VI appears to have some incorrect information (and is not updated). It claims to limit the maximum number of bytes read in one read operation to four, but I don't think this is correct. This IMU/AHRS code is designed to read six bytes per read operation for efficiency, and the current WPI library still seems to support this.

If you're handy with code tinkering, start with with issues (a)-(c) and you can probably port the code yourself. I will release a roboRIO version myself, on this thread, sometime soon. If anyone already has the IMU sensor and needs to get this working now, let me know so I can help out - I'll re-prioritize my build season to-do list!

Richard

[Bpk9p4]

i do have this sensor and have been trying to get it to work for the past couple days. Thanks for your reply now that i have a better idea of what needs to change i might be able to figure it out. if i do i will post it here. Thanks for your help

[Richard100]

Here is the RoboBees IMU-based Attitude and Heading Reference System (AHRS), designed for the roboRIO, and written in LabVIEW.
Porting to the roboRIO is complete, and issues with the WPI I2C library changes (mentioned above) have been resolved. To try it out, load the Project file, open the IMU Demo VI, and follow the instructions listed on the front panel. Some additional update details:

(a) The calibration time has been optimized to take advantage of the roboRIO’s processing speed improvement over the cRIO. A new set of cycle-time benchmarks were established for the IMU READ operation:
__________________________________________________ ______________
- IMU Read benchmarks, with roboRIO or 4-slot cRIO -

For enabled Accelerometer (A), Rate Gyro (G), and Magnetometer (M), the time to execute IMU Read in milliseconds (ms):

Sensor ... roboRIO Slow / Fast           4-slot cRIO Slow / Fast selected
A           .      0.42      0.34                   5.2      3.6
G           .      1.05      0.69                   5.2      3.6
M           .      0.42      0.34                   5.2      3.6
A + G       .      2.1       1.3                    10       6.7
A + M       .      0.7       0.6                    10       6.7
G + M       .      2.1       1.3                    10       6.7
A + G + M   .      2.9       1.9                    14.7     9.8

("Slow" is when the code’s Fast parameter is deasserted. "Fast" skips the sensor data ready verification before each sensor read operation, which is a good default setting.)
__________________________________________________ ______________

(b) The Complementary Filter uses a value called ‘Alpha’ to set the fusion point between sensors. There are now suggested Alpha value starting points on the IMU Demo Front Panel to lower the learning curve on filter tuning for optimal fusion.

(c) This code has been tested with both AdaFruit’s 9 DoF IMU Breakout Board (Product ID: 1714) and their 10 DoF IMU Breakout Board (Product ID: 1604), and is compatible with either Rate Gyroscope chip version (STMicroelectronics L3GD20 or L3GD20H).

(d) Includes a ‘Bad Cal’ indication for the Magnetometer calibration process, completing the set (A,G,M). These detect when excessive variation exists in the calibration data set produced by IMU OPEN, which can result from some of the common calibration failure modes (like bumping the Rate Gyro when is should be at rest, and attempting to cal the Mag near a field disturbance).

(e) Includes basic Magnetic Anomaly Detection. This might be used for knowing when to stop trusting the Complementary Filter’s Yaw output or knowing when another robot gets near yours. Calibrated magnetic data should be consistent with data available at NOAA’s National Geophysical Data Center at :

http://www.ngdc.noaa.gov/geomag-web/...e=true#igrfwmm
(at least it was in Southern Maryland).

(f) The code has a known issue in interfacing with the MXP’s I2C bus, using the Rev Robotics MXP Expansion Board. Works fine with the roboRIO’s On-Board I2C bus. This is probably something I’m missing in software configuration, and is under review.

Enjoy and good luck this season !

[Bpk9p4]

You just made my day. I have been working the past week on getting this into labview 2015 but i have had no luck. I was just about to give up on this idea when i saw this. I am going to try it right now. Thanks you so much

[Bpk9p4]

I just tested the code. It works great. Very impressive. Thanks again for putting this code up

[randantor]

Originally Posted by Richard100

(f) The code has a known issue in interfacing with the MXP’s I2C bus, using the Rev Robotics MXP Expansion Board. Works fine with the roboRIO’s On-Board I2C bus. This is probably something I’m missing in software configuration, and is under review.

We're using a 6DOF SparkFun IMU this year, and we've been having issues with the MXP I2C bus too. As far as I can tell, you can't open more than one I2C device on the MXP bus at a time, but the onboard bus works fine with multiple devices. It looks to me like a LabVIEW WPIlib bug.

[Richard100]

Have confirmed via testing that issue is restricted to reading multiple devices on MXP I2C bus only. Reading one I2C device on MXP appears fine.

Have reported issue to WPI Library bug tracker for disposition.

[StevenB]

Thanks for sharing! I think this raises the entry point for autonomous localization and navigation, which is great.

Originally Posted by Richard100

The Mag cal values are strongly a function of where the sensor is located on the robot, and what is installed nearby, so it's important to recalibrate fairly often, certainly after relocating the sensor, and again after the robot is completely assembled. Be sure you actuate all motors and mechanisms to test for interaction with the Yaw indication, which uses the Mag. Be aware that another robot that comes close to the Magnetometer might disturb the local magnetic field enough to cause error in Yaw – the solution to this difficulty is left to the enterprising student of sensor fusion.

I think I'd be more worried about transient magnetic fields due to my own electrical system, rather than other robots.

The strength of Earth's magnetic field is about 40-50 microtesla [wikipedia]. The magnetic field due to current in a wire is 2e-7 * I / r, where I is current in amps, and r is the distance in meters. Say you're within 33cm of a wire. That's 0.6 microtesla/amp, and if you've got a drivetrain that pulls 60+ amps (forward and backwards), you've suddenly got a 70+ microtesla swing that will overwhelm the field you're getting from the earth. The thing that scares me about this is that it will kinda-mostly work when you're doing simple testing (without a real robot, or under light load), and start going bizerk when you try it on the field.

[GuyM142]

Can it be done with only a gyro and accelerometer?

[Richard100]

Steven - You need to worry about both. I actually developed the code with the sensor installed on our last year's robot (I do this to ensure I'm accounting for all the other code we run, as the rate gyro numerical integration is particularly time sensitive). In limited testing, I found no real issue installing/operating the IMU on my robot until the drive system was engaged. I also found that pushing (an off) robot alongside my IMU-capable robot produced a similar field disturbance (which makes an interesting robot proximity detector).

Of course you would want to locate the IMU as far away as possible from both the magnets in the DC motors and the current supply wiring - 33 cm is too close. I also experimented briefly with shielding the CIMS with mu-metal, with some encouraging but admittedly incomplete results.

As I mentioned, 'challenges remain in successfully integrating the Magnetometer on an FRC bot', and 'be sure you actuate all motors and mechanisms to test for interaction with the Yaw indication, which uses the Mag'. Hint: You don't have to use the Mag all the time ...

[Richard100]

"Can it be done with only a gyro and accelerometer?"

You cannot get a Yaw indication with an accelerometer, since Yaw is rotation about the Gravity vector. You need something, like a Mag, to generate a Yaw-direction as a reference vector.

This disturbance of the Earth field, however, will have no effect on the complimentary filter's Pitch and Roll indications.

[Richard100]

Update on MXP I2C issue: FIRST reports that they have located the bug causing multiple device read issues on the MXP I2C bus, and has committed the fix into their source code.

I would think this means that the next code update we receive would resolve the issue, but I have no concrete information on this or when to expect it.