Bosch BNO055 IMU - Issues when using the “standard” ROS 2 driver

[Sergei Grichine]

Hi All,

if anyone uses this sensor in ROS2, please let me know.

When monitoring /imu/data/orientation/yaw using PlotJuggler, I noticed frequent (~1..3 seconds) spikes or jumps in the data.

This was really impacting localization and navigation. More info and a fix here:

https://github.com/slgrobotics/robots_bringup/blob/main/Docs/Sensors/BNO055%20IMU.md#issues-when-using-the-standard-ros-2-driver

Any suggestions? Thoughts?

--

Best Regards,

-- Sergei

[Michael Wimble]

I’m not sure if my code will help you, but, for your amusement, here’s what I did.

I have a pair of BNO055 sensors on Sigyn. One is mounted upside down relative to the other. My thinking was to cancel out the drift in the rate gyro when the robot is standing still, but it seems to confuse the EKF filter and, with my EKF configuration, one IMU works a treat. I had problems with the standard Arduino libraries for the chip, so with help from AI, I wrote something that didn’t generate errors or anomalies. 

My library has to deal with two hard issues: performance and correctness. Typical BNO055 libraries provided needed data (e.g., quaternion) by doing a lot of work all at once. My system requires that all sensors report data fairly frequently. So no one sensor can take a single big block of time to do anything. As a result, a lot of my code is centered around doing tasks that need a lot of time by breaking them up into a series of partial tasks. I need to make sure that sensors that need to report, say, 100 times a second are never delayed because something else takes a whole millisecond of time as a block when that millisecond task could be broken up into, say, 4 tasks that could be done in 250 microsecond increments. If it’s not obvious what I’m trying to say here, just consider that all of the things that need to be done get broken up into little pieces and the little pieces are scheduled in such a way that I can guarantee the needed reporting of sensor data. The ROS2 nav stack requires that data comes in pretty often, especially as the robot moves faster and faster.

The other problem, one which you’re up against, is that sensors lie all the time. There’s the big lies, like when a SONAR sensor’s ping gets read by a completely different sensor because the ping bounced off from angled reflecting surfaces, and there’s the little lies because of sampling errors. Sure, there are other lies, but those are the ones I most have to deal with. The big lies are dealt with with clever coding, like how, when I use SONAR, I schedule the pings in a way that minimizes the likelihood that stray echoes will show up in the sampling window of another sensor and if it does, I can often reject the data as suspicious. 

