Incorporate AHRS acc2q into zscilib as zsl_quat_from_accel #63
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
|
As per the comment in #62, can you create a PR in Zephyr that references this commit so that it can go through the normal test suite before being merged? Thanks for the changes, though ... much appreciated. |
|
Please also add some details in the commit messages and add a signed-off-by to make sure you get the proper credit for the work. |
deganii
force-pushed
the
zsl_quat_from_accel
branch
from
f10cdd0 to
1ce505d
Compare
Add zsl_quat_from_accel header to quaternions.h Incorporate AHRS acc2q into zscilib as zsl_quat_from_accel Currently, the zscilib's Madgwick orientation filter starts with an orientation of <1, 0, 0, 0> regardless of the physical IMU's orientation. The filter then begins the slow/painful process of converging to the correct orientation which can take literally minutes. For an IMU running at 120Hz, the worst-case Madgwick convergence time is about 47 seconds (β * Dt = 0.033 * 1/120Hz). This is far too long for a warm-up; a Madgwick filter that has not yet converged will output spurious and confusing orientations and result in a bad user experience. A python implementation of Magwick, called AHRS, solves this bootstrapping problem with a non-iterative algorithm called acc2q (acceleration => quat) AHRS Implementation: https://github.com/Mayitzin/ahrs/blob/595ab9877c77f5960e39b352c0ef551d2d2efcb9/ahrs/common/orientation.py#L970 Reference: ([Trimpe et al] https://idsc.ethz.ch/content/dam/ethz/special-interest/mavt/dynamic-systems-n-control/idsc-dam/Research_DAndrea/Balancing%20Cube/ICRA10_1597_web.pdf This algorithm that relies only on the current accelerometer sample. The integration of the acc2q algorithm is highlighted in the below flow diagram (dotted red block).  Computing q0 and feeing this into Madgwick results in a very reasonable q0 orientation and a much better overall experience. Signed-off-by: Ismail Degani <[email protected]>
deganii
force-pushed
the
zsl_quat_from_accel
branch
from
1ce505d to
925d63c
Compare
deganii
added a commit
to deganii/zephyr
that referenced
this pull request
Sep 17, 2024
zscilib: fix computational errors This diff adds a top-level Zephyr PR for the zscilib enhancements described in detail here: zephyrproject-rtos/zscilib#63 Signed-off-by: Ismail Degani <[email protected]>
This was referenced Sep 17, 2024
|
Thanks for the review. Please see my updates:
|
|
Ping @microbuilder |
As per some comments in the discord there are still quite a few issues with this preventing it from being it from being mergable. |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Currently, the zscilib's Madgwick orientation filter starts with an orientation of <1, 0, 0, 0> regardless of the physical IMU's orientation.
The filter then begins the slow/painful process of converging to the correct orientation which can take nearly a minute. For an IMU running at 120Hz, the worst-case Madgwick convergence time is about 47 seconds (β * Dt = 0.033 * 1/120Hz). This is far too long for a warm-up; a Madgwick filter that has not yet converged will output spurious and confusing orientations and result in a bad user experience.
A python implementation of Magwick, called AHRS, solves this bootstrapping problem with a non-iterative algorithm called acc2q (acceleration => quat)
AHRS Implementation:
https://github.com/Mayitzin/ahrs/blob/595ab9877c77f5960e39b352c0ef551d2d2efcb9/ahrs/common/orientation.py#L970
Reference:
([Trimpe et al] https://idsc.ethz.ch/content/dam/ethz/special-interest/mavt/dynamic-systems-n-control/idsc-dam/Research_DAndrea/Balancing%20Cube/ICRA10_1597_web.pdf
This algorithm that relies only on the current accelerometer sample. The integration of the acc2q algorithm is highlighted in the below flow diagram (dotted red block).
Computing q0 and feeding this into Madgwick results in a very reasonable q0 orientation and a much better overall experience.