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

Yaw Emergency estimator: store attitude as quaternion instead of DCM #24119

Merged
merged 1 commit into from
Dec 18, 2024
Merged

Yaw Emergency estimator: store attitude as quaternion instead of DCM #24119

merged 1 commit into from
Dec 18, 2024

Conversation

bresch
Copy link
Member

@bresch bresch commented Dec 17, 2024

This is a pure refactoring, no functional changes expected.

Solved Problem

The yaw estimator has its attitude stored as a rotation matrix. This requires to save 5 more floats than using a quaternion per EKF; since there are 5 EKFs per yaw estimator this gives 25 floats per EKF2 instance. Furthermore initializing, integrating and re-normalizing a quaternion is simpler than using a rotation matrix.

Solution

Store the attitude as a quaternion.

Test coverage

unit tests

@bresch
yaw_est: store attitude as quaternion instead of DCM
31dedb6 
This saves flash and makes code simpler
@bresch bresch added the EKF2 label Dec 17, 2024
@bresch bresch requested review from dagar and mahimayoga December 17, 2024 11:57
@bresch bresch self-assigned this Dec 17, 2024
@github-actions GitHub Actions
Copy link

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: -136 byte (-0.01 %)] 
    FILE SIZE        VM SIZE    
--------------  -------------- 
-0.0%     -15  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  -2.6%     -71  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
+0.0%     +24  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
   +12%     +32  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
+0.0%    +136  [ = ]       0    .debug_frame
+0.0% +4.72Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +6.5% +4.83Ki  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  -0.0%    -109  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.0% +1.74Ki  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
   +14% +1.78Ki  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  -0.0%      -9  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -0.3%      -3  [ = ]       0    task/task_cancelpt.c
+0.2% +9.14Ki  [ = ]       0    .debug_loc
   +16% +5.26Ki  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +1.2% +3.92Ki  [ = ]       0    [section .debug_loc]
  -0.0%     -49  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.1%    +856  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +4.2%    +264  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +1.1%    +512  [ = ]       0    [section .debug_ranges]
  +0.0%     +88  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-0.1% -2.82Ki  [ = ]       0    .debug_str
 -46.6% -2.71Ki  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  -0.0%    -104  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.5%      +1  [ = ]       0    .shstrtab
-0.0%     -49  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.4%      +6  [ = ]       0    ../../src/modules/control_allocator/ControlAllocator.cpp
  -9.6%    -107  [ = ]       0    ../../src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
  +7.0%     +52  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
+0.0%     +16  [ = ]       0    .symtab
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.9%     +16  [ = ]       0    ../../src/modules/control_allocator/ControlAllocator.cpp
 -12.0%     -96  [ = ]       0    ../../src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
   +12%     +96  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
+0.8%    +136  [ = ]       0    [Unmapped]
-0.0%    -136  -0.0%    -136    .text
  +0.1%      +4  +0.1%      +4    ../../src/lib/battery/battery.cpp
  -0.0%     -40  -0.0%     -40    src/modules/ekf2/modules__ekf2_unity.cpp
  -1.0%     -48  -1.0%     -48    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  -1.5%     -52  -1.5%     -52    ../../src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
+0.0% +13.7Ki  -0.0%    -136    TOTAL
px4_fmu-v6x [Total VM Diff: -120 byte (-0.01 %)] 
    FILE SIZE        VM SIZE    
--------------  -------------- 
-0.0%     -15  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  -2.6%     -71  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
+0.0%     +24  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
   +12%     +32  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
+0.0%    +136  [ = ]       0    .debug_frame
+0.0% +4.72Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +6.5% +4.83Ki  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  -0.0%    -109  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.0% +1.74Ki  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
   +14% +1.78Ki  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  -0.0%      -9  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -0.3%      -3  [ = ]       0    task/task_cancelpt.c
+0.2% +9.05Ki  [ = ]       0    .debug_loc
   +16% +5.26Ki  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +1.2% +3.92Ki  [ = ]       0    [section .debug_loc]
  -0.0%    -139  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.1%    +856  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +4.2%    +264  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +1.1%    +512  [ = ]       0    [section .debug_ranges]
  +0.0%     +88  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-0.1% -2.82Ki  [ = ]       0    .debug_str
 -46.6% -2.71Ki  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  -0.0%    -104  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.5%      +1  [ = ]       0    .shstrtab
-0.0%     -49  [ = ]       0    .strtab
  +4.8%     +44  [ = ]       0    ../../src/lib/sensor_calibration/Magnetometer.cpp
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.4%      +6  [ = ]       0    ../../src/modules/control_allocator/ControlAllocator.cpp
  -9.6%    -107  [ = ]       0    ../../src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
  +1.0%      +8  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
+0.0%     +16  [ = ]       0    .symtab
  +4.4%     +32  [ = ]       0    ../../src/lib/sensor_calibration/Magnetometer.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.9%     +16  [ = ]       0    ../../src/modules/control_allocator/ControlAllocator.cpp
 -12.0%     -96  [ = ]       0    ../../src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
  +8.0%     +64  [ = ]       0    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
+0.2%    +120  [ = ]       0    [Unmapped]
-0.0%    -120  -0.0%    -120    .text
  +1.1%     +28  +1.1%     +28    ../../src/lib/sensor_calibration/Magnetometer.cpp
  +0.0%      +5  +0.0%      +5    [section .text]
  +0.1%      +4  +0.1%      +4    ../../src/lib/battery/battery.cpp
  +0.6%      +4  +0.6%      +4    ../../src/lib/timesync/Timesync.cpp
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
  -0.0%     -40  -0.0%     -40    src/modules/ekf2/modules__ekf2_unity.cpp
  -1.5%     -52  -1.5%     -52    ../../src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
  -1.4%     -72  -1.4%     -72    ../../src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
+0.0% +13.6Ki  -0.0%    -120    TOTAL

Updated: 2024-12-17T12:02:33

mahimayoga
mahimayoga reviewed Dec 18, 2024
View reviewed changes
Copy link
Contributor

@mahimayoga mahimayoga left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Refactoring looks consistent with original code to me

src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp
// Apply the change in yaw angle to the AHRS using left multiplication to rotate
// the attitude around the earth Down axis
const Quatf dq(cosf(yawDelta / 2.f), 0.f, 0.f, sinf(yawDelta / 2.f));
_ahrs_ekf_gsf[model_index].q = (dq * _ahrs_ekf_gsf[model_index].q).normalized();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just double checking about the order - to me it makes more sense to first apply the yaw and then the remaining tilt as the rotation is applied from right to left. Could you confirm?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Rotating a vector is from right to left, yes, but here we're simply chaining rotations here (from left to right). So yes, first rotate around yaw and then tilt.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ah okay, because in the Quaternion header file it says the order is from right to left. But if it's from left to right then makes sense!

@bresch
Copy link
Member Author

bresch commented Dec 18, 2024

SITL test replayed on this branch and main shows no difference at all.
image

@bresch bresch merged commit 4a73195 into main Dec 18, 2024
60 checks passed
@bresch bresch deleted the pr-yaw_est_quat branch December 18, 2024 08:33
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@mahimayoga mahimayoga mahimayoga left review comments

@dagar dagar dagar approved these changes

Assignees

@bresch bresch

Labels
EKF2
Projects
PX4 EKF
Status: ✅ Done
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
3 participants
@bresch @dagar @mahimayoga