The little lies I usually handle with filters. My favorite being an exponentially moving average (https://en.wikipedia.org/wiki/Moving_average). 

I’ll point you to my code which sets up the two IMUs, just remember that in the end I actually use just one. The other is in place as a hot replacement in case the first fails, but I haven’t completed the work that would be needed to actually do that. Also I have an I2C multiplexor so that all my I2C devices can have whatever address they want, as long as it’s not the address used by the I2C multiplexor itself. So you’ll frequently see calls to selectSensor() to set up the multiplexor before talking to the chip. Oh, the multiplexor hardware also provides the needed pullup resistors to minimize ringing on the lines. You will also see references to SerialManager, which multiplexes and manages all the communication between the Teensy 4.1 and the PC; there are multiple kinds of messages sent each way.

Looking at https://github.com/wimblerobotics/Sigyn/blob/main/TeensyV2/modules/bno055/bno055_monitor.cpp, interesting code is as follows:

522: initializeSensor

528: Select the multiplexor channel for the chip.

534: Make sure there is a BNO055 chip at the address (code at line 674, not sure this really accomplishes anything).

542: Reset the sensor. My code makes direct read/writes to the BNO055 registers rather than using someone’s library.

548: Set up the chip so it can be configured. There are a few lines that set up the various register values. The values I use are important for the way I use the chip. If you’re using a library, you might want to see if that library does the same sort of setup. I also have a board that has an on-board crystal, which I rely on. I set NDOF mode and wait for the onboard fusion algorithm to get going.

808: prineSensorReadings — reads IMU data 3 times to get fusion going.

831: Start of sequence of reading quaternions, gyro, acceleration, and status data, retrying on failure.

After setting up the sensors, start rate-limited reading of sensor data, which is part of my overall near-hard-realtime scheduling of all the work the Teensy does, which is tuned for all the different sensors so that I get fast frame rates. So, for each time loop() is called, I iterate over all the IMUs (i.e., both of them), skipping any that failed setup (they get reinitialized eventually in case it was a transitory failure), and check to see if it’s now the scheduled time to read the sensor. All my sensor reads happen on a schedule so they all cooperate to meet the performance requirements.

211: There is a state machine to read one piece of data for each IMU. I don’t read all the data each time loop is called as that would take too much time, making it so other required sensor activity wouldn’t be able to get done in time, so I break it up to read just one bit of data per loop() call. Loop is called hundreds of times per second, so all the data gets read by the time it needs to get published. Line 211 reads the quaternion data and sets the state machine to read the gyroscope on the next loop() call. I keep track of various failures and performance statistics; some info is sent to the ROS2 log output (eventually) and all interesting data is logged to the SD card (if one is installed on the board). This is also part of the whole system safety system, but I won’t describe that as it’s a whole other discussion.

235: Read the gyroscope and set the state machine to read the accelerometer on the next loop() call.

261: Read the accelerometer and set the state machine to read the status on the next loop() call.

287: Read the chip status and set the state machine to go to the COMPLETE state for the next loop() call.

305: The last state that does anything useful for the state machine, the next loop() call will go into the IDLE state and will stay there until it’s time for one of the IMUs to go through the state machine cycles again.

There’s code then that uses a watchdog timer to, essentially, check for wonky sensors and try to recover them now and then. Finally, there is a bunch of code to track if performance guarantees are violated.

Then the code goes into my own version of reading the interesting values from the chip, such as 

622: readQuaternion. Read the registers, piece the data together, scale the data.

701: readGyroscope: Read, piece together, scale.

723: readLinearAcceleration: Read, piece together, scale.

741: readEulerAngles: Read, piece together, scale.

759: readStatus.

The rest are utilities.

--
You received this message because you are subscribed to the Google Groups "HomeBrew Robotics Club" group.
To unsubscribe from this group and stop receiving emails from it, send an email to hbrobotics+...@googlegroups.com.
To view this discussion visit https://groups.google.com/d/msgid/hbrobotics/CA%2BKVXVPcdn%2B39LksizibyYTHe5MBVaMV%3D24gmd9kT1hA5bUfOA%40mail.gmail.com.

[James H Phelan]

Mike,

Leave it to you to twiddle the last bit!

Would be great if you added this explanation as a .md to your 

Sigyn/TeensyV2/modules/bno055/ repo!

James H Phelan
"Nihil est sine ratione cur potius sit quam non sit"
Leibniz

To view this discussion visit https://groups.google.com/d/msgid/hbrobotics/19EA1D4F-0F7D-474C-8F4E-0A2F5053436F%40gmail.com.

[Sergei Grichine]

Hi Michael,

Thanks for the insight — that’s quite a feat of engineering.

To be clear, you and I are working on orthogonal planes (or, better to say, playing on them): you seem to be pushing the limits of performance, while I’m trying to build an island of stability on the quicksand of ROS 2 recipes.

As I see it, ROS 2 relies heavily on the orientation quaternion, particularly the derived yaw component. It may benefit from fusing accelerations and rotational velocities — if you take the trouble to enable that in the EKF parameters file.
The /imu/data topic will then be consumed by other nodes, contributing both to control (e.g., limiting rotational velocity in closed loop) and observation (mapping, navigation).

A brief sensor description is here:
https://www.bosch-sensortec.com/media/boschsensortec/downloads/application_notes_1/bst-bno055-an007.pdf (more links are in my previous post).

If you’re using a single BNO055 sensor (as in my case), with its on-chip NDOF feature enabled ('operation_mode': 0x0C), you should get an already fused quaternion output at up to 100 Hz, which is more than adequate for my control needs.

Sometimes higher rate of data (e.g. wheels odometry) destabilizes control loops (causes oscillations) - I saw that mentioned on the Internet and in my experiments with Seggy. I had to tune down some rates in Nav2 config.

I’m not entirely sure about the “observation” part, but my guess is that a Raspberry Pi 5 at 2.4 GHz (or an overclocked Pi 4 at 2 GHz on my Turtle) might already be overwhelmed running Nav2 and localizer even at a 20 Hz rate.

Anyway — that’s my case.

While I truly admire the effort that went into designing the Teensy interface boards and their firmware, they’d be the wrong ingredient in my particular recipe book. I use an interface (control) board for the wheels; everything else connects over I²C or USB.

That’s probably enough disagreement for one day — more to come on our Tuesday ROS2 zooms. 😉

Best Regards,

-- Sergei

To view this discussion visit https://groups.google.com/d/msgid/hbrobotics/868af8f9-2212-4e61-b584-956cd64f46fb%40hal-pc.org.

[Michael Wimble]

On Nov 8, 2025, at 9:49 AM, Sergei Grichine <vital...@gmail.com> wrote:

Hi Michael,

Thanks for the insight — that’s quite a feat of engineering.

To be clear, you and I are working on orthogonal planes (or, better to say, playing on them): you seem to be pushing the limits of performance, while I’m trying to build an island of stability on the quicksand of ROS 2 recipes.

Well, if that’s true it’s only because during most of the day I’ve already achieved stability so performance is the next important thing for me to work on. Until I do something silly like ’sudo apt upgrade’ (often dangerous) or start editing my code (sometimes foolish).

As I see it, ROS 2 relies heavily on the orientation quaternion, particularly the derived yaw component. It may benefit from fusing accelerations and rotational velocities — if you take the trouble to enable that in the EKF parameters file.
The /imu/data topic will then be consumed by other nodes, contributing both to control (e.g., limiting rotational velocity in closed loop) and observation (mapping, navigation).

Yeah, my EKF node only pays attention to IMU yaw. I’m not at all sure that’s the optimum use of the IMU, but it does work. One of these days I really need to get someone to tell me how I can come up with useful covariance values for everything without having a lot of expensive test equipment. I think I understand the importance of covariance, but haven’t yet seen a single article suggesting we come up with good values for the  matrices nor how to compute them.

A brief sensor description is here:
https://www.bosch-sensortec.com/media/boschsensortec/downloads/application_notes_1/bst-bno055-an007.pdf (more links are in my previous post).

If you’re using a single BNO055 sensor (as in my case), with its on-chip NDOF feature enabled ('operation_mode': 0x0C), you should get an already fused quaternion output at up to 100 Hz, which is more than adequate for my control needs.

Sometimes higher rate of data (e.g. wheels odometry) destabilizes control loops (causes oscillations) - I saw that mentioned on the Internet and in my experiments with Seggy. I had to tune down some rates in Nav2 config.

Very interesting. I need to look into that someday. We’ve had speakers at the meetings that have suggested various control loop rates, but I’m not sure if they are speaking with any authority or with any science. Remember recently how one vendor talked of his bumper-input-only navigation system needed a control loop of 200 Hz to walk the perimeter of a house? My robot is running a lot faster, speed wise, and I’m probably getting a control loop of 30Hz or so most of the time. Of course, I have zero bumper data (thank you, I’ve done a lot of work not to bump into things any more) and have two LIDARs, fairly accurate wheel odometry and 8 close-in time-of-flight sensors to help, but that’s not really the point. Who has written any paper on the effect of control loop frequency and also how the kind and quality of sensor data plays into the needed control loop frequency? Well, maybe I haven’t really looked too hard for such papers. Yet.

I’m not entirely sure about the “observation” part, but my guess is that a Raspberry Pi 5 at 2.4 GHz (or an overclocked Pi 4 at 2 GHz on my Turtle) might already be overwhelmed running Nav2 and localizer even at a 20 Hz rate.

One day I need to re-instrument the nav2 stack (again), especially the controller server and all of the critics, to find out better where computational cycles are most used and in particular how the critic’s scores are contributing to planning. I tried instrumenting the critics once but it’s a lot of stuff to understand because of the number of combinations you have to deal with. On my system, the CPU hardly gets loaded even when I run rviz2 on my robot, so I’m not getting much observational, anecdotal data to tell me how to tune the nav2 stack for performance. 

I do, however, have some real questions about AMCL failures. I’ve read the paper of the underlying algorithm, maybe a few times, and understand it well enough (though I haven’t really sussed the a couple of the formulas) that I am surprised to see the localizer suddenly make my robot jump meters away, sometimes many meters away. Apparently I’ve not found the right thing to tune so that the particles are never sprinkled far from the robot. I mean, AMCL sees quite clearly that the robot only could have moved a short distance during the last cycle, what the frell made it think that the robot sensors make more sense if the robot somehow make a flea-like leap to a completely non-sensical place in the house. 

But, at least on my system, the nav2 stack never seems to be a CPU hog, not even the AMCL node even when I turn up the parameters that make it do more calculation. Maybe it’s because my house is only about 20 meters square and my grids are on about 50 mm spacing and my control loop is only 20 or 30 Hz, but, still, without rviz2 and gazebo running on my robot, speed is not my issue anymore. Even rviz2 doesn’t make much difference, but my PC has 24 threads and some of them run at 5 GHz.

Anyway — that’s my case.

While I truly admire the effort that went into designing the Teensy interface boards and their firmware, they’d be the wrong ingredient in my particular recipe book. I use an interface (control) board for the wheels; everything else connects over I²C or USB.

A lot of my sensors use I2C, though at 400 KHz it’s not blazingly fast. And I use USB 3.1 or 3.2 to talk between my Teensy and the PC because it’s faster than a lot of ethernet stuff, even wifi stuff. Ethernet seems to often suck up bandwidth pointlessly, in my not so humble opinion. Sure, it has layers for error correction and retry, just like DDS has all of those layers to guarantee performance. But I need the Teensy board because on a time-sliced computer, unless it’s very fast and you bypass the usual default scheduling, trying to deal with sensors will pretty much just never get you results that lie less that my brother did all the time when my mother asked what he and his girlfriend did last night. That’s not the way to do sampling. 

And, frankly, every time I’ve implemented error detection and correction it turns out to be pretty much pointless. I mean, I’m just talking between the Teensy and the PC. It’s a 10 inch cable with twistly little wires, all of which look alike, which are wrapped in shielding and using differential signals. The communication errors are pretty much non existent. And, with most of the consumers of sensor data, the nodes already know the sensors are lying anyway, so the rare error pretty much disappears in the black box anyway. As you’ve probably seen, especially on a Raspberry Pi, if you turn on Reliable in the quality of service for any interesting observer of a topic, you’re very likely going to back up every process eventually. DDS is solving a problem we don’t have, in my observation. TCP is solving a problem that USB 3 over short connections doesn’t have. Just ask Camp what happens when he tries to look at camera images with Reliable QOS turned on :-)

