Repository navigation
Derive backend attitude eulers/rot-matrix from quaternion - #34564
peterbarker wants to merge 13 commits into
Conversation
Global Size Summary (Total Flash delta in bytes)Built from 6bbf8f3 rebased onto b032bcb
|
Previous review (2026-10-01)Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting. Reviewed at head Thanks, this is a nice cleanup. Having one quaternion as the source of truth for attitude removes the old euler/matrix/quaternion trim inconsistencies. I found no blocking issues at fe842d9. The new TrimCompositionMatchesRotationMatrix test passes, every commit builds copter and replay, and test.Copter.Replay passes including the new Trim subtest. I also confirmed that the Trim subtest fails (about 18.6k mismatches) if the AP_DAL RFRN trim rebuild is removed, so it really does guard that fix. A few comments:
Not exercised in this review: Plane or other-vehicle SITL with non-zero trim, hardware, and ExternalAHRS hardware. |
fe842d9 to
0fd8419
Compare
Previous review (2026-10-01)Automated review note — AI-generated (Claude), validated against the live diff. Please sanity-check before acting. Reviewed at head Thanks for the follow-up. The head moved from fe842d9 to 0fd8419. The library commits differ only by the blank-line cleanup, and the new Plane test is the only functional addition. The blank-line nit is fixed. The description now covers the single-axis EKF euler change and the Plane impact. The Plane test passes at this head, as do the quaternion gtest and test.Copter.Replay including Trim. No blocking issues.
The earlier replay-trim timing note is unchanged and still informational only. Not exercised in this review: hardware or ExternalAHRS flight with non-zero trim, and full vehicle suites. |
0fd8419 to
5701032
Compare
Previous review (2026-10-13)Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head Re-reviewed at 5701032 (previously 0fd8419). The head moved by a rebase only: a range-diff shows the 13 commits unchanged apart from hunk context in the Plane test commit. Nothing newer is covered. CI had not run at this head when checked (75 of 77 checks queued), so no CI result is used. No blocking issues. At this head the quaternion gtest passes 13/13, test.Copter.Replay passes with 0 errors in all 7 subtests, the Plane DCM and ExternalAHRS tests (WindEstimatesTrim, DCMFallback, ForcedDCM, VectorNav, MicroStrain 7, InertialLabs) and the Copter AHRS trim tests pass, and copter, plane and Replay build at every commit that touches libraries/. The two points from the last review are unchanged, so here they are again briefly with fresh numbers.
One thing I only half covered in the first review. I said the new Trim replay subtest guards the AP_DAL rebuild. It does, but the existing GPS subtest does too, since it already sets AHRS_TRIM_X 0.01 and AHRS_TRIM_Y -0.03 (Tools/autotest/arducopter.py:16771). With the two rebuild lines removed, test.Copter.Replay failed here in the GPS subtest with 18469 mismatches before it reached Trim (and Trim run on its own failed with 18705). So Trim adds a larger trim rather than the only cover. No change needed; it is your call whether the extra flight is worth it. The replay-trim timing note (https://github.com/ArduPilot/ardupilot/pull/34564/files#diff-9b93c10628938da0dc08557a4c945dd1751bff6ca24e33aa33f39095b8dd2248R89) is unchanged and still informational only. Not exercised: hardware flight with a non-zero AHRS_TRIM, ExternalAHRS on real hardware, embedded builds at this head, and full vehicle suites. |
5701032 to
68ba0d9
Compare
Previous review (2026-10-15)Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head Re-reviewed at 68ba0d9 (previously 5701032). The branch was rebased onto 0922bce and the Plane test commit was rewritten; the other 12 commits are unchanged. Nothing newer is covered. No blocking issues. The two points from the last review are fixed. test.Plane.AHRSTrimMission now discriminates: it passes here at 0.15/0.25 deg mean banked pitch/yaw error and fails on the merge base at 1.33/3.24 deg. The 'What changes numerically' wording is now the right way round.
The replay-trim timing note (https://github.com/ardupilot/ardupilot/pull/34564/files#diff-9b93c10628938da0dc08557a4c945dd1751bff6ca24e33aa33f39095b8dd2248R89) is unchanged and still informational only. Not exercised: a hardware flight with non-zero AHRS_TRIM, ExternalAHRS on real hardware, embedded builds at this head, and full vehicle suites. |
68ba0d9 to
f88784b
Compare
The vehicle-body-to-autopilot-body rotation is only set in start_frame(), which is not called when replaying, so on Replay the rotation remained identity rather than reflecting the AHRS_TRIM of the original flight. Rebuild it from the trim received in the RFRN message. Currently latent: nothing consumes this rotation during replay, but any future use of getRotationBodyToNED (or anything else derived from this rotation) in EKF-internal code would silently lose trim on replay.
Flies the standard replay mission with 3/-2 degrees of trim so that any replayed estimator output which depends on the trim rotation restored through the DAL is checked against the original flight.
…trix quat.rotate(-trim) treats the trim parameters as a rotation vector; everywhere else trim is applied as the euler rotation matrix get_rotation_vehicle_body_to_autopilot_body() - in DCM's body matrix and the EKFs' getRotationBodyToNED. The two conventions differ at second order in the trim angles (measured 0.05 degrees of attitude with 3/-2 degrees of trim), so each backend's quaternion disagreed with the rotation matrix supplied by the same backend. Apply the same rotation to the quaternion that is applied to the matrices, via a quaternion built from the trim euler angles, cached alongside the trim rotation matrices.
Backends now supply only a quaternion as their attitude estimate; the euler angles and rotation matrix in the results are derived from it by common code. This guarantees all attitude representations supplied by a backend agree; previously the euler angles disagreed with the quaternion by up to 3.8 degrees (EKF2/EKF3, with 3/-2 degrees of trim, at high attitude angles) and the rotation matrix by 0.05 degrees. For DCM the derived matrix is also now exactly orthonormal rather than being the renormalized integration matrix.
Mirrors the existing rotation-matrix accessor. In Replay the quaternion is reconstructed from the trim logged in RFRN, so existing logs replay without change.
AP_AHRS no longer consumes euler angles from the EKFs. The remaining internal consumers - NKF1/XKF1 logging and the lane-switch yaw reset accounting - now derive euler angles the same way AP_AHRS does: compose the attitude quaternion with the trim quaternion and convert, so logged eulers are in the same (vehicle-body) frame as ATT and can never disagree with the quaternion-derived attitude. This also replaces the previous euler-component subtraction of trim, which was only a first-order approximation; with 3/-2 degrees of trim it was wrong by up to 3.8 degrees at high attitude angles.
AP_AHRS no longer consumes euler angles from the EKFs. The remaining internal consumers - NKF1/XKF1 logging and the lane-switch yaw reset accounting - now derive euler angles the same way AP_AHRS does: compose the attitude quaternion with the trim quaternion and convert, so logged eulers are in the same (vehicle-body) frame as ATT and can never disagree with the quaternion-derived attitude. This also replaces the previous euler-component subtraction of trim, which was only a first-order approximation; with 3/-2 degrees of trim it was wrong by up to 3.8 degrees at high attitude angles.
This was the last consumer of getRotationBodyToNED, which took a detour through a rotation matrix purely to apply trim; apply the trim quaternion instead, as AP_AHRS does, and remove getRotationBodyToNED from the cores and frontends.
This was the last consumer of getRotationBodyToNED, which took a detour through a rotation matrix purely to apply trim; apply the trim quaternion instead, as AP_AHRS does, and remove getRotationBodyToNED from the cores and frontends.
The EKFs now apply trim via the quaternion form only.
The rotation matrix is now derived from the backend quaternion, so converting it back to a quaternion was a round trip; return the stored quaternion directly.
AP_AHRS now applies the board-mounting trim to a backend's attitude by composing its quaternion with from_euler(trim).inverse() and deriving the rotation matrix from the result, rather than keeping a separately-trimmed matrix. Assert that this yields the same rotation matrix as the previous form, R_backend * rotation_vehicle_body_to_autopilot_body, across a range of attitudes and a trim that is non-zero on all three axes at once.
Previous review (2026-10-15)Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head Re-reviewed at f88784b (previously 68ba0d9). Only the Plane AHRSTrimMission commit changed; the other 12 commits are identical. Nothing newer is covered. No blocking issues. The new waypoints fix the last point: the square now flies real corner turns, and the test still discriminates. It passes 2/2 here at 0.19/0.30 deg mean banked pitch/yaw error and fails 2/2 against the merge-base libraries at 1.28/3.17 deg. The quaternion gtest passes 13/13. One small comment nit remains in AHRSTrimMission. Item 6 becomes current before the turn at item 5 starts. fly_home_land_and_disarm() then loads flaps.txt about 0.7 s later, so the heading is still about 270 deg and the bank only reaches 12.9 deg. The square therefore makes three corner turns, not four. '# a square circuit (four turns)' (https://github.com/ardupilot/ardupilot/pull/34564/files#diff-df4815fbb326217ce097d5db86c3945dfce9ee5fe915980c9705fa99c8bd4c2cR3723) and 'the turns at the four corners (items 2 to 5)' (https://github.com/ardupilot/ardupilot/pull/34564/files#diff-df4815fbb326217ce097d5db86c3945dfce9ee5fe915980c9705fa99c8bd4c2cR3735) overstate it by one. The check itself is fine. Optional: say three corner turns, or wait until item 5's turn is done. QAUTOTUNE: in this round it passed 6/6 at this head and 5/6 on the merge base, so it looks marginal on both rather than broken by this PR. Your note about landing #33628 first covers it. CI had not reached it at this head when checked. The replay-trim timing note (https://github.com/ardupilot/ardupilot/pull/34564/files#diff-9b93c10628938da0dc08557a4c945dd1751bff6ca24e33aa33f39095b8dd2248R89) is unchanged and still informational only. Not exercised: a hardware flight with non-zero AHRS_TRIM, ExternalAHRS on real hardware, embedded builds at this head, and full vehicle suites. |
Exercise the trimmed attitude - published by the backends and feeding the attitude and navigation controllers - through turns: fly a square AUTO circuit and the flaps.txt landing with a board pitch mounting offset corrected by AHRS_TRIM_Y, then check the logged ATT against SIM truth while banked 20 degrees or more. Applying a single-axis trim correctly matters once banked, where it mixes into more than one euler axis.
f88784b to
6bbf8f3
Compare
|
Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head Re-reviewed at 6bbf8f3 (previously f88784b). Only the Plane AHRSTrimMission commit changed; the other 12 commits are identical. Nothing newer is covered. No blocking issues. The previous comment nit is fixed: the new wait_heading(0) (https://github.com/ardupilot/ardupilot/pull/34564/files#diff-df4815fbb326217ce097d5db86c3945dfce9ee5fe915980c9705fa99c8bd4c2cR3739) lets the fourth turn finish (yaw 270 to 349 deg at 53 deg bank) before the landing mission loads, so 'four turns' is now right. AHRSTrimMission passes 2/2 at 0.19/0.30-0.31 deg and fails against the merge-base libraries at 1.35/3.26 deg. The quaternion gtest passes 13/13. QAUTOTUNE passed 4/4 here at this head. Your table shows it is environment-dependent, though, so the stated merge order (#33628 first) still stands. That PR was still open when checked. The replay-trim timing note (https://github.com/ardupilot/ardupilot/pull/34564/files#diff-9b93c10628938da0dc08557a4c945dd1751bff6ca24e33aa33f39095b8dd2248R89) is unchanged and remains informational only. Not exercised: a hardware flight with non-zero AHRS_TRIM, ExternalAHRS on real hardware, embedded builds at this head, full vehicle suites, and CI at this head. Only the primary pass flew SITL; the other two passes made no flight checks. |
|
Flew this on the Hummer (tailsitter), 7" quad and a Bixler today. All good. |
Summary
Make the AHRS backends publish their attitude as a single quaternion and derive
the euler angles and rotation matrix from it, instead of each backend supplying
all three representations independently and applying the board-mounting trim in
its own way.
Classification & Testing (check all that apply and add your own)
Description
Each AHRS backend used to fill in the roll/pitch/yaw eulers, the rotation matrix
and the quaternion separately, and applied
AHRS_TRIMdifferently in each(
quaternion.rotate(-get_trim())in some paths, a trimmed matrix in others,euler subtraction elsewhere). Those forms could disagree. The trimmed rotation
matrix was exact, but the quaternion's
rotate(-trim)treated the trim as anaxis-angle rotation vector, so with a trim that is non-zero on more than one axis
the quaternion disagreed with the matrix. The EKF euler angles disagreed with both
for any non-zero trim, by up to 3.8 deg with a (3,-2,0) deg trim at large
attitudes.
This change makes the quaternion the single source of truth:
Estimates::quaternionand callsderive_attitude_from_quaternion(), which fills the euler angles and therotation matrix from that quaternion, so the three representations can never
disagree.
AHRS_TRIM(the autopilot-body to vehicle-body mounting offset) is applied bycomposing the backend quaternion with a trim quaternion,
get_quat_vehicle_body_to_autopilot_body()(=from_euler(trim).inverse()),which is exact for simultaneous trim on all three axes. This reproduces the
historical trimmed rotation matrix
(
R_backend * rotation_vehicle_body_to_autopilot_body) rather than the oldcomponent-wise euler subtraction.
EKF2/EKF3no longer exposegetEulerAngles()/getRotationBodyToNED()tothe AHRS layer;
getQuaternionBodyToNED()composes the EKF quaternion with theDAL trim quaternion, and the logged NKF1/XKF1 eulers are derived from it.
AP_DALcarries the trim as a quaternion and rebuilds it from the logged trimon replay, so replayed estimators see the same trim as the original flight.
get_quat_body_to_ned()returns the stored quaternion directly instead ofreconstructing it from the rotation matrix.
ExternalAHRS is unchanged in behaviour: it already supplies a vehicle-body
attitude, so it does not apply the trim composition.
What changes numerically
The new values are always the correct ones — they make the published eulers,
rotation matrix and quaternion mutually consistent, and consistent with DCM.
are DCM's euler angles.
AP_AHRS::get_quaternion(),ATTITUDE_QUATERNION):unchanged for a single-axis trim. With a trim that is non-zero on more than one
axis it changes for DCM, EKF2, EKF3 and SIM, because the old
quaternion.rotate(-trim)treated the trim as an axis-angle rotation vectorrather than as the euler rotation used for the matrices: 0.05 deg for a trim of
(3,-2,0) deg, 0.38 deg for (5,4,6), 0.87 deg for (10,10,0). SIM's matrix and
eulers move with it.
get_quat_body_to_ned()and the EKFgetQuaternionBodyToNED()were already derived from the exact matrix, so theyare unchanged for DCM and the EKFs.
AHRS_TRIM, including a single axis — not only multi-axis trim. For examplewith
AHRS_TRIM_Y=5°at 30° bank and level pitch the old EKF output wasroll 30.09 / pitch -0.67 / yaw +2.5; it is now 30 / 0 / 0. These eulers feed
the Plane attitude and navigation controllers, and a single-axis
AHRS_TRIM_Yis common on Plane, so this is behaviour worth noting even though the new
values are the correct ones.
Testing
SITL, all pass on this branch:
AP_Mathunit testQuaternionTest.TrimCompositionMatchesRotationMatrix:asserts the quaternion-composition trim gives the same rotation matrix as the
historical matrix form across a range of attitudes and a trim that is non-zero
on all three axes.
test.Plane.AHRSTrimMission: flies a Plane AUTO square circuit (four turns)and then the
flaps.txtlanding, with a 5 deg board pitch mounting offset(
SIM_BRD_TRIM_Y) corrected byAHRS_TRIM_Y. It then compares the logged ATTattitude with SIM truth wherever the bank is 20 deg or more. Mean pitch/yaw
error over about 385 banked samples is 0.20/0.30 deg on this branch, and
1.34/3.24 deg with the same test against master's code. The test fails above
0.75/1.5 deg, so it fails without this PR.
test.Copter.Replayincluding itsTrimsubtest (significantAHRS_TRIM):replayed estimators reproduce the trim-dependent outputs of the original
flight, exercising the AP_DAL trim-quaternion-on-replay path.
test.QuadPlane.QAUTOTUNEfails in CI on this PR. It is a marginal test ratherthan a behaviour change here. Autotune's position hold yaws the vehicle towards
its start point once it has drifted more than 5 m
(
AC_AutoTune::get_poshold_attitude_rad), and the level timeout can expireduring that turn, ending the tune with "AutoTune: Failed to level". In the pair of
logs examined, the attitude matched SIM truth throughout on both; this branch's
flight drifted to 5.4 m, where master's peaked at 5.0 m.
Whether a run crosses that threshold depends on the exact trajectory, which
rounding-level differences in the binary and harness timing change. Interleaved
runs of each build (number of runs failing with "Failed to level"):
--speedup100 (default), 20 runs--speedup30, 15 runsAC_AutoTunecommitAt the default speedup this branch's binary lands on the failing side far more
often, but at
--speedup30 it fails no more often than master. WithAHRS_TRIMzero, as in this test, that commit changes the published attitudeonly at rounding level. #33628 fixes the underlying level-timeout race, and
with it applied the test passes 20/20 on this branch. Merging #33628 first (or
carrying its commit) would clear this CI failure.
Still recommended before merge: a hardware flight with a non-zero
AHRS_TRIM_Ythrough turns.
Companion PR #34563 stacks temporary DEBUG instrumentation on top of this one (a
QVALlog of the divergence between each backend's supplied andquaternion-derived attitude) plus data-gathering autotests, so the validation can
be reviewed separately from this change. On this branch that divergence is 0.000
deg for every backend (they are derived from the same quaternion); on master the
same instrumentation shows the pre-change divergence, up to ~12 deg in the EKF
logged eulers with a large multi-axis trim.
This PR was AI-assisted (Claude). The human author has reviewed and is
responsible for the change.