PX4 Firmware
PX4 Autopilot Software http://px4.io
dual.cpp
Go to the documentation of this file.
1 #include "test_macros.hpp"
2 #include <matrix/math.hpp>
3 #include <iostream>
4 
5 using namespace matrix;
6 
7 template <typename Scalar, size_t N>
9 {
10  return isEqualF(a.value, b.value) && a.derivative == b.derivative;
11 }
12 
13 template <typename T>
14 T testFunction(const Vector<T, 3>& point) {
15  // function is f(x,y,z) = x^2 + 2xy + 3y^2 + z
16  return point(0)*point(0)
17  + 2.f * point(0) * point(1)
18  + 3.f * point(1) * point(1)
19  + point(2);
20 }
21 
22 template <typename Scalar>
24  const Vector<Scalar, 3>& velocityStateBody,
25  const Quaternion<Scalar>& bodyOrientation,
26  const Vector<Scalar, 3>& positionMeasurement,
27  Scalar dt
28  )
29 {
30  return positionMeasurement - (positionState + bodyOrientation.conjugate(velocityStateBody) * dt);
31 }
32 
33 int main()
34 {
35  const Dual<float, 1> a(3,0);
36  const Dual<float, 1> b(6,0);
37 
38  {
39  TEST(isEqualF(a.value, 3.f));
40  TEST(isEqualF(a.derivative(0), 1.f));
41  }
42 
43  {
44  // addition
45  Dual<float, 1> c = a + b;
46  TEST(isEqualF(c.value, 9.f));
47  TEST(isEqualF(c.derivative(0), 2.f));
48 
49  Dual<float, 1> d = +a;
50  TEST(isEqualAll(d, a));
51  d += b;
52  TEST(isEqualAll(d, c));
53 
54  Dual<float, 1> e = a;
55  e += b.value;
56  TEST(isEqualF(e.value, c.value));
58 
59  Dual<float, 1> f = b.value + a;
60  TEST(isEqualAll(f, e));
61  }
62 
63  {
64  // subtraction
65  Dual<float, 1> c = b - a;
66  TEST(isEqualF(c.value, 3.f));
67  TEST(isEqualF(c.derivative(0), 0.f));
68 
69  Dual<float, 1> d = b;
70  TEST(isEqualAll(d, b));
71  d -= a;
72  TEST(isEqualAll(d, c));
73 
74  Dual<float, 1> e = b;
75  e -= a.value;
76  TEST(isEqualF(e.value, c.value));
78 
79  Dual<float, 1> f = a.value - b;
80  TEST(isEqualAll(f, -e));
81  }
82 
83  {
84  // multiplication
85  Dual<float, 1> c = a*b;
86  TEST(isEqualF(c.value, 18.f));
87  TEST(isEqualF(c.derivative(0), 9.f));
88 
89  Dual<float, 1> d = a;
90  TEST(isEqualAll(d, a));
91  d *= b;
92  TEST(isEqualAll(d, c));
93 
94  Dual<float, 1> e = a;
95  e *= b.value;
96  TEST(isEqualF(e.value, c.value));
97  TEST(isEqual(e.derivative, a.derivative * b.value));
98 
99  Dual<float, 1> f = b.value * a;
100  TEST(isEqualAll(f, e));
101  }
102 
103  {
104  // division
105  Dual<float, 1> c = b/a;
106  TEST(isEqualF(c.value, 2.f));
107  TEST(isEqualF(c.derivative(0), -1.f/3.f));
108 
109  Dual<float, 1> d = b;
110  TEST(isEqualAll(d, b));
111  d /= a;
112  TEST(isEqualAll(d, c));
113 
114  Dual<float, 1> e = b;
115  e /= a.value;
116  TEST(isEqualF(e.value, c.value));
118 
119  Dual<float, 1> f = a.value / b;
120  TEST(isEqualAll(f, 1.f/e));
121  }
122 
123  {
124  Dual<float, 1> blank;
125  TEST(isEqualF(blank.value, 0.f));
126  TEST(isEqualF(blank.derivative(0), 0.f));
127  }
128 
129  {
130  // sqrt
131  TEST(isEqualF(sqrt(a).value, sqrt(a.value)));
132  TEST(isEqualF(sqrt(a).derivative(0), 1.f/sqrt(12.f)));
133  }
134 
135  {
136  // abs
137  TEST(isEqualAll(a, abs(-a)));
138  TEST(!isEqualAll(-a, abs(a)));
139  TEST(isEqualAll(-a, -abs(a)));
140  }
141 
142  {
143  // ceil
144  Dual<float, 1> c(1.5,0);
145  TEST(isEqualF(ceil(c).value, ceil(c.value)));
146  TEST(isEqualF(ceil(c).derivative(0), 0.f));
147  }
148 
149  {
150  // floor
151  Dual<float, 1> c(1.5,0);
152  TEST(isEqualF(floor(c).value, floor(c.value)));
153  TEST(isEqualF(floor(c).derivative(0), 0.f));
154  }
155 
156  {
157  // fmod
158  TEST(isEqualF(fmod(a, 0.8f).value, fmod(a.value, 0.8f)));
159  TEST(isEqual(fmod(a, 0.8f).derivative, a.derivative));
160  }
161 
162  {
163  // max/min
164  TEST(isEqualAll(b, max(a, b)));
165  TEST(isEqualAll(a, min(a, b)));
166  }
167 
168  {
169  // isnan
170  TEST(!IsNan(a));
171  Dual<float, 1> c(sqrt(-1.f),0);
172  TEST(IsNan(c));
173  }
174 
175  {
176  // isfinite/isinf
177  TEST(IsFinite(a));
178  TEST(!IsInf(a));
179  Dual<float, 1> c(sqrt(-1.f),0);
180  TEST(!IsFinite(c));
181  TEST(!IsInf(c));
182  Dual<float, 1> d(INFINITY,0);
183  TEST(!IsFinite(d));
184  TEST(IsInf(d));
185  }
186 
187  {
188  // sin/cos/tan
189  TEST(isEqualF(sin(a).value, sin(a.value)));
190  TEST(isEqualF(sin(a).derivative(0), cos(a.value))); // sin'(x) = cos(x)
191 
192  TEST(isEqualF(cos(a).value, cos(a.value)));
193  TEST(isEqualF(cos(a).derivative(0), -sin(a.value))); // cos'(x) = -sin(x)
194 
195  TEST(isEqualF(tan(a).value, tan(a.value)));
196  TEST(isEqualF(tan(a).derivative(0), 1.f + tan(a.value)*tan(a.value))); // tan'(x) = 1 + tan^2(x)
197  }
198 
199  {
200  // asin/acos/atan
201  Dual<float, 1> c(0.3f, 0);
202  TEST(isEqualF(asin(c).value, asin(c.value)));
203  TEST(isEqualF(asin(c).derivative(0), 1.f/sqrt(1.f - 0.3f*0.3f))); // asin'(x) = 1/sqrt(1-x^2)
204 
205  TEST(isEqualF(acos(c).value, acos(c.value)));
206  TEST(isEqualF(acos(c).derivative(0), -1.f/sqrt(1.f - 0.3f*0.3f))); // acos'(x) = -1/sqrt(1-x^2)
207 
208  TEST(isEqualF(atan(c).value, atan(c.value)));
209  TEST(isEqualF(atan(c).derivative(0), 1.f/(1.f + 0.3f*0.3f))); // tan'(x) = 1 + x^2
210  }
211 
212  {
213  // atan2
214  TEST(isEqualF(atan2(a, b).value, atan2(a.value, b.value)));
215  TEST(isEqualAll(atan2(a, Dual<float,1>(b.value)), atan(a/b.value))); // atan2'(y, x) = atan'(y/x)
216  }
217 
218  {
219  // partial derivatives
220  // function is f(x,y,z) = x^2 + 2xy + 3y^2 + z, we need with respect to d/dx and d/dy at the point (0.5, -0.8, 2)
221 
222  using D = Dual<float, 2>;
223 
224  // set our starting point, requesting partial derivatives of x and y in column 0 and 1
225  Vector3<D> dualPoint(D(0.5f, 0), D(-0.8f, 1), D(2.f));
226 
227  Dual<float, 2> dualResult = testFunction(dualPoint);
228 
229  // compare to a numerical derivative:
230  Vector<float, 3> floatPoint = collectReals(dualPoint);
231  float floatResult = testFunction(floatPoint);
232 
233  float h = 0.0001f;
234  Vector<float, 3> floatPoint_plusDX = floatPoint;
235  floatPoint_plusDX(0) += h;
236  float floatResult_plusDX = testFunction(floatPoint_plusDX);
237 
238  Vector<float, 3> floatPoint_plusDY = floatPoint;
239  floatPoint_plusDY(1) += h;
240  float floatResult_plusDY = testFunction(floatPoint_plusDY);
241 
242  Vector2f numerical_derivative((floatResult_plusDX - floatResult)/h,
243  (floatResult_plusDY - floatResult)/h);
244 
245  TEST(isEqualF(dualResult.value, floatResult, 0.0f));
246  TEST(isEqual(dualResult.derivative, numerical_derivative, 1e-2f));
247 
248  }
249 
250  {
251  // jacobian
252  // get residual of x/y/z with partial derivatives of rotation
253 
254  Vector3f direct_error;
255  Matrix<float, 3, 4> numerical_jacobian;
256  {
257  Vector3f positionState(5,6,7);
258  Vector3f velocityState(-1,0,1);
259  Quaternionf velocityOrientation(0.2f,-0.1f,0,1);
260  Vector3f positionMeasurement(4.5f, 6.2f, 7.9f);
261  float dt = 0.1f;
262 
263  direct_error = positionError(positionState,
264  velocityState,
265  velocityOrientation,
266  positionMeasurement,
267  dt);
268  float h = 0.001f;
269  for (size_t i = 0; i < 4; i++)
270  {
271  Quaternion<float> h4 = velocityOrientation;
272  h4(i) += h;
273  numerical_jacobian.col(i) = (positionError(positionState,
274  velocityState,
275  h4,
276  positionMeasurement,
277  dt)
278  - direct_error)/h;
279  }
280  }
281  Vector3f auto_error;
282  Matrix<float, 3, 4> auto_jacobian;
283  {
284  using D4 = Dual<float, 4>;
285  using Vector3d4 = Vector3<D4>;
286  Vector3d4 positionState(D4(5), D4(6), D4(7));
287  Vector3d4 velocityState(D4(-1), D4(0), D4(1));
288 
289  // request partial derivatives of velocity orientation
290  // by setting these variables' derivatives in corresponding columns [0...3]
291  Quaternion<D4> velocityOrientation(D4(0.2f, 0),D4(-0.1f, 1),D4(0, 2),D4(1, 3));
292 
293  Vector3d4 positionMeasurement(D4(4.5f), D4(6.2f), D4(7.9f));
294  D4 dt(0.1f);
295 
296 
297  Vector3d4 error = positionError(positionState,
298  velocityState,
299  velocityOrientation,
300  positionMeasurement,
301  dt);
302  auto_error = collectReals(error);
303  auto_jacobian = collectDerivatives(error);
304  }
305  TEST(isEqual(direct_error, auto_error, 0.0f));
306  TEST(isEqual(numerical_jacobian, auto_jacobian, 1e-3f));
307 
308  }
309  return 0;
310 }
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 > ceil(const Dual< Scalar, N > &a)
Definition: Dual.hpp:203
Vector3< Type > conjugate(const Vector3< Type > &vec) const
Rotates vector v_1 in frame 1 to vector v_2 in frame 2 using the rotation quaternion q_21 describing ...
Definition: Quaternion.hpp:398
Dual< Scalar, N > asin(const Dual< Scalar, N > &a)
Definition: Dual.hpp:301
Quaternion class.
Definition: Dcm.hpp:24
const Slice< Type, M, 1, M, N > col(size_t j) const
Definition: Matrix.hpp:395
Vector< Scalar, N > derivative
Definition: Dual.hpp:30
Dual< Scalar, N > cos(const Dual< Scalar, N > &a)
Definition: Dual.hpp:286
Matrix< Scalar, M, N > collectReals(const Matrix< Dual< Scalar, D >, M, N > &input)
Definition: Dual.hpp:345
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
Definition: Matrix.hpp:571
T testFunction(const Vector< T, 3 > &point)
Definition: dual.cpp:14
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Dual< Scalar, N > min(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:231
Dual< Scalar, N > tan(const Dual< Scalar, N > &a)
Definition: Dual.hpp:293
Vector< Scalar, 3 > positionError(const Vector< Scalar, 3 > &positionState, const Vector< Scalar, 3 > &velocityStateBody, const Quaternion< Scalar > &bodyOrientation, const Vector< Scalar, 3 > &positionMeasurement, Scalar dt)
Definition: dual.cpp:23
Dual< Scalar, N > fmod(const Dual< Scalar, N > &a, Scalar mod)
Definition: Dual.hpp:217
#define TEST(X)
Definition: test_macros.hpp:14
float dt
Definition: px4io.c:73
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 isEqualAll(Dual< Scalar, N > a, Dual< Scalar, N > b)
Definition: dual.cpp:8
bool isEqualF(const Type x, const Type y, const Type eps=1e-4f)
Compare if two floating point numbers are equal.
bool IsNan(Scalar a)
Definition: Dual.hpp:238
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Definition: Dual.hpp:196
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
int main()
Definition: dual.cpp:33
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