Or maybe I’ve just been lucky. Which is very possible. At one ham radio banquet I went to, of the 11 door prizes handed out to a hall of maybe 50 hams, I think I walked away with 7. Well, I had to decline taking a couple of the door prizes because I could hear them plucking chickens and smell them heating up the tar. I do lead a charmed life at times.

That’s probably enough disagreement for one day — more to come on our Tuesday ROS2 zooms. 😉

Best Regards,

-- Sergei

Well, don’t stop disagreeing with me. Remember, I know a lot of stuff, of which approximately half is actually true. I still learn from reading your stuff. Your repo and a cup of hot chocolate make for a fine ending of the day. And my robot seems to work amazingly well right now because when I see it work well I stop pushing it any harder. As one of our politicians has observed, if you quit testing, you have no problems.

[Chris Albertson]

about the control loop frequency. Yes, determining the loop frquency is well studied. So much so that you don’t find papers on it because “everyone already knows”. The trouble is that determining the optimal frequency is hard because you have to know things like the frequency response of your robot's traction drive. to get that you would need to do some controlled experiments. It’s not just the electronics but the mass of the wheels and the mass of the robot. You would need to put in a sine wave inputs to the motors and measure the amplitude the change in velocity of the robot and plot the ratio vs frequency. not easy to do.

