fix(matrix): clamp asin() domain in Euler(Dcm) to prevent NaN pitch near gimbal lock#27872
Open
94xhn wants to merge 3 commits into
Open
fix(matrix): clamp asin() domain in Euler(Dcm) to prevent NaN pitch near gimbal lock#2787294xhn wants to merge 3 commits into
94xhn wants to merge 3 commits into
Conversation
…ear gimbal lock
Euler(const Dcm<Type> &dcm) computes theta() as:
theta() = std::asin(-dcm(2, 0));
In exact arithmetic, -dcm(2, 0) is always within [-1, 1] for a proper
rotation matrix. In floating point, quaternion normalization and other
rounding can push it a ULP or two outside that range, which makes
asin() return NaN. This is most likely to happen near the +-90 degree
pitch singularity (gimbal lock) -- an attitude tailsitter VTOL
vehicles fly through on every hover<->forward-flight transition, and
one that can also occur transiently for any vehicle during aggressive
maneuvers.
This is the same class of issue already guarded against for acos() in
MapProjection::project() (src/lib/geo/geo.cpp), which clamps its
argument to [-1, 1] before calling acos() for exactly this reason;
Euler(Dcm)'s asin() call had no equivalent guard.
Once theta() is NaN, it also defeats the near-gimbal-lock special
case just below it: both `fabs(theta() - pi/2) < 1e-3` and
`fabs(theta() + pi/2) < 1e-3` are false for a NaN theta() (NaN
comparisons are always false), so phi()/psi() silently fall through
to the general-case atan2() formulas instead of the intended
gimbal-lock handling.
Real-world consequence: src/modules/commander/failure_detector/
FailureDetector.cpp reads pitch via Eulerf(Quatf(attitude.q)).theta()
and compares fabsf(pitch) > max_pitch to decide whether to trigger the
FD_FAIL_P attitude failure detector. If pitch is NaN, that comparison
is always false, so a genuinely extreme pitch sample can silently fail
to trip the failure detector. The same file explicitly composes a 90
degree pitch rotation for its tailsitter handling a few lines below,
confirming near-gimbal-lock attitudes are a normal operating condition
for a vehicle class PX4 supports, not just a theoretical edge case.
Fix: clamp the asin() argument to [-1, 1] before calling asin(),
mirroring the existing acos() guard in project().
Test plan:
- Added MatrixAttitudeTest.EulerFromDcmAsinDomainNearGimbalLock to
src/lib/matrix/test/MatrixAttitudeTest.cpp: constructs a unit
quaternion representing a small, physically realistic perturbation
away from an exact 90 degree pitch attitude (the kind of floating
point noise ordinary quaternion normalization produces), asserts the
resulting DCM(2,0) element does exceed unit magnitude (so the test
actually exercises the domain edge instead of passing vacuously),
then asserts Euler(dcm).theta() is finite and equals pi/2.
- Compiled and ran the real, unmodified test file directly against
the standalone src/lib/matrix headers with system gtest (Ubuntu
24.04, g++ 13.3.0), stubbing only the generated
px4_platform_common/defines.h dependency (a handful of float Pi
literals, copied verbatim from platforms/common/include/
px4_platform_common/defines.h, needed only because that header
otherwise pulls in a board-config file this standalone build does
not generate):
- Against the pre-fix Euler.hpp: the new regression test FAILS
with theta() == nan, and the existing MatrixAttitudeTest.Attitude
test still passes (confirming the bug was previously uncovered
by the existing suite).
- Against the fix: both tests PASS, and theta() == 1.5707963...
(pi/2) as expected.
- Also verified with a standalone reproduction program that links the
real (unmodified) src/lib/matrix headers directly and reproduces the
FailureDetector-style comparison: before the fix, fabsf(nan) >
max_pitch evaluates to false (failure NOT flagged) for a vehicle at
a genuinely extreme ~90 degree pitch; after the fix it correctly
evaluates to true (failure flagged).
Signed-off-by: yi chen <94xhn1@gmail.com>
github-actions
Bot
added
kind:bug
The previous commit's asin() domain clamp used std::fmin(std::fmax(...)), which does not compile on the NuttX cross-compiler toolchain (its stripped libcxx headers under -nostdinc++ do not provide std::fmin/ std::fmax), breaking every NuttX-targeted CI build. Use the library's own matrix::constrain() helper (already used throughout mathlib for the same min/upper-bound clamping pattern) instead, which avoids the extra <cmath> functions entirely. Signed-off-by: yi chen <94xhn1@gmail.com>
![github-actions[bot]](https://avatars.githubusercontent.com/in/15368?s=60&v=4){kind=small}
| // e.g. tailsitter VTOL vehicles fly through during every transition. | ||
| // This mirrors the equivalent acos() guard already used for the same | ||
| // reason in MapProjection::project() (src/lib/geo/geo.cpp). | ||
| const Type sin_theta = constrain(-dcm(2, 0), Type(-1), Type(1)); |
Contributor
There was a problem hiding this comment.
[error] clang-diagnostic-error [error]
no matching function for call to constrain
….hpp The previous commit (a0c9882) replaced std::fmin/std::fmax with an unqualified constrain() call, assuming it lived directly in namespace matrix. It actually lives one level deeper, in matrix::typeFunction (see Matrix.hpp's own typeFunction::max/min usage and MatrixAssignmentTest.cpp), so the unqualified call did not resolve and broke every CI build target with the same compile error. Euler.hpp also did not include Matrix.hpp itself, relying on whatever aggregator (math.hpp) happened to include it — math.hpp includes Euler.hpp before Matrix.hpp, so even a qualified call could depend on include order in other translation units that pull in Euler.hpp directly. Fix: qualify the call as typeFunction::constrain(...), matching the existing convention, and include "Matrix.hpp" directly so the fix does not depend on transitive include order. Verified locally with a standalone harness (matrix/math.hpp + Quaternion -> Dcm -> Euler construction, including the near +-90 degree pitch singularity branch) compiled with g++ -std=c++17. Signed-off-by: yi chen <94xhn1@gmail.com>
| @@ -83,7 +87,17 @@ class Euler : public Vector<Type, 3> | |||
| */ | |||
| Euler(const Dcm<Type> &dcm) | |||
Contributor
There was a problem hiding this comment.
[error] clang-diagnostic-error [error]
no template named Dcm
Contributor
🔎 FLASH Analysispx4_fmu-v5x [Total VM Diff: 96 byte (0 %)]px4_fmu-v6x [Total VM Diff: -40 byte (-0 %)]Updated: 2026-07-12T18:38:12 |
Contributor
|
Making shorter, to the point, well thought out comments (or just none where they are not needed) and PR description will not only be appreciated by anyone who reads it but also build your own understanding much more effectively than copying claude output |
This file contains hidden or 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
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
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.
Problem
Euler(const Dcm<Type> &dcm)computes pitch as:where
dcm(2, 0) = 2 * (b*d - a*c)is built from a unit quaternion inDcm(const Quaternion<Type>&)(src/lib/matrix/matrix/Dcm.hpp). In exact arithmetic-dcm(2, 0)is always within[-1, 1]for a proper rotation matrix, butfloatrounding from quaternion normalization (and other operations) can push it a ULP or two outside that range, which makesasin()returnNaN. A targeted perturbation sweep around a 90-degree-pitch attitude (small random perturbations of a unit quaternion, renormalized exactly asQuaternion::normalize()would) showed this happens for roughly 15% of such perturbations — i.e. this is not a rare corner case near the pitch singularity, it's a fairly common one.Once
theta()isNaN, it also defeats the near-gimbal-lock special case immediately below it: bothfabs(theta() - M_PI/2) < 1e-3andfabs(theta() + M_PI/2) < 1e-3are false whentheta()isNaN(NaN comparisons are always false), sophi()/psi()silently fall through to the general-caseatan2()formulas instead of the intended gimbal-lock handling.Real-world consequence:
src/modules/commander/failure_detector/FailureDetector.cppreads pitch viaEulerf(Quatf(attitude.q)).theta()and comparesfabsf(pitch) > max_pitchto drive theFD_FAIL_Pattitude failure detector. If pitch isNaN, that comparison is alwaysfalse, so a genuinely extreme pitch sample can silently fail to trip the failure detector. The same file explicitly composes a 90-degree pitch rotation for tailsitter handling a few lines below, confirming near-gimbal-lock attitudes are a normal operating condition for a PX4-supported vehicle class (tailsitter VTOL, every hover<->forward-flight transition), not just a theoretical edge case.src/modules/airspeed_selector/AirspeedValidator.cppalso readsmatrix::Eulerf(att_q).theta()for its first-principle airspeed check; it already has a defensivePX4_ISFINITE(pitch)bailout, so the severity there is lower, but it's still an accuracy gap right at extreme pitch.Solution
Clamp the
asin()argument to[-1, 1]before calling it, mirroring the existingacos()guard already used for exactly this reason inMapProjection::project()(src/lib/geo/geo.cpp):This is the identity for any legitimately in-range input, so it changes nothing for normal attitudes — it only fixes the out-of-domain rounding case. Added
#include <cmath>since the header now usesstd::fmin/std::fmaxexplicitly rather than relying on transitive includes.Test plan
Added
MatrixAttitudeTest.EulerFromDcmAsinDomainNearGimbalLocktosrc/lib/matrix/test/MatrixAttitudeTest.cpp: constructs a unit quaternion representing a small, physically realistic perturbation away from an exact 90-degree-pitch attitude (the kind of floating-point noise ordinary quaternion normalization produces), asserts the resultingdcm(2, 0)does exceed unit magnitude (so the test actually exercises the domain edge instead of passing vacuously), then assertsEuler(dcm).theta()is finite and equalspi/2.I compiled and ran the real, unmodified test file directly against the standalone
src/lib/matrixheaders with system gtest 1.14 (WSL Ubuntu 24.04, g++ 13.3.0), stubbing only the generatedpx4_platform_common/defines.hdependency (a handful of float Pi literals copied verbatim from the real header, needed only because it otherwise pulls in a generated board-config file a standalone build doesn't produce):Euler.hpp: the new regression test fails withtheta() == nan(Value of: std::isfinite(euler.theta()) / Actual: false / Expected: true), while the existingMatrixAttitudeTest.Attitudetest still passes — confirming the bug was previously uncovered by the existing suite.theta() == 1.5707963...(pi/2) as expected.I also built a standalone reproduction program (g++ 8.1.0 MinGW, C++17) linking the real, unmodified
matrix/*.hppheaders directly, that reproduces theFailureDetector-style comparison for a unit quaternion at a genuinely realistic ~90-degree pitch:dcm(2,0) = -1.000000119(magnitude > 1.0),euler.theta() = nan,phi() = -0.58(wrong — general-case fallthrough instead of the intended forced-zero gimbal-lock branch), andfabsf(pitch) > max_pitchevaluates tofalse(failure not flagged) despite the genuinely extreme pitch.euler.theta() = 1.570796371(pi/2, correct),phi()correctly forced to0(gimbal-lock branch taken), and the check correctly evaluates totrue(failure flagged).Disclosure
Generative AI (Claude) was used to help investigate this issue, implement, and verify this fix. All changes were reviewed by me before submission.