Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

pbio/imu: Implement 3D attitude and fusion of IMU data. #270

Open
wants to merge 14 commits into
base: master
Choose a base branch
from
Open

pbio/imu: Implement 3D attitude and fusion of IMU data. #270

wants to merge 14 commits into from

Conversation

laurensvalk
Copy link
Member

This is an exciting update that implements 3D attitude estimation for the gyro with fusion of accelerometer data.

The main changes:

  • Implement 3D attitude estimation.
  • Implement fusion with accelerometer data.
  • Improve heading estimation for hub.imu.heading(). Now heading will still be close to the correct value when you drive across ramps or lift it from the table.
  • Implement hub.imu.tilt() to use the new fused gyro data. This should be much less noisy than the pure accelerometer result, and it is no longer affected by horizontal or vertical acceleration.
  • Implement hub.imu.orientation() which gives you the rotation matrix of your robot. This is almost always more useful than some kind of (non-)singular angular representation. We'll add some examples to show why this is useful. You could always extract your favorite angular representation from the rotation matrix elements.
  • Add calibration options for gyro and accelerometer offset and bias.

Things that still need to be done:

  • Replace the (still temporary) hub.imu.update_heading_calibration() with an interactive routine that estimates the 3D calibration constants.
  • Make improvements for robustness. Since the new additions depend on user calibration constants saved on flash, we'll want to make sure nothing can go wrong (like zero division or singularities for some attitudes) if the user sets unexpected calibration values.
  • See how well everything works in practice in different use cases.
  • Verify that the 3D heading estimation is no worse than the pure 1D integration. If it is, we need to consider making that separately available.

Other notes:

  • I did not use a standard fusion algorithm. Since we don't have any magnetic sensors and since most LEGO robots don't fly, I think we can benefit from a simpler algorithm that makes use of some of the assumptions that we can make about LEGO robots. Roughly, it works like this:
  • The angular velocity bias is currently still estimated independently as a long term moving average. Since we can make the robot sit still (unlike a drone mid flight), this works quite well.
  • Attitude estimation works by converting the angular velocity to the quaternion rate of change and integrating it over time.
  • Fusion happens by steering the estimated gravity vector towards the measured gravity vector when the hub is (close to) stationary, as if connecting them by a virtual spring.
  • The initial quaternion is set to align with the first available gravity sample (with an arbitrary initial heading).

@laurensvalk
pbio/imu: Drop redundant struct nesting.
4ddd2d8
@laurensvalk
.vscode: Add task for deploying Prime Hub.
f79af7f
@laurensvalk
pbio/sys/storage: Refactor IMU settings to IMU module.
6006d2b
@laurensvalk
pbio/imu: Refactor settings setters with flags.
9c75126 
These flags will later be used to test which calibration has been done already.

We can use the same flag to optionally set settings, rather than use NAN to indicate nothing set.
@laurensvalk
pbio/imu: Add setters for gravity offsets.
60759b4
@laurensvalk
pbio/imu: Calibrate acceleration values.
f00e345
@laurensvalk
pybricks.common.IMU: Fix failing to raise on bad axes.
60273d6
@laurensvalk
pybricks/util_mp: Unify all args None test.
4fa2174 
Since this is done in quite a few functions, this should make the build size smaller and the code easier to follow.
@laurensvalk
pybricks/util_mp: Use args None test.
15c84e7 
Since this is done in quite a few functions, this should make the build size smaller and the code easier to follow.
@laurensvalk
pbio/imu: Set calibrated gyro values.
9388ddb
@laurensvalk
pbio/imu: Implement 3D fusion.
216a0b6
@coveralls
Copy link

coveralls commented Oct 25, 2024 •
edited
Loading

Coverage Status

coverage: 56.385% (-0.4%) from 56.81%
when pulling accb117 on imu
into 4d457a1 on master.

@github-actions GitHub Actions
Copy link

github-actions bot commented Oct 25, 2024 •
edited
Loading

Download the artifacts for this pull request:

@laurensvalk
pbio/imu: Save one off bias calibration.
30974de 
When the hub first boots, it is good to have an initial value for the bias to help suppress drift during the first nonstationary seconds.

As an estimate, we can use the bias first saved on the previous boot.
@laurensvalk
pbio/drv/imu_lsm6ds3: Start stationary detection from slow average.
037543d 
When starting from the latest sample as we did before, the odds are higher that we start on an outlier and have to restart, or allow larger errors.
@laurensvalk
pbio/imu: Reduce default gyro threshold to 2deg/s.
accb117 
Now that it is a bit less sensitive to outliers, we can reduce this value. See pybricks/support#1105
@laurensvalk laurensvalk mentioned this pull request Oct 29, 2024
Open
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers
No reviews
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@laurensvalk @coveralls