For wheeled platforms, think everyone goes too fast but the motors have such high inductance that they act like a brick-wall filter. Going too fast wastes CPU cycles

I think the best way to set the loop frequency is to slow it down and measure performance and make it as slow as you can until you see oscillation, then make it faster. Remember that the PID constants need to be adjusted when the loop frequency changes, if you go half as fast you have to double P to keep the rate of change constant. Experimenting seems crude but it would be really hard to measure the robot’s frequency response and make Bode Plots.

[Chris Albertson]

Sory, I never said what is theoretically “best” control loop. (Assuming you have the means to measure)

There will always be a lag from control input to change in the velocity of the robot. You can express lag as time (milliseconds) or as phase at some frequency. At some frequency the lag will be 180 degrees. You want the control loop to be roughly about 10X faster then that frquency. You can be 100X faster but you waste resources.

PID constants are like this too. In theory, we can compute them, but collecting the data you would need is nearly impossible, so we guess and test

[Sergei Grichine]

All - thanks for responding. Just a couple thoughts:

1. "trying to build an island of stability on the quicksand of ROS 2 recipes" - that's me, but not about stability of robots' movements, but stability of my code base (and related notes).

That's why "do(ing) something silly like ’sudo apt upgrade’, testing on several robots and trying ROS2 Kilted  is a daily routine for me.

