EKF2 simplify covariance propagation #22344
Merged
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
bresch
force-pushed
the
pr-ekf2-cov-pred-simplify
branch
from
a7be1cf
to
9509c52
Compare
bresch
commented
Nov 10, 2023
| imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var); | ||
| imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var, | ||
| imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var, | ||
| 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt)); |
There was a problem hiding this comment.
@dagar The dt here should really by the time between 2 covariance updates. Actually it should be the same for the other process noise integrations below that are currently using _dt_ekf_avg. I think we should use the exact dt in most places, and not the _dt_ekf_avg.
There was a problem hiding this comment.
Yes I think we should be using the real dt everywhere (time between delayed IMU samples) and if anything have protections on it upstream.
added 2 commits
November 14, 2023 13:53
Slight changes due to simplifications done in the covariance prediction
bresch
force-pushed
the
pr-ekf2-cov-pred-simplify
branch
from
9509c52
to
e4b1eba
Compare
dagar
approved these changes
Nov 14, 2023
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.
Solved Problem
Symbolic derivation tools don't automatically simplify for unit vectors and we were still keeping
dt^2terms that are not necessary (see App. C.2 in https://arxiv.org/pdf/1711.02508.pdf)Solution
Force the simplifications of unit quaternions and
dt^2terms before generating the code. To help extracting thedt^2, the derivation now uses "gyro", "accel" and "dt" inputs instead of "d_ang" and "d_vel".This reduces the covariance prediction operations by 652.
Also frees
3688bytes of flash (testes on fmu-v5).Test coverage
unit, SITL