Got Gyro drift? Try Sensor Fusion !

[Richard100]

Phil -

You might like this ... I developed a new vi to constantly re-calibrate the gyro while the robot is in Disabled Mode. It is integrated into this updated IMU Demo Project, see the True case within the IMU Read Loop for the new vi "IMU ZeroBias". Alternatively, you could use this vi within the Disabled.vi if you like, I just implemented it all within Periodic tasks. Here, you would simply use the existing "Robot Mode" variable to drive the case structure within the IMU Read Loop.

IMU ZeroBias will read a sample set of measurements from each axis for Gyro calibration. Samples are placed into a circular buffer to maintain the most recent period of samples, used to compute the most current zero rate bias. The intent is to constantly update the Gyro rate bias (during Disabled Mode, which is assumed to satisfy the condition that the sensor is not moving). This ensures we have the best possible calibration available immediately prior to the termination of Disabled Mode (DM), effectively dealing with the Gyro's thermal stabilization and the uncertainty in the variation of how long DM may persist from one match to the next - this way you always start with a 'fresh' cal even if it takes 10 minutes for a field start.

If excessive variation in the circular buffer's dataset is detected, the gyro calibration is not updated, and automatically retains the last good calibration until the situation improves. In this way the code will logically adapt to both the delay before match start and any errant movement while Disabled that would otherwise spoil the cal - and your 32 point Autonomous. You can watch the IMU ZeroBias Front Panel to observe this behavior ... watch the constant calibration updates during DM, then bump it to see it freeze on the last good cal until both the physical disturbance is over and the bad data is removed from the circular buffer.

So ... this is the best way I can think of, for the moment. A repeating IMU Open is probably no-go since the I2C bus needs to be closed first.

Only caveat is I haven't been able to test this as much as I normally would like ... see it if works for you and let me know if you see any issues. See you at Chesapeake?

[Richard100]

Phil - Your IMU ReCal looks good to me. The code you have will also recalibrate the accelerometer bias. The DevRef access seems fine. This is a valid alternative to the IMU ZeroBias for a quick Gyro re-cal.

[PhilBot]

Hi Richard

