Odometry Adventure Begins and Ends

Now that I have caught up to the point Keith started to question Finmark’s odometry accuraccy, I am ready to document where I am with the ROS2 GoPiGo3 odometry.

These are the two tuning tests - 1meter translate and a 360 degree spin both with 30s history and .001meter squares:

After 1m slow 0.075m/s translate toward the wall:

After one 360 degree slow spin at 0.5 rad/sec:

So now off to investigate EKF fusion of this encoder odometry with IMU data.


looks pretty good


I’m so torn up with how to proceed. The encoder odom looks OK but clearly needs fusion with the IMU.

I found an tutorial to follow to implement the EKF and started a “NOTES_EKF_Node.txt” then decided I first, really, needed a safety shutdown node.

Today, in testing the safety shutdown node, I discovered the GoPiGo3 node and the IMU node are each consuming “30% CPU” according to htop, and the sum total of Dave without running the mapping is averaging 1.7 out of 4 or 42% with some 1 minute peaks of 50-60%.

These numbers make me want to start all over to code the ROS2 GoPiGo3 in C++, but that would not be progressing, so I just can’t start over. I get distracted too easily by interesting lines of investigation.

I really want clean odometry and then figure out how to optimize the mapping, and then I need to learn how to add navigation, and then behavior trees, and then wall following, and then …


I guess I’m not entirely surprised by the GoPiGo3 node, but seems really high for the IMU node.

Yeah - I’ve got multiple things to keep me distracted. Always way too much to do.


Must have something wrong:


How fast were you spinning? When I tried that last year I had to spin at a fairly slow rate for everything to keep up.


When I go very slow, the filtered odometry perfectly matched the wheel odometry. There are too many configuration parameters. Did you put your ekf.yaml up on Git somewhere?

I have the feeling I have too many things putting out /tf also - I’m really drowning to understand the data flow, I think I have combined too many different tutorials into one robot which is confusing something.


So I started back at checking the inputs - oh boy - it is getting interesting right there.

When I started up ROS2HH Dave, and commanded 10 slow, full circle 360 degree spins with my wheelBaseRotateTest.py, Dave ended up about 5 degrees off the original heading. The wheel odometry Dave generated and the EKF “fused” odometry both end up claiming the same 9.3 degrees heading. This would seem to indicate the encoder angle accuracy is 4 degrees in 3600 degrees turned being 0.1% error or 4 degrees out of 360 possible headings being 1% error. I don’t know how to describe it but either number is small.

Repeating the 10 full circle spins without ROS running, Dave ended up about 20 degrees off the original heading, and the IMU claimed the heading change was 8 degrees. So here the error is either 12 over 3600 being 0.3% or 12 out of 360 possible headings being 3% error.

Either way of looking at the error, the IMU is generating three times less reliable headings, and that makes me think trying to fuse the encoder odometry with IMU data will end up worse than the wheel odometry alone.

This concludes my investigation into EKF fusing of GoPiGo3 encoder odometry with GoPiGo3 IMU data.

This test says I cannot trust my IMU driver and DI BNO055 IMU at all.

I performed the calibrateIMU, resetIMU, then hand spun Dave in the air 10 turns and set him down as close to the original heading as visually discernable … IMU read 342.8 degrees, roughly 17 degrees error. Additionally, just sitting there without moving, the heading ranges from 339 degrees to 346 degrees for a +/- 4 degree variation, which is only +/- 1% but that +/- 4 degrees multiplied by travelling 3 meters across a room means you could be anywhere in a 42 cm circle.


If I remember correctly, the IMU requires calibration before it can be used reliably.

I forget the exact process, but the IMU on other devices, (like the micro:bit), need to be moved in a figure-8 covering all three axes to set up the accelerometer and magnetometer before use.

You did some experiments before using the IMU, checking the calibration status, and a particular kind of data collection, (or fusion using a special algorithm).

Maybe you need to do that again?

Maybe @cleoqc remembers something?


And came to the same conclusion that the IMU is not better than the encoders. I don’t have any explanation though for why, or confidence to believe my tests.


That’s odd.

You would think that an IMU, (with all its sensors and a zillion degrees of freedom), would do better than that.

There are GPS receivers that claim centimeter accuracy.  In the sixties, military IMU units could accurately predict position within a foot or so over distances of thousands of miles for submerged submarines.

You would think that a present day device would do better than that.

This leads me to two conclusions:

  • We’re doing something wrong.
    • Calibration?
    • Arithmetic precision?
    • Sloppy data aquisition due to errors?
  • The sensor itself vacuums do to age of technology and/or lousy initial manufacturing/programming.

What say ye?


Nothing, nada, nope, don’t have a clue, over my pay grade, haven’t the foggiest.


Now that I thought about it, this is just one more problem I thought buying a commercial ROS2 robot would solve. It did, but there were other problems so I’m feeling doubly bummed out.


Don’t think I have an ekf.yaml file (don’t think I used ekf). My different YAML files are all here (https://github.com/KFW/gpg3_pc/tree/main/config) if they’re of any help.



So weird - I hooked up an MPU9250 IMU that I had played with a little six or seven years ago. It seems to talk to the RPI via I2C sufficient to respond with the proper “Who Am I” info, and the drivers do not throw any errors, when configuring it or going through the calibration steps, or loading a saved calibration.

But the readings are garbage. I don’t know if the board is partially bad, or being anywhere near the GoPiGo3 is just “bad juju” for this IMU.

It looks like Dave is just not destined to have an IMU.


I may not be remembering correctly, but you did have an IMU that was punk and it might have been that one.