Skip to content

Commit 471c584

Browse files
ronisbrronisbr
committed
🐛 Fix near-gimbal-lock failure for Float32
1 parent e61a7e2 commit 471c584

2 files changed

Lines changed: 59 additions & 24 deletions

File tree

‎src/conversions/dcm_to_angle.jl‎

Lines changed: 36 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -61,10 +61,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
6161

6262
if rot_seq == :ZYX
6363
# Check for singularities.
64-
if !(abs(dcm[1, 3]) ≥ one(Tf) - eps(Tf))
64+
h = hypot(dcm[1, 1], dcm[1, 2])
65+
if h > eps(Tf)
6566
return EulerAngles{Tf}(
6667
_mod_atan(+dcm[1, 2], +dcm[1, 1]),
67-
_mod_asin(-dcm[1, 3]),
68+
_mod_atan(-dcm[1, 3], h),
6869
_mod_atan(+dcm[2, 3], +dcm[3, 3]),
6970
rot_seq,
7071
)
@@ -75,10 +76,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
7576
end
7677
elseif rot_seq == :XYX
7778
# Check for singularities.
78-
if !(abs(dcm[1, 1]) ≥ one(Tf) - eps(Tf))
79+
h = hypot(dcm[1, 2], dcm[1, 3])
80+
if h > eps(Tf)
7981
return EulerAngles{Tf}(
8082
_mod_atan(+dcm[1, 2], -dcm[1, 3]),
81-
_mod_acos(+dcm[1, 1]),
83+
_mod_atan(h, +dcm[1, 1]),
8284
_mod_atan(+dcm[2, 1], +dcm[3, 1]),
8385
rot_seq,
8486
)
@@ -89,10 +91,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
8991
end
9092
elseif rot_seq == :XYZ
9193
# Check for singularities.
92-
if !(abs(dcm[3, 1]) ≥ one(Tf) - eps(Tf))
94+
h = hypot(dcm[3, 2], dcm[3, 3])
95+
if h > eps(Tf)
9396
return EulerAngles{Tf}(
9497
_mod_atan(-dcm[3, 2], +dcm[3, 3]),
95-
_mod_asin(+dcm[3, 1]),
98+
_mod_atan(+dcm[3, 1], h),
9699
_mod_atan(-dcm[2, 1], +dcm[1, 1]),
97100
rot_seq,
98101
)
@@ -103,10 +106,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
103106
end
104107
elseif rot_seq == :XZX
105108
# Check for singularities.
106-
if !(abs(dcm[1, 1]) ≥ one(Tf) - eps(Tf))
109+
h = hypot(dcm[1, 2], dcm[1, 3])
110+
if h > eps(Tf)
107111
return EulerAngles{Tf}(
108112
_mod_atan(+dcm[1, 3], +dcm[1, 2]),
109-
_mod_acos(+dcm[1, 1]),
113+
_mod_atan(h, +dcm[1, 1]),
110114
_mod_atan(+dcm[3, 1], -dcm[2, 1]),
111115
rot_seq,
112116
)
@@ -117,10 +121,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
117121
end
118122
elseif rot_seq == :XZY
119123
# Check for singularities.
120-
if !(abs(dcm[2, 1]) ≥ one(Tf) - eps(Tf))
124+
h = hypot(dcm[2, 2], dcm[2, 3])
125+
if h > eps(Tf)
121126
return EulerAngles{Tf}(
122127
_mod_atan(+dcm[2, 3], +dcm[2, 2]),
123-
_mod_asin(-dcm[2, 1]),
128+
_mod_atan(-dcm[2, 1], h),
124129
_mod_atan(+dcm[3, 1], +dcm[1, 1]),
125130
rot_seq,
126131
)
@@ -131,10 +136,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
131136
end
132137
elseif rot_seq == :YXY
133138
# Check for singularities.
134-
if !(abs(dcm[2, 2]) ≥ one(Tf) - eps(Tf))
139+
h = hypot(dcm[2, 1], dcm[2, 3])
140+
if h > eps(Tf)
135141
return EulerAngles{Tf}(
136142
_mod_atan(+dcm[2, 1], +dcm[2, 3]),
137-
_mod_acos(+dcm[2, 2]),
143+
_mod_atan(h, +dcm[2, 2]),
138144
_mod_atan(+dcm[1, 2], -dcm[3, 2]),
139145
rot_seq,
140146
)
@@ -144,10 +150,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
144150
)
145151
end
146152
elseif rot_seq == :YXZ
147-
if !(abs(dcm[3, 2]) ≥ one(Tf) - eps(Tf))
153+
h = hypot(dcm[3, 1], dcm[3, 3])
154+
if h > eps(Tf)
148155
return EulerAngles{Tf}(
149156
_mod_atan(+dcm[3, 1], +dcm[3, 3]),
150-
_mod_asin(-dcm[3, 2]),
157+
_mod_atan(-dcm[3, 2], h),
151158
_mod_atan(+dcm[1, 2], +dcm[2, 2]),
152159
rot_seq,
153160
)
@@ -158,10 +165,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
158165
end
159166
elseif rot_seq == :YZX
160167
# Check for singularities.
161-
if !(abs(dcm[1, 2]) ≥ one(Tf) - eps(Tf))
168+
h = hypot(dcm[1, 1], dcm[1, 3])
169+
if h > eps(Tf)
162170
return EulerAngles{Tf}(
163171
_mod_atan(-dcm[1, 3], +dcm[1, 1]),
164-
_mod_asin(+dcm[1, 2]),
172+
_mod_atan(+dcm[1, 2], h),
165173
_mod_atan(-dcm[3, 2], +dcm[2, 2]),
166174
rot_seq,
167175
)
@@ -172,10 +180,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
172180
end
173181
elseif rot_seq == :YZY
174182
# Check for singularities.
175-
if !(abs(dcm[2, 2]) ≥ one(Tf) - eps(Tf))
183+
h = hypot(dcm[2, 1], dcm[2, 3])
184+
if h > eps(Tf)
176185
return EulerAngles{Tf}(
177186
_mod_atan(+dcm[2, 3], -dcm[2, 1]),
178-
_mod_acos(+dcm[2, 2]),
187+
_mod_atan(h, +dcm[2, 2]),
179188
_mod_atan(+dcm[3, 2], +dcm[1, 2]),
180189
rot_seq,
181190
)
@@ -186,10 +195,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
186195
end
187196
elseif rot_seq == :ZXY
188197
# Check for singularities.
189-
if !(abs(dcm[2, 3]) ≥ one(Tf) - eps(Tf))
198+
h = hypot(dcm[2, 1], dcm[2, 2])
199+
if h > eps(Tf)
190200
return EulerAngles{Tf}(
191201
_mod_atan(-dcm[2, 1], +dcm[2, 2]),
192-
_mod_asin(+dcm[2, 3]),
202+
_mod_atan(+dcm[2, 3], h),
193203
_mod_atan(-dcm[1, 3], +dcm[3, 3]),
194204
rot_seq,
195205
)
@@ -200,10 +210,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
200210
end
201211
elseif rot_seq == :ZXZ
202212
# Check for singularities.
203-
if !(abs(dcm[3, 3]) ≥ one(Tf) - eps(Tf))
213+
h = hypot(dcm[3, 1], dcm[3, 2])
214+
if h > eps(Tf)
204215
return EulerAngles{Tf}(
205216
_mod_atan(+dcm[3, 1], -dcm[3, 2]),
206-
_mod_acos(+dcm[3, 3]),
217+
_mod_atan(h, +dcm[3, 3]),
207218
_mod_atan(+dcm[1, 3], +dcm[2, 3]),
208219
rot_seq,
209220
)
@@ -214,10 +225,11 @@ function dcm_to_angle(dcm::DCM{T}, rot_seq::Symbol = :ZYX) where {T <: Number}
214225
end
215226
elseif rot_seq == :ZYZ
216227
# Check for singularities.
217-
if !(abs(dcm[3, 3]) ≥ one(Tf) - eps(Tf))
228+
h = hypot(dcm[3, 1], dcm[3, 2])
229+
if h > eps(Tf)
218230
return EulerAngles{Tf}(
219231
_mod_atan(+dcm[3, 2], +dcm[3, 1]),
220-
_mod_acos(+dcm[3, 3]),
232+
_mod_atan(h, +dcm[3, 3]),
221233
_mod_atan(+dcm[2, 3], -dcm[1, 3]),
222234
rot_seq,
223235
)