Your new Zero Bias looks outstanding.
I tore out my code and implemented your new system. (I kept a backup just in case of course

I'll get a chance to try it out tomorrow. I'll let you know how it goes.

I see the code that rejects the bad calibration. How much movement is required to trigger this rejection? ie: Is it pretty strong in rejecting bumps etc?

Yes, definitely see you at Chesapeake.

Thanks again for this great code. The digital gyro I/F is really so much better than the Analog one.

Phil.

[Richard100]

How much movement is required to trigger this rejection? ie: Is it pretty strong in rejecting bumps etc?

Disturbance rejection is fairly strong. For example, it would definitely detect a running on-board compressor, even with isolation mounts, should the compressor activate during the continuous calibration process (which of course cannot happen in Disabled Mode).

Sensitivity is set by the constant (0.5) which is the threshold used to evaluate the data set standard deviation. The tradeoff involved in selecting the threshold is that while lower values will reject smaller movements, it will also increase false positives as the threshold nears the noise floor of the sensor. I arrived at the currently coded value experimentally to balance this tradeoff. You can modify the constant to a control and explore while monitoring the IMU ZeroBias Front Panel as you trigger it with movement.

[sraque]

I am starting to experiment with this code and IMU in preparation for next season. First of all, thanks Richard for the code and thorough documentation/example.

I am assuming both Robobees and Gaco used this during competition. How did it work out (particularly the use of the magnetometer)?

Thanks,
-Steve Raque
2614 MARS

[Richard100]

Steve - We had the code installed but the capability ended up being overcome by events ... something to do with cutting our robot in half with a sawzall at Chesapeake to produce an entirely new bi-robot design (all within carry-in weight limits, which was the real challenge!). Turned out well but changed our focus, you might say, and having a AHRS wasn't key at the time.

[sraque]

Richard,

What is the calibration Use Case when the IMU is on the robot. Do you run the Demo project live and get the values, or do you run the robot code with the MagCal in IMUOpen set to TRUE?

[Richard100]

Steve - To conduct the calibration, you want to run 'Live', i.e. using a run-button deploy / debug mode, rather than compiling and deploying code to the roboRIO. This simply allows you to interact with the Dialog boxes that I use to prompt the calibration process (if you try to run the code as deployed when MagCal is TRUE then the program will appear to hang while it waits for you to respond to a Dialog box that, well, isn't displayed anywhere). Yes, you request magnetometer calibration by setting MagCal in IMUOpen to TRUE. Whether you use the Demo Project, or move the IMU code within Demo into your robot code, doesn't matter.

To use the IMU on the robot, it is also important that the calibration be performed with the sensor installed, to ensure the robot hard iron effects are compensated for during the calibration process. If you move the sensor, or change the metallic configuration of the robot, a re-calibration is strongly recommended.

The general idea is to set MagCal to TRUE to perform the cal, run the code, execute the cal, practicing the process until you get a good result (referring to the Example Magnetometer Cal Plot), then stop the code. Copy the four resulting magnetic calibration values (Scale & Zero for both X & Y) listed under Magnetometer Cal Results on the IMUOpen Front Panel, to the corresponding Mag Set control input for the same VI. Save this and reconfigure MagCal to FALSE. Now when you run the code (which doesn't need to be 'Live' anymore) the IMU is fully calibrated and you can use the sensor fusion results from the Complementary Filter VI.

Adjust tau mag and tau acc to taste, to balance the fusion.

Some teams (I believe GaCo was one) only used the IMU and this software for it's gyro readings, which is fine if you don't need the benefit of sensor fusion. This still gives you a three-axis digital gyro. In this case, the magnetometer calibration is not required, and you would read your gyro results directly from the IMURead VI. The IMUZeroBias VI provides the circular buffer to constantly calibrate the Gyro while the robot is disabled, which can be useful in mitigating measurement drift.

[sraque]

Originally Posted by Richard100

practicing the process until you get a good result (referring to the Example Magnetometer Cal Plot), then stop the code.

I'm glad you mentioned this because it reminded me of a question I had forgotten. What makes for a good calibration run? I have done a fair number of these with it now, and I have seen quite a few shapes that are not good, so I redo. Slow turn, fast turn, (no wobble/tilt is important I expect), etc?

Thanks!

[Richard100]

"Rotate robot 360 deg in *horizontal plane*. Best data results from rotating *about actual sensor axis*". (Dialog box instruction, * for emphasis.)

Try not to stray from the horizontal plane. Once the IMU is on robot, this is probably easier to ensure, since you're spinning the entire bot on the ground (and the robot wants to stay on the ground).

Don't make a big wandering loop, spin the sensor about it's vertical axis. If the sensor is in your hand (off robot), you would place it flat on a (non-metal) table (and away from metal fasteners - i.e. look under your table!), and simply rotate the sensor in place without 'orbiting' anything. I usually mount the board on a small block of wood with squared-up sides which facilitates experimentation. If sensor is on-robot, stare at the sensor while you manually spin the robot, adjusting it's motion to maintain the sensor over the same spot on the ground, as much as you can - it won't be perfect.

Make a complete 360 degree rotation, it's ok to go over some.

I usually take about 20 seconds to complete the rotation when the sensor is on-robot, the software is collecting data samples during this period. IOW, don't try to go too fast. It's ok to partially rotate and stop long enough to readjust your grip, then start rotating again. The data analysis process is insensitive to this - you'll just see some data concentration on some locations of the data plot.

If you receive an ellipse on the plot, instead of a circle, your sensor is experiencing soft iron effects for which the code does not compensate. Ensure the graph axes are scaled the same (equally sized ranges), prior to deciding that you see an ellipse, because the graph is set to auto range and might visually distort even though it's really a circle. Follow the guidance under the XY Mag Plot on the IMUOpen Front Panel should you get an ellipse.

After trying all these tricks, if you still can't achieve a fairly noise-free circle, try moving your operation to a completely different location. You may be attempting the cal in an Earth field that is disturbed. I had no trouble with the calibration in my stick-framed workshop, but a metal-sided building might disturb the Earth field. (If you have access to an airport, you could perform the cal on their compass rose, as that area is certified to be magnetically clean ... joking - that's definitely going overboard). I don't expect this to be a real issue, just a possibility.

Bottom line is to use the example plot for reference. This gives you a simple way to show you what you're trying to produce. The reference is typical of what I achieve after some practice. Like many things in metrology, technique can influence the result.

[tetris]

Originally Posted by 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.]

Hello,
thank you for the programm. I am using an Adafruit 10-DOF IMU Breakout - L3GD20H + LSM303 + BMP180 with an ARDUINO UNO.
The problem is: in your programm IMU demo.VI I receive some errors. For instance one is:
"The master copy for this type definition could not be found or the master copy has errors. You must find and fix the master copy, or right-click this type definition and select Disconnect."

Is it possible to work with your programm but with ARDUINO UNO?

best regards

[Richard100]

tetris - It's not clear from your post that you are using the FIRST FRC version of LabVIEW and a National Instruments roboRIO. This is a FIRST Robotics forum and the assumed context is use of FIRST hardware/software. In either case, you're of course welcome to the software.

It seems the DIO Type Def error is coming up due a code change. The grayed out "Digital Module" Type Def is supposed to be a Type Def for selecting which I2C port is being used to connect the IMU to an NI roboRIO. If you have different hardware you may be able to adapt the code. The other two errors can be resolved by removing the two vi's with '?' - they also are not part of the original code release.

If you are using FIRST software/hardware, try re-downloading the demonstration code, or re-installing LabVIEW. If you're not using the FRC release of LabVIEW or roboRIO, you'll have to adapt to what you're using.

This LabVIEW code will not run on an Arduino. You might try the code library that AdaFruit maintains for their breakout board. You can download from their site :

https://learn.adafruit.com/adafruit-...mp180/software

Their code will provide you basic access to the readings from each sensor, but it does not perform sensor fusion - you would add that code yourself. If you can read LabVIEW, you can follow the logic used here to develop C code for the Arduino.

[tetris]

Hello,
I am using the National instrument NI USB-8451. Do you think your programm can work with this device?

When I open the programm in Labview, it looks for library " ROCK ROBOTICS" but it doesn't find it. Do you know where I could download this library? ( then i will maybe not have these "?").

I don't understand what is "FIRST FRC version of LabVIEW ". ( I am using LABVIEW 2014 STUDENT EDITION).
What is FIRST software/hardware?

Best regards

[sraque]

Unfortunately, tetris, this is a forum for a specific set of hardware and software only available to FIRST robotics competition teams (google it to find out). We probably won't be able to help with using other NI hardware and software.

I suggest posting to one of the forums that NI sponsors.

-Steve

[tetris]

HI,
where can I download lobrary "ROCK ROBOTICS"? in order not to have these "?"

Best regards