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 …
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.
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.
Sloppy data aquisition due to errors?
The sensor itself vacuums do to age of technology and/or lousy initial manufacturing/programming.
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.