‎test/conversions/dcm_to_angle.jl‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -166,6 +166,29 @@ end
166166
end
167167
end
168168

169+
@testset "DCM => Euler angles (near gimbal lock)" begin
170+
# Its sine lies within eps(Float32) of one, while its cosine is large enough
171+
# to distinguish it from an exact singularity.
172+
T = Float32
173+
a₁, a₂, a₃ = T(-0.86634374), -T(π / 2) + T(0.0004136), T(1.6752636)
174+
175+
for rot_seq in (:XYZ, :XZY, :YXZ, :YZX, :ZXY, :ZYX)
176+
ea = dcm_to_angle(angle_to_dcm(a₁, a₂, a₃, rot_seq), rot_seq)
177+
@test ea.a1 ≈ a₁ atol = 50 * eps(T)
178+
@test ea.a2 ≈ a₂ atol = 50 * eps(T)
179+
@test ea.a3 ≈ a₃ atol = 50 * eps(T)
180+
end
181+
182+
for a₂ in (T(0.0004136), T(π) - T(0.0004136))
183+
for rot_seq in (:XYX, :XZX, :YXY, :YZY, :ZXZ, :ZYZ)
184+
ea = dcm_to_angle(angle_to_dcm(a₁, a₂, a₃, rot_seq), rot_seq)
185+
@test ea.a1 ≈ a₁ atol = 50 * eps(T)
186+
@test ea.a2 ≈ a₂ atol = 50 * eps(T)
187+
@test ea.a3 ≈ a₃ atol = 50 * eps(T)
188+
end
189+
end
190+
end
191+
169192
@testset "DCM => Euler angles (generic numeric stability)" begin
170193
for T in (Int, Rational{Int}, Float32, Float64, BigFloat)
171194
Tf = float(T)

0 commit comments

Comments
 (0)