Alternative Madgwick filter for IMU #45
Conversation
Convert to Python from https://github.com/arduino-libraries/MadgwickAHRS
Remove other angle reset and set functions
|
For now, I've just removed the original set and reset functions, and added a single Something to note: after calibration, if the robot is held at an angle (eg. RPY=(10,10,10), calling |
|
With this taking ~3ms, should we disable all data rates faster than the 5ms cycle, since they wouldn't be likely to work anyways? Also, can you test this alongside having 4 motors run speed control, just to make sure that we're not pushing past the limits of the processor with this implementation |
Hmm, maybe. Although I could see some users possibly wanting to do their own implementation for whatever reason, and being annoyed that they can't go faster than 208Hz. It's also not something that most users will touch, and anyone who does touch it probably knows what they're doing, so I don't think it hurts to include them all.
This got me thinking, it'd actually be best if we can have the IMU timer running on the other core. Is that easy enough to do with MicroPython? I've not looked into that much yet. |
| @@ -326,7 +339,7 @@ def get_heading(self): | |||
| :return: The heading of the IMU in degrees, bound between [0, 360) | |||
| :rtype: float | |||
| """ | |||
| return self.running_yaw % 360 | |||
| return self.get_yaw() % 360 | |||
|
|
|||
| def get_roll(self): | |||
There was a problem hiding this comment.
In testing, roll and pitch seem to be swapped. I'm also seeing unexpected results for roll (when calling pitch) when the bot is rotated more than 90 degrees to the left or right. Here's me rotating the bot 180 degrees to the right (rotating to the left is the same but negative):
There was a problem hiding this comment.
roll and pitch seem to be swapped
Yeah, they're flipped from what's intuitive (thinking of an airplane). I can fix this by adjusting which axes are used for X and Y, stay tuned!
unexpected results for roll
I think this is an artifact of the axes being flipped, test again once I update the axes. Should also note that some values may rapidly change when pointing nearly straight up - this is a singularity condition that quaternions handle well, but RPY values can suddenly change. I also expect most people won't be driving their robots straight up/down 😉
There was a problem hiding this comment.
Ok, just pushed a commit that flips the axes, the roll and pitch should make more sense now!
| def get_yaw(self): | ||
| """ | ||
| Get the yaw (heading) of the IMU in degrees. Unbounded in range | ||
|
|
||
| :return: The yaw (heading) of the IMU in degrees | ||
| :rtype: float | ||
| """ | ||
| return self.running_yaw | ||
| if self.yawComputed == False: | ||
| self.yaw = math.atan2(self.q[1]*self.q[2] + self.q[0]*self.q[3], 0.5 - self.q[2]*self.q[2] - self.q[3]*self.q[3]) * 57.23 |
There was a problem hiding this comment.
The yaw reading is fine most of the time, and works while the bot is at any angle other than vertical. When the robot is rotated such that roll is near 90 (or -90), the yaw jumps up/down by a fair margin. Here's an example of it jumping from -90 to -165ish
There was a problem hiding this comment.
yaw jumps up/down by a fair margin
Again, this has to do with the singularity condition when near vertical. Think of a vector pointing straight out the front of the robot, projected onto the horizontal plane; the direction of that vector is what's reported as the yaw. Image the robot RPY starts at (0,0,0), then it's pitched up to 89 degrees (almost vertical). The vector pointing out from the front of the robot would get projected into the horizontal plane, and is still pointing at 0 degrees, so the yaw would still get reported as 0. Now imagine the robot is pitched 2 more degrees, going through vertical. Now projecting that vector onto the horizontal plane would cause it to point the opposite direction from the origin, so the yaw gets reported as 180 degrees (or -180).
This is all to be expected with a proper AHRS algorithm, since RPY has a singularity around the vertical directions, which is why quaternions are superior for this. Probably also worth noting that the pitch is actually bounded between [-90, 90], so when the robot gets rotated that extra 2 degrees, the pitch would still get reported as 89 degrees instead of 91 like you might expect. It's very much related to spherical coordinates.
|
If we intend on allowing users to make and use their own AHRS implementations, if only as possible justification for allowing all ODR settings, I'd want to restructure this a little closer to what I did for my Madgwick branch, where I made an abstract AHRS class and allowed the users to set their own if they wanted, as well as provide a relatively simple example that just did the direct integration so they can see the difference between that and Madgwick. That way the users can actually make and use their own implementations without needing to edit the library |
|
As for getting the IMU timer on the other core, I've looked into this a bit for the context of getting the webserver to serve tasks to the other core as well. Micropython's threading stuff is currently undocumented due to still being in development, with the API not yet settled, but I've found a few sources that have info on what is currently available for use. From what I've found, it's definitely worth trying to move this update timer to the other core so it's not fully a concern, but I've also seen somewhere that the garbage collector isn't great about cleaning up the stuff in the other core, so there may be an issue with that? Also, if we decide to use the second thread for this, i'll have to figure out a different solution for the webserver. Let me know what you think |
|
We would also need to test how the thread gets cleaned up when we ctrl-c
out of the program.
…On Sat, Aug 5, 2023 at 3:21 PM Kevin Siegall ***@***.***> wrote:
As for getting the IMU timer on the other core, I've looked into this a
bit for the context of getting the webserver to serve tasks to the other
core as well.
Micropython's threading stuff is currently undocumented due to still being
in development, with the API not yet settled, but I've found a few sources
that have info on what is currently available for use. From what I've
found, it's definitely worth trying to move this update timer to the other
core so it's not fully a concern, but I've also seen somewhere that the
garbage collector isn't great about cleaning up the stuff in the other
core, so there may be an issue with that?
Also, if we decide to use the second thread for this, i'll have to figure
out a different solution for the webserver.
Let me know what you think
—
Reply to this email directly, view it on GitHub
<#45 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/AAKMHRAMWGDG47SDWOGMW6LXT2MM3ANCNFSM6AAAAAA24ZKVFM>
.
You are receiving this because you are subscribed to this thread.Message
ID: ***@***.***>
|
Guess I should have clarified. I meant implementing their own algorithm just as one possible reason, but there are other reasons for allowing all ODR settings. For example, it could be used as an educational thing (eg. talking about really fast ODR values, and how that affects measurements, without implementing any AHRS algorithm). But again, most users won't actually touch the ODR values, so I don't see any harm in including it. Anyone who does touch it probably knows what they're doing, and I try to operate on the principle that if something exists, someone will probably want to use it, so may as well include it.
That's a bit beyond the scope of what I was trying to implement here. I think it's a cool idea, and if you think it's worth doing, feel free! But it's not something I have time for right now.
I've noticed that as well. I played around with some of it a while ago, but it wasn't a perfect experience by any means.
Also a good point. Should probably just wait to implement this until there's better docs and/or we know what we're doing 😉 |
Converted to Python from https://github.com/arduino-libraries/MadgwickAHRS
I'm measuring ~3ms execution time of
_update_imu_readings(), including the I2C read. Given the ~5ms update period, that still gives some overhead for other things to happen. If that's too much compute time, could try lowering the ODR to 104Hz (will affect accuracy though).Still need to look into the
set_xyz()andreset_xyz()functions, since the rotation is now actually stored as a quaternion. I need to brush up on my quaternion math 😉