PX4 Firmware
PX4 Autopilot Software http://px4.io
Dual.hpp
Go to the documentation of this file.
1 /**
2  * @file Dual.hpp
3  *
4  * This is a dual number type for calculating automatic derivatives.
5  *
6  * Based roughly on the methods described in:
7  * Automatic Differentiation, C++ Templates and Photogrammetry, by Dan Piponi
8  * and
9  * Ceres Solver's excellent Jet.h
10  *
11  * @author Julian Kent <julian@auterion.com>
12  */
13 
14 #pragma once
15 
16 #include "math.hpp"
17 
18 namespace matrix
19 {
20 
21 template <typename Type, size_t M>
22 class Vector;
23 
24 template <typename Scalar, size_t N>
25 struct Dual
26 {
27  static constexpr size_t WIDTH = N;
28 
31 
32  Dual() = default;
33 
34  explicit Dual(Scalar v, size_t inputDimension = 65535)
35  {
36  value = v;
37  if (inputDimension < N) {
38  derivative(inputDimension) = Scalar(1);
39  }
40  }
41 
42  explicit Dual(Scalar v, const Vector<Scalar, N>& d) :
43  value(v), derivative(d)
44  {}
45 
47  {
48  derivative.setZero();
49  value = a;
50  return *this;
51  }
52 
54  {
55  return (*this = *this + a);
56  }
57 
59  {
60  return (*this = *this * a);
61  }
62 
64  {
65  return (*this = *this - a);
66  }
67 
69  {
70  return (*this = *this / a);
71  }
72 
74  {
75  return (*this = *this + a);
76  }
77 
79  {
80  return (*this = *this - a);
81  }
82 
84  {
85  return (*this = *this * a);
86  }
87 
89  {
90  return (*this = *this / a);
91  }
92 
93 };
94 
95 // operators
96 
97 template <typename Scalar, size_t N>
99 {
100  return a;
101 }
102 
103 template <typename Scalar, size_t N>
105 {
106  return Dual<Scalar, N>(-a.value, -a.derivative);
107 }
108 
109 template <typename Scalar, size_t N>
111 {
112  return Dual<Scalar, N>(a.value + b.value, a.derivative + b.derivative);
113 }
114 
115 template <typename Scalar, size_t N>
117 {
118  return a + (-b);
119 }
120 
121 template <typename Scalar, size_t N>
123 {
124  return Dual<Scalar, N>(a.value + b, a.derivative);
125 }
126 
127 template <typename Scalar, size_t N>
129 {
130  return a + (-b);
131 }
132 
133 template <typename Scalar, size_t N>
135 {
136  return Dual<Scalar, N>(a + b.value, b.derivative);
137 }
138 
139 template <typename Scalar, size_t N>
141 {
142  return a + (-b);
143 }
144 
145 template <typename Scalar, size_t N>
147 {
148  return Dual<Scalar, N>(a.value * b.value, a.value * b.derivative + b.value * a.derivative);
149 }
150 
151 template <typename Scalar, size_t N>
153 {
154  return Dual<Scalar, N>(a.value * b, a.derivative * b);
155 }
156 
157 template <typename Scalar, size_t N>
159 {
160  return b * a;
161 }
162 
163 template <typename Scalar, size_t N>
165 {
166  const Scalar inv_b_real = Scalar(1) / b.value;
167  return Dual<Scalar, N>(a.value * inv_b_real, a.derivative * inv_b_real -
168  a.value * b.derivative * inv_b_real * inv_b_real);
169 }
170 
171 template <typename Scalar, size_t N>
173 {
174  return a * (Scalar(1) / b);
175 }
176 
177 template <typename Scalar, size_t N>
179 {
180  const Scalar inv_b_real = Scalar(1) / b.value;
181  return Dual<Scalar, N>(a * inv_b_real, (-inv_b_real * a * inv_b_real) * b.derivative);
182 }
183 
184 // basic math
185 
186 // sqrt
187 template <typename Scalar, size_t N>
189 {
190  Scalar real = sqrt(a.value);
191  return Dual<Scalar, N>(real, a.derivative * (Scalar(1) / (Scalar(2) * real)));
192 }
193 
194 // abs
195 template <typename Scalar, size_t N>
197 {
198  return a.value >= Scalar(0) ? a : -a;
199 }
200 
201 // ceil
202 template <typename Scalar, size_t N>
204 {
205  return Dual<Scalar, N>(ceil(a.value));
206 }
207 
208 // floor
209 template <typename Scalar, size_t N>
211 {
212  return Dual<Scalar, N>(floor(a.value));
213 }
214 
215 // fmod
216 template <typename Scalar, size_t N>
218 {
219  return Dual<Scalar, N>(a.value - Scalar(size_t(a.value / mod)) * mod, a.derivative);
220 }
221 
222 // max
223 template <typename Scalar, size_t N>
225 {
226  return a.value >= b.value ? a : b;
227 }
228 
229 // min
230 template <typename Scalar, size_t N>
232 {
233  return a.value < b.value ? a : b;
234 }
235 
236 // isnan
237 template <typename Scalar>
238 bool IsNan(Scalar a)
239 {
240  return isnan(a);
241 }
242 
243 template <typename Scalar, size_t N>
244 bool IsNan(const Dual<Scalar, N>& a)
245 {
246  return IsNan(a.value);
247 }
248 
249 // isfinite
250 template <typename Scalar>
252 {
253  return isfinite(a);
254 }
255 
256 template <typename Scalar, size_t N>
257 bool IsFinite(const Dual<Scalar, N>& a)
258 {
259  return IsFinite(a.value);
260 }
261 
262 // isinf
263 template <typename Scalar>
264 bool IsInf(Scalar a)
265 {
266  return isinf(a);
267 }
268 
269 template <typename Scalar, size_t N>
270 bool IsInf(const Dual<Scalar, N>& a)
271 {
272  return IsInf(a.value);
273 }
274 
275 // trig
276 
277 // sin
278 template <typename Scalar, size_t N>
280 {
281  return Dual<Scalar, N>(sin(a.value), cos(a.value) * a.derivative);
282 }
283 
284 // cos
285 template <typename Scalar, size_t N>
287 {
288  return Dual<Scalar, N>(cos(a.value), -sin(a.value) * a.derivative);
289 }
290 
291 // tan
292 template <typename Scalar, size_t N>
294 {
295  Scalar real = tan(a.value);
296  return Dual<Scalar, N>(real, (Scalar(1) + real * real) * a.derivative);
297 }
298 
299 // asin
300 template <typename Scalar, size_t N>
302 {
303  Scalar asin_d = Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
304  return Dual<Scalar, N>(asin(a.value), asin_d * a.derivative);
305 }
306 
307 // acos
308 template <typename Scalar, size_t N>
310 {
311  Scalar acos_d = -Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
312  return Dual<Scalar, N>(acos(a.value), acos_d * a.derivative);
313 }
314 
315 // atan
316 template <typename Scalar, size_t N>
318 {
319  Scalar atan_d = Scalar(1) / (Scalar(1) + a.value * a.value);
320  return Dual<Scalar, N>(atan(a.value), atan_d * a.derivative);
321 }
322 
323 // atan2
324 template <typename Scalar, size_t N>
326 {
327  // derivative is equal to that of atan(a/b), so substitute a/b into atan and simplify
328  Scalar atan_d = Scalar(1) / (a.value * a.value + b.value * b.value);
329  return Dual<Scalar, N>(atan2(a.value, b.value), (a.derivative * b.value - a.value * b.derivative) * atan_d);
330 }
331 
332 // retrieve the derivative elements of a vector of Duals into a matrix
333 template <typename Scalar, size_t M, size_t N>
335 {
337  for (size_t i = 0; i < M; i++) {
338  jac.row(i) = input(i, 0).derivative;
339  }
340  return jac;
341 }
342 
343 // retrieve the real (non-derivative) elements of a matrix of Duals into an equally sized matrix
344 template <typename Scalar, size_t M, size_t N, size_t D>
346 {
348  for (size_t i = 0; i < M; i++) {
349  for (size_t j = 0; j < N; j++) {
350  r(i,j) = input(i,j).value;
351  }
352  }
353  return r;
354 }
355 
356 #if defined(SUPPORT_STDIOSTREAM)
357 template<typename Type, size_t N>
358 std::ostream& operator<<(std::ostream& os,
359  const matrix::Dual<Type, N>& dual)
360 {
361  os << "[";
362  os << std::setw(10) << dual.value << ";";
363  for (size_t j = 0; j < N; ++j) {
364  os << "\t";
365  os << std::setw(10) << static_cast<double>(dual.derivative(j));
366  }
367  os << "]";
368  return os;
369 }
370 #endif // defined(SUPPORT_STDIOSTREAM)
371 
372 }
373 
Dual< Scalar, N > acos(const Dual< Scalar, N > &a)
Definition: Dual.hpp:309
Dual< Scalar, N > atan(const Dual< Scalar, N > &a)
Definition: Dual.hpp:317
Dual< Scalar, N > & operator=(const Scalar &a)
Definition: Dual.hpp:46
Dual< Scalar, N > operator-(const Dual< Scalar, N > &a)
Definition: Dual.hpp:104
Dual< Scalar, N > ceil(const Dual< Scalar, N > &a)
Definition: Dual.hpp:203
Dual< Scalar, N > asin(const Dual< Scalar, N > &a)
Definition: Dual.hpp:301
Dual< Scalar, N > & operator-=(const Dual< Scalar, N > &a)
Definition: Dual.hpp:63
Dual(Scalar v, size_t inputDimension=65535)
Definition: Dual.hpp:34
static constexpr size_t WIDTH
Definition: Dual.hpp:27
Vector< Scalar, N > derivative
Definition: Dual.hpp:30
Dual< Scalar, N > cos(const Dual< Scalar, N > &a)
Definition: Dual.hpp:286
Dual(Scalar v, const Vector< Scalar, N > &d)
Definition: Dual.hpp:42
Matrix< Scalar, M, N > collectReals(const Matrix< Dual< Scalar, D >, M, N > &input)
Definition: Dual.hpp:345
Dual< Scalar, N > min(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:231
Dual< Scalar, N > operator/(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:164
Dual< Scalar, N > tan(const Dual< Scalar, N > &a)
Definition: Dual.hpp:293
Dual< Scalar, N > & operator+=(const Dual< Scalar, N > &a)
Definition: Dual.hpp:53
Dual< Scalar, N > & operator*=(const Dual< Scalar, N > &a)
Definition: Dual.hpp:58
Dual< Scalar, N > operator*(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:146
Dual< Scalar, N > & operator/=(const Dual< Scalar, N > &a)
Definition: Dual.hpp:68
Dual< Scalar, N > fmod(const Dual< Scalar, N > &a, Scalar mod)
Definition: Dual.hpp:217
Dual< Scalar, N > operator+(const Dual< Scalar, N > &a)
Definition: Dual.hpp:98
const Slice< Type, 1, N, M, N > row(size_t i) const
Definition: Matrix.hpp:385
Dual< Scalar, N > max(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:224
Scalar value
Definition: Dual.hpp:29
bool IsNan(Scalar a)
Definition: Dual.hpp:238
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Definition: Dual.hpp:196
Dual()=default
Dual< Scalar, N > sin(const Dual< Scalar, N > &a)
Definition: Dual.hpp:279
Dual< Scalar, N > floor(const Dual< Scalar, N > &a)
Definition: Dual.hpp:210
Matrix< Scalar, M, N > collectDerivatives(const Matrix< Dual< Scalar, N >, M, 1 > &input)
Definition: Dual.hpp:334
bool IsInf(Scalar a)
Definition: Dual.hpp:264
Dual< Scalar, N > sqrt(const Dual< Scalar, N > &a)
Definition: Dual.hpp:188
Dual< Scalar, N > atan2(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:325
bool IsFinite(Scalar a)
Definition: Dual.hpp:251