My goal isn’t so much to build a robot — somehow, it’s turned into building a codebase that anyone can pick up and drop onto their own creation.

2. Control loop frequency in ROS2 is an interesting subject. For basic PID control (wheel motors following cmd_vel), it’s straightforward.

But not so much when the whole loop — odometry → IMU → EKF → localizer → Nav2 → diff_drive → wheels — comes into play.

I was intrigued to see how Seggy’s BLDC wheel odometry (at 60 ticks/rev) behaved differently with various frequency/rate settings — sometimes, at higher rates, I observed increasing oscillations while following a straight line.

My stable settings are here: https://github.com/slgrobotics/articubot_one/tree/dev/robots/seggy/config  - here is one especially weird:

velocity_smoother:
  ros__parameters:
    enable_stamped_cmd_vel: true  # Whether to stamp the velocity. True uses TwistStamped. False uses Twist
    smoothing_frequency: 6.0      # Rate to run smoother. Default: 20.0 Hz. Experimental - 1.0...12.0 good. Higher value causes oscillations

The other frequencies aren’t high either. To be fair, Seggy’s velocity limits are quite low — even though it can run fast.

Note: I fuse only rotational velocity Z (yaw_vel) on Seggy, with MPU9250. Turtle works better with fused yaw and yaw_vel   - BNO055 has better yaw reading, when spikes are filtered out. Go Bosch!

BTW, I ordered a couple of BNO085's and will be trying another driver with them: https://github.com/bnbhat/bno08x_ros2_driver

I tried that driver with my BNO055 - no go.

--
You received this message because you are subscribed to the Google Groups "HomeBrew Robotics Club" group.
To unsubscribe from this group and stop receiving emails from it, send an email to hbrobotics+...@googlegroups.com.

To view this discussion visit https://groups.google.com/d/msgid/hbrobotics/167B321E-6153-47E4-B694-F0234DED0140%40gmail.com.