Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
pprz_algebra_float.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2014 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
27 #include "pprz_algebra_float.h"
28 
30 void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, float dt)
31 {
32  vec->x += dv->x * dt;
33  vec->y += dv->y * dt;
34  vec->z += dv->z * dt;
35 }
36 
38 void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr, float dt)
39 {
40  r->p += dr->p * dt;
41  r->q += dr->q * dt;
42  r->r += dr->r * dt;
43 }
44 
45 void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e, struct FloatEulers *edot)
46 {
47  r->p = edot->phi - sinf(e->theta) * edot->psi;
48  r->q = cosf(e->phi) * edot->theta + sinf(e->phi) * cosf(e->theta) * edot->psi;
49  r->r = -sinf(e->phi) * edot->theta + cosf(e->phi) * cosf(e->theta) * edot->psi;
50 }
51 
52 
53 
54 
55 void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
56 {
57  RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);
58  RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0);
59  RMAT_ELMT(*m_b2a, 0, 2) = RMAT_ELMT(*m_a2b, 2, 0);
60  RMAT_ELMT(*m_b2a, 1, 0) = RMAT_ELMT(*m_a2b, 0, 1);
61  RMAT_ELMT(*m_b2a, 1, 1) = RMAT_ELMT(*m_a2b, 1, 1);
62  RMAT_ELMT(*m_b2a, 1, 2) = RMAT_ELMT(*m_a2b, 2, 1);
63  RMAT_ELMT(*m_b2a, 2, 0) = RMAT_ELMT(*m_a2b, 0, 2);
64  RMAT_ELMT(*m_b2a, 2, 1) = RMAT_ELMT(*m_a2b, 1, 2);
65  RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2);
66 }
67 
68 float float_rmat_norm(struct FloatRMat *rm)
69 {
70  return sqrtf(SQUARE(rm->m[0]) + SQUARE(rm->m[1]) + SQUARE(rm->m[2]) +
71  SQUARE(rm->m[3]) + SQUARE(rm->m[4]) + SQUARE(rm->m[5]) +
72  SQUARE(rm->m[6]) + SQUARE(rm->m[7]) + SQUARE(rm->m[8]));
73 }
74 
78 void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
79 {
80  m_a2c->m[0] = m_b2c->m[0] * m_a2b->m[0] + m_b2c->m[1] * m_a2b->m[3] + m_b2c->m[2] * m_a2b->m[6];
81  m_a2c->m[1] = m_b2c->m[0] * m_a2b->m[1] + m_b2c->m[1] * m_a2b->m[4] + m_b2c->m[2] * m_a2b->m[7];
82  m_a2c->m[2] = m_b2c->m[0] * m_a2b->m[2] + m_b2c->m[1] * m_a2b->m[5] + m_b2c->m[2] * m_a2b->m[8];
83  m_a2c->m[3] = m_b2c->m[3] * m_a2b->m[0] + m_b2c->m[4] * m_a2b->m[3] + m_b2c->m[5] * m_a2b->m[6];
84  m_a2c->m[4] = m_b2c->m[3] * m_a2b->m[1] + m_b2c->m[4] * m_a2b->m[4] + m_b2c->m[5] * m_a2b->m[7];
85  m_a2c->m[5] = m_b2c->m[3] * m_a2b->m[2] + m_b2c->m[4] * m_a2b->m[5] + m_b2c->m[5] * m_a2b->m[8];
86  m_a2c->m[6] = m_b2c->m[6] * m_a2b->m[0] + m_b2c->m[7] * m_a2b->m[3] + m_b2c->m[8] * m_a2b->m[6];
87  m_a2c->m[7] = m_b2c->m[6] * m_a2b->m[1] + m_b2c->m[7] * m_a2b->m[4] + m_b2c->m[8] * m_a2b->m[7];
88  m_a2c->m[8] = m_b2c->m[6] * m_a2b->m[2] + m_b2c->m[7] * m_a2b->m[5] + m_b2c->m[8] * m_a2b->m[8];
89 }
90 
94 void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struct FloatRMat *m_b2c)
95 {
96  m_a2b->m[0] = m_b2c->m[0] * m_a2c->m[0] + m_b2c->m[3] * m_a2c->m[3] + m_b2c->m[6] * m_a2c->m[6];
97  m_a2b->m[1] = m_b2c->m[0] * m_a2c->m[1] + m_b2c->m[3] * m_a2c->m[4] + m_b2c->m[6] * m_a2c->m[7];
98  m_a2b->m[2] = m_b2c->m[0] * m_a2c->m[2] + m_b2c->m[3] * m_a2c->m[5] + m_b2c->m[6] * m_a2c->m[8];
99  m_a2b->m[3] = m_b2c->m[1] * m_a2c->m[0] + m_b2c->m[4] * m_a2c->m[3] + m_b2c->m[7] * m_a2c->m[6];
100  m_a2b->m[4] = m_b2c->m[1] * m_a2c->m[1] + m_b2c->m[4] * m_a2c->m[4] + m_b2c->m[7] * m_a2c->m[7];
101  m_a2b->m[5] = m_b2c->m[1] * m_a2c->m[2] + m_b2c->m[4] * m_a2c->m[5] + m_b2c->m[7] * m_a2c->m[8];
102  m_a2b->m[6] = m_b2c->m[2] * m_a2c->m[0] + m_b2c->m[5] * m_a2c->m[3] + m_b2c->m[8] * m_a2c->m[6];
103  m_a2b->m[7] = m_b2c->m[2] * m_a2c->m[1] + m_b2c->m[5] * m_a2c->m[4] + m_b2c->m[8] * m_a2c->m[7];
104  m_a2b->m[8] = m_b2c->m[2] * m_a2c->m[2] + m_b2c->m[5] * m_a2c->m[5] + m_b2c->m[8] * m_a2c->m[8];
105 }
106 
110 void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
111 {
112  vb->x = m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z;
113  vb->y = m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z;
114  vb->z = m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z;
115 }
116 
120 void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
121 {
122  vb->x = m_b2a->m[0] * va->x + m_b2a->m[3] * va->y + m_b2a->m[6] * va->z;
123  vb->y = m_b2a->m[1] * va->x + m_b2a->m[4] * va->y + m_b2a->m[7] * va->z;
124  vb->z = m_b2a->m[2] * va->x + m_b2a->m[5] * va->y + m_b2a->m[8] * va->z;
125 }
126 
130 void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b, struct FloatEulers *ra)
131 {
132  rb->phi = m_a2b->m[0] * ra->phi + m_a2b->m[1] * ra->theta + m_a2b->m[2] * ra->psi;
133  rb->theta = m_a2b->m[3] * ra->phi + m_a2b->m[4] * ra->theta + m_a2b->m[5] * ra->psi;
134  rb->psi = m_a2b->m[6] * ra->phi + m_a2b->m[7] * ra->theta + m_a2b->m[8] * ra->psi;
135 }
136 
140 void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a, struct FloatEulers *ra)
141 {
142  rb->phi = m_b2a->m[0] * ra->phi + m_b2a->m[3] * ra->theta + m_b2a->m[6] * ra->psi;
143  rb->theta = m_b2a->m[1] * ra->phi + m_b2a->m[4] * ra->theta + m_b2a->m[7] * ra->psi;
144  rb->psi = m_b2a->m[2] * ra->phi + m_b2a->m[5] * ra->theta + m_b2a->m[8] * ra->psi;
145 }
146 
150 void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
151 {
152  rb->p = m_a2b->m[0] * ra->p + m_a2b->m[1] * ra->q + m_a2b->m[2] * ra->r;
153  rb->q = m_a2b->m[3] * ra->p + m_a2b->m[4] * ra->q + m_a2b->m[5] * ra->r;
154  rb->r = m_a2b->m[6] * ra->p + m_a2b->m[7] * ra->q + m_a2b->m[8] * ra->r;
155 }
156 
160 void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
161 {
162  rb->p = m_b2a->m[0] * ra->p + m_b2a->m[3] * ra->q + m_b2a->m[6] * ra->r;
163  rb->q = m_b2a->m[1] * ra->p + m_b2a->m[4] * ra->q + m_b2a->m[7] * ra->r;
164  rb->r = m_b2a->m[2] * ra->p + m_b2a->m[5] * ra->q + m_b2a->m[8] * ra->r;
165 }
166 
167 
169 void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
170 {
171  const float ux2 = uv->x * uv->x;
172  const float uy2 = uv->y * uv->y;
173  const float uz2 = uv->z * uv->z;
174  const float uxuy = uv->x * uv->y;
175  const float uyuz = uv->y * uv->z;
176  const float uxuz = uv->x * uv->z;
177  const float can = cosf(angle);
178  const float san = sinf(angle);
179  const float one_m_can = (1. - can);
180 
181  RMAT_ELMT(*rm, 0, 0) = ux2 + (1. - ux2) * can;
182  RMAT_ELMT(*rm, 0, 1) = uxuy * one_m_can + uv->z * san;
183  RMAT_ELMT(*rm, 0, 2) = uxuz * one_m_can - uv->y * san;
184  RMAT_ELMT(*rm, 1, 0) = RMAT_ELMT(*rm, 0, 1);
185  RMAT_ELMT(*rm, 1, 1) = uy2 + (1. - uy2) * can;
186  RMAT_ELMT(*rm, 1, 2) = uyuz * one_m_can + uv->x * san;
187  RMAT_ELMT(*rm, 2, 0) = RMAT_ELMT(*rm, 0, 2);
188  RMAT_ELMT(*rm, 2, 1) = RMAT_ELMT(*rm, 1, 2);
189  RMAT_ELMT(*rm, 2, 2) = uz2 + (1. - uz2) * can;
190 }
191 
192 
193 /* C n->b rotation matrix */
194 void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e)
195 {
196  const float sphi = sinf(e->phi);
197  const float cphi = cosf(e->phi);
198  const float stheta = sinf(e->theta);
199  const float ctheta = cosf(e->theta);
200  const float spsi = sinf(e->psi);
201  const float cpsi = cosf(e->psi);
202 
203  RMAT_ELMT(*rm, 0, 0) = ctheta * cpsi;
204  RMAT_ELMT(*rm, 0, 1) = ctheta * spsi;
205  RMAT_ELMT(*rm, 0, 2) = -stheta;
206  RMAT_ELMT(*rm, 1, 0) = sphi * stheta * cpsi - cphi * spsi;
207  RMAT_ELMT(*rm, 1, 1) = sphi * stheta * spsi + cphi * cpsi;
208  RMAT_ELMT(*rm, 1, 2) = sphi * ctheta;
209  RMAT_ELMT(*rm, 2, 0) = cphi * stheta * cpsi + sphi * spsi;
210  RMAT_ELMT(*rm, 2, 1) = cphi * stheta * spsi - sphi * cpsi;
211  RMAT_ELMT(*rm, 2, 2) = cphi * ctheta;
212 }
213 
214 void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e)
215 {
216  const float sphi = sinf(e->phi);
217  const float cphi = cosf(e->phi);
218  const float stheta = sinf(e->theta);
219  const float ctheta = cosf(e->theta);
220  const float spsi = sinf(e->psi);
221  const float cpsi = cosf(e->psi);
222 
223  RMAT_ELMT(*rm, 0, 0) = ctheta * cpsi - sphi * stheta * spsi;
224  RMAT_ELMT(*rm, 0, 1) = ctheta * spsi + sphi * stheta * cpsi;
225  RMAT_ELMT(*rm, 0, 2) = -cphi * stheta;
226  RMAT_ELMT(*rm, 1, 0) = -cphi * spsi;
227  RMAT_ELMT(*rm, 1, 1) = cphi * cpsi;
228  RMAT_ELMT(*rm, 1, 2) = sphi;
229  RMAT_ELMT(*rm, 2, 0) = stheta * cpsi + sphi * ctheta * spsi;
230  RMAT_ELMT(*rm, 2, 1) = stheta * spsi - sphi * ctheta * cpsi;
231  RMAT_ELMT(*rm, 2, 2) = cphi * ctheta;
232 }
233 
234 
235 /* C n->b rotation matrix */
236 void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
237 {
238  const float _a = M_SQRT2 * q->qi;
239  const float _b = M_SQRT2 * q->qx;
240  const float _c = M_SQRT2 * q->qy;
241  const float _d = M_SQRT2 * q->qz;
242  const float a2_1 = _a * _a - 1;
243  const float ab = _a * _b;
244  const float ac = _a * _c;
245  const float ad = _a * _d;
246  const float bc = _b * _c;
247  const float bd = _b * _d;
248  const float cd = _c * _d;
249  RMAT_ELMT(*rm, 0, 0) = a2_1 + _b * _b;
250  RMAT_ELMT(*rm, 0, 1) = bc + ad;
251  RMAT_ELMT(*rm, 0, 2) = bd - ac;
252  RMAT_ELMT(*rm, 1, 0) = bc - ad;
253  RMAT_ELMT(*rm, 1, 1) = a2_1 + _c * _c;
254  RMAT_ELMT(*rm, 1, 2) = cd + ab;
255  RMAT_ELMT(*rm, 2, 0) = bd + ac;
256  RMAT_ELMT(*rm, 2, 1) = cd - ab;
257  RMAT_ELMT(*rm, 2, 2) = a2_1 + _d * _d;
258 }
259 
261 void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
262 {
263  struct FloatRMat exp_omega_dt = {
264  {
265  1. , dt *omega->r, -dt *omega->q,
266  -dt *omega->r, 1. , dt *omega->p,
267  dt *omega->q, -dt *omega->p, 1.
268  }
269  };
270  struct FloatRMat R_tdt;
271  float_rmat_comp(&R_tdt, rm, &exp_omega_dt);
272  memcpy(rm, &R_tdt, sizeof(R_tdt));
273 }
274 
275 static inline float renorm_factor(float n)
276 {
277  if (n < 1.5625f && n > 0.64f) {
278  return .5 * (3 - n);
279  } else if (n < 100.0f && n > 0.01f) {
280  return 1. / sqrtf(n);
281  } else {
282  return 0.;
283  }
284 }
285 
286 float float_rmat_reorthogonalize(struct FloatRMat *rm)
287 {
288  const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0, 0),
289  RMAT_ELMT(*rm, 0, 1),
290  RMAT_ELMT(*rm, 0, 2)
291  };
292  const struct FloatVect3 r1 = {RMAT_ELMT(*rm, 1, 0),
293  RMAT_ELMT(*rm, 1, 1),
294  RMAT_ELMT(*rm, 1, 2)
295  };
296  float _err = -0.5 * VECT3_DOT_PRODUCT(r0, r1);
297  struct FloatVect3 r0_t;
298  VECT3_SUM_SCALED(r0_t, r0, r1, _err);
299  struct FloatVect3 r1_t;
300  VECT3_SUM_SCALED(r1_t, r1, r0, _err);
301  struct FloatVect3 r2_t;
302  VECT3_CROSS_PRODUCT(r2_t, r0_t, r1_t);
303  float s = renorm_factor(VECT3_NORM2(r0_t));
304  MAT33_ROW_VECT3_SMUL(*rm, 0, r0_t, s);
305  s = renorm_factor(VECT3_NORM2(r1_t));
306  MAT33_ROW_VECT3_SMUL(*rm, 1, r1_t, s);
307  s = renorm_factor(VECT3_NORM2(r2_t));
308  MAT33_ROW_VECT3_SMUL(*rm, 2, r2_t, s);
309 
310  return _err;
311 }
312 
313 
314 /*
315  *
316  * Quaternion functions.
317  *
318  */
319 
320 void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
321 {
322  a2c->qi = a2b->qi * b2c->qi - a2b->qx * b2c->qx - a2b->qy * b2c->qy - a2b->qz * b2c->qz;
323  a2c->qx = a2b->qi * b2c->qx + a2b->qx * b2c->qi + a2b->qy * b2c->qz - a2b->qz * b2c->qy;
324  a2c->qy = a2b->qi * b2c->qy - a2b->qx * b2c->qz + a2b->qy * b2c->qi + a2b->qz * b2c->qx;
325  a2c->qz = a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi;
326 }
327 
328 void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
329 {
330  a2b->qi = a2c->qi * b2c->qi + a2c->qx * b2c->qx + a2c->qy * b2c->qy + a2c->qz * b2c->qz;
331  a2b->qx = -a2c->qi * b2c->qx + a2c->qx * b2c->qi - a2c->qy * b2c->qz + a2c->qz * b2c->qy;
332  a2b->qy = -a2c->qi * b2c->qy + a2c->qx * b2c->qz + a2c->qy * b2c->qi - a2c->qz * b2c->qx;
333  a2b->qz = -a2c->qi * b2c->qz - a2c->qx * b2c->qy + a2c->qy * b2c->qx + a2c->qz * b2c->qi;
334 }
335 
336 void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
337 {
338  b2c->qi = a2b->qi * a2c->qi + a2b->qx * a2c->qx + a2b->qy * a2c->qy + a2b->qz * a2c->qz;
339  b2c->qx = a2b->qi * a2c->qx - a2b->qx * a2c->qi - a2b->qy * a2c->qz + a2b->qz * a2c->qy;
340  b2c->qy = a2b->qi * a2c->qy + a2b->qx * a2c->qz - a2b->qy * a2c->qi - a2b->qz * a2c->qx;
341  b2c->qz = a2b->qi * a2c->qz - a2b->qx * a2c->qy + a2b->qy * a2c->qx - a2b->qz * a2c->qi;
342 }
343 
344 void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
345 {
346  float_quat_comp(a2c, a2b, b2c);
347  float_quat_wrap_shortest(a2c);
348  float_quat_normalize(a2c);
349 }
350 
351 void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
352 {
353  float_quat_comp_inv(a2b, a2c, b2c);
354  float_quat_wrap_shortest(a2b);
355  float_quat_normalize(a2b);
356 }
357 
358 void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
359 {
360  float_quat_inv_comp(b2c, a2b, a2c);
361  float_quat_wrap_shortest(b2c);
362  float_quat_normalize(b2c);
363 }
364 
365 void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt)
366 {
367  const float v_norm = sqrtf(w->p * w->p + w->q * w->q + w->r * w->r);
368  const float c2 = cos(dt * v_norm / 2.0);
369  const float s2 = sin(dt * v_norm / 2.0);
370  if (v_norm < 1e-8) {
371  q_out->qi = 1;
372  q_out->qx = 0;
373  q_out->qy = 0;
374  q_out->qz = 0;
375  } else {
376  q_out->qi = c2;
377  q_out->qx = w->p / v_norm * s2;
378  q_out->qy = w->q / v_norm * s2;
379  q_out->qz = w->r / v_norm * s2;
380  }
381 }
382 
384 void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt)
385 {
386  const float qi = q->qi;
387  const float qx = q->qx;
388  const float qy = q->qy;
389  const float qz = q->qz;
390  const float dp = 0.5 * dt * omega->p;
391  const float dq = 0.5 * dt * omega->q;
392  const float dr = 0.5 * dt * omega->r;
393  q->qi = qi - dp * qx - dq * qy - dr * qz;
394  q->qx = dp * qi + qx + dr * qy - dq * qz;
395  q->qy = dq * qi - dr * qx + qy + dp * qz;
396  q->qz = dr * qi + dq * qx - dp * qy + qz;
397 }
398 
400 void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
401 {
402  const float no = FLOAT_RATES_NORM(*omega);
403  if (no > FLT_MIN) {
404  const float a = 0.5 * no * dt;
405  const float ca = cosf(a);
406  const float sa_ov_no = sinf(a) / no;
407  const float dp = sa_ov_no * omega->p;
408  const float dq = sa_ov_no * omega->q;
409  const float dr = sa_ov_no * omega->r;
410  const float qi = q->qi;
411  const float qx = q->qx;
412  const float qy = q->qy;
413  const float qz = q->qz;
414  q->qi = ca * qi - dp * qx - dq * qy - dr * qz;
415  q->qx = dp * qi + ca * qx + dr * qy - dq * qz;
416  q->qy = dq * qi - dr * qx + ca * qy + dp * qz;
417  q->qz = dr * qi + dq * qx - dp * qy + ca * qz;
418  }
419 }
420 
421 void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
422 {
423  const float qi2_M1_2 = q->qi * q->qi - 0.5;
424  const float qiqx = q->qi * q->qx;
425  const float qiqy = q->qi * q->qy;
426  const float qiqz = q->qi * q->qz;
427  float m01 = q->qx * q->qy; /* aka qxqy */
428  float m02 = q->qx * q->qz; /* aka qxqz */
429  float m12 = q->qy * q->qz; /* aka qyqz */
430 
431  const float m00 = qi2_M1_2 + q->qx * q->qx;
432  const float m10 = m01 - qiqz;
433  const float m20 = m02 + qiqy;
434  const float m21 = m12 - qiqx;
435  m01 += qiqz;
436  m02 -= qiqy;
437  m12 += qiqx;
438  const float m11 = qi2_M1_2 + q->qy * q->qy;
439  const float m22 = qi2_M1_2 + q->qz * q->qz;
440  v_out->x = 2 * (m00 * v_in->x + m01 * v_in->y + m02 * v_in->z);
441  v_out->y = 2 * (m10 * v_in->x + m11 * v_in->y + m12 * v_in->z);
442  v_out->z = 2 * (m20 * v_in->x + m21 * v_in->y + m22 * v_in->z);
443 }
444 
450 void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
451 {
452  qd->qi = -0.5 * (r->p * q->qx + r->q * q->qy + r->r * q->qz);
453  qd->qx = -0.5 * (-r->p * q->qi - r->r * q->qy + r->q * q->qz);
454  qd->qy = -0.5 * (-r->q * q->qi + r->r * q->qx - r->p * q->qz);
455  qd->qz = -0.5 * (-r->r * q->qi - r->q * q->qx + r->p * q->qy);
456 }
457 
461 void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
462 {
463  const float K_LAGRANGE = 1.;
464  const float c = K_LAGRANGE * (1 - float_quat_norm(q)) / -0.5;
465  qd->qi = -0.5 * (c * q->qi + r->p * q->qx + r->q * q->qy + r->r * q->qz);
466  qd->qx = -0.5 * (-r->p * q->qi + c * q->qx - r->r * q->qy + r->q * q->qz);
467  qd->qy = -0.5 * (-r->q * q->qi + r->r * q->qx + c * q->qy - r->p * q->qz);
468  qd->qz = -0.5 * (-r->r * q->qi - r->q * q->qx + r->p * q->qy + c * q->qz);
469 }
470 
477 void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
478 {
479 
480  const float phi2 = e->phi / 2.f;
481  const float theta2 = e->theta / 2.f;
482  const float psi2 = e->psi / 2.f;
483 
484  const float s_phi2 = sinf(phi2);
485  const float c_phi2 = cosf(phi2);
486  const float s_theta2 = sinf(theta2);
487  const float c_theta2 = cosf(theta2);
488  const float s_psi2 = sinf(psi2);
489  const float c_psi2 = cosf(psi2);
490 
491  q->qi = c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2;
492  q->qx = -c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2;
493  q->qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2;
494  q->qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2;
495 }
496 
504 void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e)
505 {
506  const float phi2 = e->phi / 2.f;
507  const float theta2 = e->theta / 2.f;
508  const float psi2 = e->psi / 2.f;
509 
510  const float s_phi2 = sinf(phi2);
511  const float c_phi2 = cosf(phi2);
512  const float s_theta2 = sinf(theta2);
513  const float c_theta2 = cosf(theta2);
514  const float s_psi2 = sinf(psi2);
515  const float c_psi2 = cosf(psi2);
516 
517  q->qi = c_phi2 * c_theta2 * c_psi2 - s_phi2 * s_theta2 * s_psi2;
518  q->qx = s_phi2 * c_theta2 * c_psi2 - c_phi2 * s_theta2 * s_psi2;
519  q->qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2;
520  q->qz = s_phi2 * s_theta2 * c_psi2 + c_phi2 * c_theta2 * s_psi2;
521 }
522 
532 void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e)
533 {
534  const float phi2 = e->phi / 2.f;
535  const float theta2 = e->theta / 2.f;
536  const float psi2 = e->psi / 2.f;
537 
538  const float s_phi2 = sinf(phi2);
539  const float c_phi2 = cosf(phi2);
540  const float s_theta2 = sinf(theta2);
541  const float c_theta2 = cosf(theta2);
542  const float s_psi2 = sinf(psi2);
543  const float c_psi2 = cosf(psi2);
544 
545  q->qi = c_theta2 * c_phi2 * c_psi2 + s_theta2 * s_phi2 * s_psi2;
546  q->qx = c_theta2 * s_phi2 * c_psi2 + s_theta2 * c_phi2 * s_psi2;
547  q->qy = s_theta2 * c_phi2 * c_psi2 - c_theta2 * s_phi2 * s_psi2;
548  q->qz = c_theta2 * c_phi2 * s_psi2 - s_theta2 * s_phi2 * c_psi2;
549 }
550 
551 void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
552 {
553  const float san = sinf(angle / 2.f);
554  q->qi = cosf(angle / 2.f);
555  q->qx = san * uv->x;
556  q->qy = san * uv->y;
557  q->qz = san * uv->z;
558 }
559 
560 void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
561 {
562  const float ov_norm = sqrtf(ov->x * ov->x + ov->y * ov->y + ov->z * ov->z);
563  if (ov_norm < 1e-8) {
564  q->qi = 1;
565  q->qx = 0;
566  q->qy = 0;
567  q->qz = 0;
568  } else {
569  const float s2_normalized = sinf(ov_norm / 2.0) / ov_norm;
570  q->qi = cosf(ov_norm / 2.0);
571  q->qx = ov->x * s2_normalized;
572  q->qy = ov->y * s2_normalized;
573  q->qz = ov->z * s2_normalized;
574  }
575 }
576 
577 void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
578 {
579  const float tr = RMAT_TRACE(*rm);
580  if (tr > 0) {
581  const float two_qi = sqrtf(1. + tr);
582  const float four_qi = 2. * two_qi;
583  q->qi = 0.5 * two_qi;
584  q->qx = (RMAT_ELMT(*rm, 1, 2) - RMAT_ELMT(*rm, 2, 1)) / four_qi;
585  q->qy = (RMAT_ELMT(*rm, 2, 0) - RMAT_ELMT(*rm, 0, 2)) / four_qi;
586  q->qz = (RMAT_ELMT(*rm, 0, 1) - RMAT_ELMT(*rm, 1, 0)) / four_qi;
587  /*printf("tr > 0\n");*/
588  } else {
589  if (RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 1, 1) &&
590  RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 2, 2)) {
591  const float two_qx = sqrtf(RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 1, 1)
592  - RMAT_ELMT(*rm, 2, 2) + 1);
593  const float four_qx = 2. * two_qx;
594  q->qi = (RMAT_ELMT(*rm, 1, 2) - RMAT_ELMT(*rm, 2, 1)) / four_qx;
595  q->qx = 0.5 * two_qx;
596  q->qy = (RMAT_ELMT(*rm, 0, 1) + RMAT_ELMT(*rm, 1, 0)) / four_qx;
597  q->qz = (RMAT_ELMT(*rm, 2, 0) + RMAT_ELMT(*rm, 0, 2)) / four_qx;
598  /*printf("m00 largest\n");*/
599  } else if (RMAT_ELMT(*rm, 1, 1) > RMAT_ELMT(*rm, 2, 2)) {
600  const float two_qy =
601  sqrtf(RMAT_ELMT(*rm, 1, 1) - RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 2, 2) + 1);
602  const float four_qy = 2. * two_qy;
603  q->qi = (RMAT_ELMT(*rm, 2, 0) - RMAT_ELMT(*rm, 0, 2)) / four_qy;
604  q->qx = (RMAT_ELMT(*rm, 0, 1) + RMAT_ELMT(*rm, 1, 0)) / four_qy;
605  q->qy = 0.5 * two_qy;
606  q->qz = (RMAT_ELMT(*rm, 1, 2) + RMAT_ELMT(*rm, 2, 1)) / four_qy;
607  /*printf("m11 largest\n");*/
608  } else {
609  const float two_qz =
610  sqrtf(RMAT_ELMT(*rm, 2, 2) - RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 1, 1) + 1);
611  const float four_qz = 2. * two_qz;
612  q->qi = (RMAT_ELMT(*rm, 0, 1) - RMAT_ELMT(*rm, 1, 0)) / four_qz;
613  q->qx = (RMAT_ELMT(*rm, 2, 0) + RMAT_ELMT(*rm, 0, 2)) / four_qz;
614  q->qy = (RMAT_ELMT(*rm, 1, 2) + RMAT_ELMT(*rm, 2, 1)) / four_qz;
615  q->qz = 0.5 * two_qz;
616  /*printf("m22 largest\n");*/
617  }
618  }
619 }
620 
621 
622 /*
623  *
624  * Euler angle functions.
625  *
626  */
627 
628 void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
629 {
630  const float dcm00 = rm->m[0];
631  const float dcm01 = rm->m[1];
632  float dcm02 = rm->m[2];
633  const float dcm12 = rm->m[5];
634  const float dcm22 = rm->m[8];
635 
636  // asinf does not exist outside [-1,1]
637  BoundAbs(dcm02, 1.0);
638 
639  e->phi = atan2f(dcm12, dcm22);
640  e->theta = -asinf(dcm02);
641  e->psi = atan2f(dcm01, dcm00);
642 }
643 
650 void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
651 {
652  const float qx2 = q->qx * q->qx;
653  const float qy2 = q->qy * q->qy;
654  const float qz2 = q->qz * q->qz;
655  const float qiqx = q->qi * q->qx;
656  const float qiqy = q->qi * q->qy;
657  const float qiqz = q->qi * q->qz;
658  const float qxqy = q->qx * q->qy;
659  const float qxqz = q->qx * q->qz;
660  const float qyqz = q->qy * q->qz;
661  const float dcm00 = 1.0 - 2.*(qy2 + qz2);
662  const float dcm01 = 2.*(qxqy + qiqz);
663  float dcm02 = 2.*(qxqz - qiqy);
664  const float dcm12 = 2.*(qyqz + qiqx);
665  const float dcm22 = 1.0 - 2.*(qx2 + qy2);
666 
667  // asinf does not exist outside [-1,1]
668  BoundAbs(dcm02, 1.0);
669 
670  e->phi = atan2f(dcm12, dcm22);
671  e->theta = -asinf(dcm02);
672  e->psi = atan2f(dcm01, dcm00);
673 }
674 
684 void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q)
685 {
686  const float qx2 = q->qx * q->qx;
687  const float qy2 = q->qy * q->qy;
688  const float qz2 = q->qz * q->qz;
689  const float qi2 = q->qi * q->qi;
690  const float qiqx = q->qi * q->qx;
691  const float qiqy = q->qi * q->qy;
692  const float qiqz = q->qi * q->qz;
693  const float qxqy = q->qx * q->qy;
694  const float qxqz = q->qx * q->qz;
695  const float qyqz = q->qy * q->qz;
696  const float r11 = 2.f * (qxqz + qiqy);
697  const float r12 = qi2 - qx2 + qy2 + qz2;
698  float r21 = -2.f * (qyqz - qiqx);
699  const float r31 = 2.f * (qxqy + qiqz);
700  const float r32 = qi2 - qx2 + qy2 - qz2;
701 
702  // asinf does not exist outside [-1,1]
703  BoundAbs(r21, 1.0);
704 
705  e->theta = atan2f(r11, r12);
706  e->phi = asinf(r21);
707  e->psi = atan2f(r31, r32);
708 }
709 
717 void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
718 {
719  const float qx2 = q->qx * q->qx;
720  const float qy2 = q->qy * q->qy;
721  const float qz2 = q->qz * q->qz;
722  const float qi2 = q->qi * q->qi;
723  const float qiqx = q->qi * q->qx;
724  const float qiqy = q->qi * q->qy;
725  const float qiqz = q->qi * q->qz;
726  const float qxqy = q->qx * q->qy;
727  const float qxqz = q->qx * q->qz;
728  const float qyqz = q->qy * q->qz;
729  const float r11 = -2 * (qxqy - qiqz);
730  const float r12 = qi2 - qx2 + qy2 - qz2;
731  float r21 = 2 * (qyqz + qiqx);
732  const float r31 = -2 * (qxqz - qiqy);
733  const float r32 = qi2 - qx2 - qy2 + qz2;
734 
735  // asinf does not exist outside [-1,1]
736  BoundAbs(r21, 1.0);
737 
738  e->psi = atan2f(r11, r12);
739  e->phi = asinf(r21);
740  e->theta = atan2f(r31, r32);
741 }
742 
751 bool float_mat_inv_2d(float inv_out[4], float mat_in[4])
752 {
753  float det = mat_in[0] * mat_in[3] - mat_in[1] * mat_in[2];
754 
755  if (fabsf(det) < 1e-4) { return 1; } //not invertible
756 
757  inv_out[0] = mat_in[3] / det;
758  inv_out[1] = -mat_in[1] / det;
759  inv_out[2] = -mat_in[2] / det;
760  inv_out[3] = mat_in[0] / det;
761 
762  return 0; //return success
763 }
764 
772 void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in)
773 {
774  vect_out->x = mat[0] * vect_in.x + mat[1] * vect_in.y;
775  vect_out->y = mat[2] * vect_in.x + mat[3] * vect_in.y;
776 }
777 
778 /*
779  * 4x4 Matrix inverse.
780  * obtained from: http://rodolphe-vaillant.fr/?e=7
781  */
782 
783 static float float_mat_minor_4d(float m[16], int r0, int r1, int r2, int c0, int c1, int c2)
784 {
785  return m[4 * r0 + c0] * (m[4 * r1 + c1] * m[4 * r2 + c2] - m[4 * r2 + c1] * m[4 * r1 + c2]) -
786  m[4 * r0 + c1] * (m[4 * r1 + c0] * m[4 * r2 + c2] - m[4 * r2 + c0] * m[4 * r1 + c2]) +
787  m[4 * r0 + c2] * (m[4 * r1 + c0] * m[4 * r2 + c1] - m[4 * r2 + c0] * m[4 * r1 + c1]);
788 }
789 
790 
791 static void float_mat_adjoint_4d(float adjOut[16], float m[16])
792 {
793  adjOut[ 0] = float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3);
794  adjOut[ 1] = -float_mat_minor_4d(m, 0, 2, 3, 1, 2, 3);
795  adjOut[ 2] = float_mat_minor_4d(m, 0, 1, 3, 1, 2, 3);
796  adjOut[ 3] = -float_mat_minor_4d(m, 0, 1, 2, 1, 2, 3);
797  adjOut[ 4] = -float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3);
798  adjOut[ 5] = float_mat_minor_4d(m, 0, 2, 3, 0, 2, 3);
799  adjOut[ 6] = -float_mat_minor_4d(m, 0, 1, 3, 0, 2, 3);
800  adjOut[ 7] = float_mat_minor_4d(m, 0, 1, 2, 0, 2, 3);
801  adjOut[ 8] = float_mat_minor_4d(m, 1, 2, 3, 0, 1, 3);
802  adjOut[ 9] = -float_mat_minor_4d(m, 0, 2, 3, 0, 1, 3);
803  adjOut[10] = float_mat_minor_4d(m, 0, 1, 3, 0, 1, 3);
804  adjOut[11] = -float_mat_minor_4d(m, 0, 1, 2, 0, 1, 3);
805  adjOut[12] = -float_mat_minor_4d(m, 1, 2, 3, 0, 1, 2);
806  adjOut[13] = float_mat_minor_4d(m, 0, 2, 3, 0, 1, 2);
807  adjOut[14] = -float_mat_minor_4d(m, 0, 1, 3, 0, 1, 2);
808  adjOut[15] = float_mat_minor_4d(m, 0, 1, 2, 0, 1, 2);
809 }
810 
811 static float float_mat_det_4d(float m[16])
812 {
813  return m[0] * float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3) -
814  m[1] * float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3) +
815  m[2] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 3) -
816  m[3] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 2);
817 }
818 
825 bool float_mat_inv_4d(float invOut[16], float mat_in[16])
826 {
827  float_mat_adjoint_4d(invOut, mat_in);
828 
829  float det = float_mat_det_4d(mat_in);
830  if (fabsf(det) < 1e-4) { return 1; } //not invertible
831 
832  float inv_det = 1.0f / det;
833  int i;
834  for (i = 0; i < 16; ++i) {
835  invOut[i] = invOut[i] * inv_det;
836  }
837 
838  return 0; //success
839 }
840 
845 void float_mat_invert(float **o, float **mat, int n)
846 {
847  int i, j, k;
848  float t;
849  float a[n][2 * n];
850 
851  // Append an identity matrix on the right of the original matrix
852  for (i = 0; i < n; i++) {
853  for (j = 0; j < 2 * n; j++) {
854  if (j < n) {
855  a[i][j] = mat[i][j];
856  } else if ((j >= n) && (j == i + n)) {
857  a[i][j] = 1.0;
858  } else {
859  a[i][j] = 0.0;
860  }
861  }
862  }
863 
864  // Do the inversion
865  for (i = 0; i < n; i++) {
866  t = a[i][i]; // Store diagonal variable (temp)
867 
868  for (j = i; j < 2 * n; j++) {
869  a[i][j] = a[i][j] / t; // Divide by the diagonal value
870  }
871 
872  for (j = 0; j < n; j++) {
873  if (i != j) {
874  t = a[j][i];
875  for (k = 0; k < 2 * n; k++) {
876  a[j][k] = a[j][k] - t * a[i][k];
877  }
878  }
879  }
880  }
881 
882  // Cut out the identity, which has now moved to the left side
883  for (i = 0 ; i < n ; i++) {
884  for (j = n; j < 2 * n; j++) {
885  o[i][j - n] = a[i][j];
886  }
887  }
888 }
889 
890 /*
891  * o[n][n] = e^a[n][n]
892  * Replicates expm(a) in Matlab
893  *
894  * Adapted from the following reference:
895  * Cleve Moler, Charles VanLoan,
896  * Nineteen Dubious Ways to Compute the Exponential of a Matrix,
897  * Twenty-Five Years Later, SIAM Review, Volume 45, Number 1, March 2003, pages 3-49.
898  * https://people.sc.fsu.edu/~jburkardt/c_src/matrix_exponential/matrix_exponential.c
899  */
900 void float_mat_exp(float **a, float **o, int n)
901 {
902  float a_norm, c, t;
903  const int q = 6;
904  float d[n][n];
905  float x[n][n];
906  float a_copy[n][n];
907  int ee, k, s;
908  int p;
909 
910  MAKE_MATRIX_PTR(_a, a, n);
911  MAKE_MATRIX_PTR(_o, o, n);
912  MAKE_MATRIX_PTR(_d, d, n);
913  MAKE_MATRIX_PTR(_x, x, n);
914  MAKE_MATRIX_PTR(_a_copy, a_copy, n);
915 
916  float_mat_copy(_a_copy, _a, n, n); // Make a copy of a to compute on
917  a_norm = float_mat_norm_li(_a_copy, n, n); // Compute the infinity norm of the matrix
918  ee = (int)(float_log_n(a_norm, 2)) + 1;
919  s = Max(0, ee + 1);
920  t = 1.0 / powf(2.0, s);
921  float_mat_scale(_a_copy, t, n, n);
922  float_mat_copy(_x, _a_copy, n, n); // x = a_copy
923  c = 0.5;
924 
925  float_mat_diagonal_scal(_o, 1.0, n); // make identiy
926  float_mat_sum_scaled(_o, _a_copy, c, n, n);
927 
928  float_mat_diagonal_scal(_d, 1.0, n);
929  float_mat_sum_scaled(_d, _a_copy, -c, n, n);
930 
931  p = 1;
932  for (k = 2; k <= q; k++) {
933  c = c * (float)(q - k + 1) / (float)(k * (2 * q - k + 1));
934  float_mat_mul_copy(_x, _x, _a_copy, n, n, n);
935 
936  float_mat_sum_scaled(_o, _x, c, n, n);
937 
938  if (p) {
939  float_mat_sum_scaled(_d, _x, c, n, n);
940  } else {
941  float_mat_sum_scaled(_d, _x, -c, n, n);
942  }
943  p = !p;
944  }
945 
946  // E -> inverse(D) * E
947  float temp[n][n];
948  MAKE_MATRIX_PTR(_temp, temp, n);
949  float_mat_invert(_temp, _d, n);
950  float_mat_mul_copy(_o, _temp, _o, n, n, n);
951 
952  // E -> E^(2*S)
953  for (k = 1; k <= s; k++) {
954  float_mat_mul_copy(_o, _o, _o, n, n, n);
955  }
956 
957 }
958 
959 /* Returns L-oo of matrix a */
960 float float_mat_norm_li(float **a, int m, int n)
961 {
962  float row_sum;
963  float value;
964 
965  value = 0.0;
966  for (int i = 0; i < m; i++) {
967  row_sum = 0.0;
968  for (int j = 0; j < n; j++) {
969  row_sum = row_sum + fabsf(a[i][j]);
970  }
971  value = Max(value, row_sum);
972  }
973  return value;
974 }
975 
976 /* Scale a 3D vector to within a 2D bound */
977 void vect_bound_in_2d(struct FloatVect3 *vect3, float bound) {
978  float norm = FLOAT_VECT2_NORM(*vect3);
979  if(norm>bound) {
980  float scale = bound/norm;
981  vect3->x *= scale;
982  vect3->y *= scale;
983  }
984 }
985 
986 /* Scale a 3D vector to a certain length in 2D */
987 void vect_scale(struct FloatVect3 *vect3, float norm_des) {
988  float norm = FLOAT_VECT2_NORM(*vect3);
989  if(norm>0.1) {
990  float scale = norm_des/norm;
991  vect3->x *= scale;
992  vect3->y *= scale;
993  }
994 }
float_mat_minor_4d
static float float_mat_minor_4d(float m[16], int r0, int r1, int r2, int c0, int c1, int c2)
Definition: pprz_algebra_float.c:783
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
c
VIC slots used for the LPC2148 define name e g gps UART1_VIC_SLOT e g modem SPI1_VIC_SLOT SPI1 in mcu_periph spi_arch c or spi_slave_hs_arch c(and some others not using the SPI peripheral yet..) I2C0_VIC_SLOT 8 mcu_periph/i2c_arch.c I2C1_VIC_SLOT 9 mcu_periph/i2c_arch.c USB_VIC_SLOT 10 usb
float_quat_integrate_fi
void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place first order quaternion integration with constant rotational velocity
Definition: pprz_algebra_float.c:384
float_mat_scale
static void float_mat_scale(float **a, float k, int m, int n)
a *= k, where k is a scalar value
Definition: pprz_algebra_float.h:779
float_mat_exp
void float_mat_exp(float **a, float **o, int n)
Definition: pprz_algebra_float.c:900
float_quat_comp
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_float.c:320
FLOAT_RATES_NORM
#define FLOAT_RATES_NORM(_v)
Definition: pprz_algebra_float.h:195
float_eulers_of_rmat
void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
Definition: pprz_algebra_float.c:628
scale
static const float scale[]
Definition: dw1000_arduino.c:200
float_rmat_norm
float float_rmat_norm(struct FloatRMat *rm)
Norm of a rotation matrix.
Definition: pprz_algebra_float.c:68
float_mat_invert
void float_mat_invert(float **o, float **mat, int n)
Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.
Definition: pprz_algebra_float.c:845
MAKE_MATRIX_PTR
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
Definition: pprz_algebra_float.h:635
VECT3_CROSS_PRODUCT
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:244
float_rmat_mult
void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b, struct FloatEulers *ra)
rotate angle by rotation matrix.
Definition: pprz_algebra_float.c:130
SQUARE
#define SQUARE(_a)
Definition: pprz_algebra.h:48
s
static uint32_t s
Definition: light_scheduler.c:33
float_mat_diagonal_scal
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
Definition: pprz_algebra_float.h:848
float_mat_copy
static void float_mat_copy(float **a, float **b, int m, int n)
a = b
Definition: pprz_algebra_float.h:656
MAT33_ROW_VECT3_SMUL
#define MAT33_ROW_VECT3_SMUL(_mat, _row, _vin, _s)
Definition: pprz_algebra.h:514
float_vect3_integrate_fi
void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, float dt)
in place first order integration of a 3D-vector
Definition: pprz_algebra_float.c:30
float_mat_inv_4d
bool float_mat_inv_4d(float invOut[16], float mat_in[16])
4x4 Matrix inverse
Definition: pprz_algebra_float.c:825
VECT3_DOT_PRODUCT
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:250
pprz_algebra_float.h
Paparazzi floating point algebra.
float_mat_adjoint_4d
static void float_mat_adjoint_4d(float adjOut[16], float m[16])
Definition: pprz_algebra_float.c:791
logger_uart_parse.s2
s2
Definition: logger_uart_parse.py:9
VECT3_NORM2
#define VECT3_NORM2(_v)
Definition: pprz_algebra.h:252
float_rmat_comp_inv
void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
Definition: pprz_algebra_float.c:94
float_mat_det_4d
static float float_mat_det_4d(float m[16])
Definition: pprz_algebra_float.c:811
float_rmat_integrate_fi
void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
in place first order integration of a rotation matrix
Definition: pprz_algebra_float.c:261
RMAT_TRACE
#define RMAT_TRACE(_rm)
Definition: pprz_algebra.h:663
float_rmat_transp_mult
void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a, struct FloatEulers *ra)
rotate angle by transposed rotation matrix.
Definition: pprz_algebra_float.c:140
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
float_quat_inv_comp
void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_float.c:336
FloatVect2
Definition: pprz_algebra_float.h:49
FloatVect3
Definition: pprz_algebra_float.h:54
float_rmat_transp_vmult
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_float.c:120
float_rmat_vmult
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
Definition: pprz_algebra_float.c:110
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
float_quat_of_eulers_zxy
void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e)
quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
Definition: pprz_algebra_float.c:504
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
float_mat_inv_2d
bool float_mat_inv_2d(float inv_out[4], float mat_in[4])
2x2 matrix inverse
Definition: pprz_algebra_float.c:751
float_rmat_of_eulers_312
void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e)
Definition: pprz_algebra_float.c:214
FLOAT_VECT2_NORM
#define FLOAT_VECT2_NORM(_v)
Definition: pprz_algebra_float.h:132
float_mat2_mult
void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in)
Multiply 2D matrix with vector.
Definition: pprz_algebra_float.c:772
vect_scale
void vect_scale(struct FloatVect3 *vect3, float norm_des)
Definition: pprz_algebra_float.c:987
float_rmat_comp
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
Definition: pprz_algebra_float.c:78
float_mat_norm_li
float float_mat_norm_li(float **a, int m, int n)
Definition: pprz_algebra_float.c:960
float_rates_integrate_fi
void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr, float dt)
in place first order integration of angular rates
Definition: pprz_algebra_float.c:38
float_quat_normalize
static void float_quat_normalize(struct FloatQuat *q)
Definition: pprz_algebra_float.h:380
float_rmat_of_quat
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
Definition: pprz_algebra_float.c:236
RMAT_ELMT
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:660
float_mat_mul_copy
static void float_mat_mul_copy(float **o, float **a, float **b, int m, int n, int l)
o = a * b
Definition: pprz_algebra_float.h:739
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
float_quat_of_rmat
void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
Quaternion from rotation matrix.
Definition: pprz_algebra_float.c:577
float_rmat_reorthogonalize
float float_rmat_reorthogonalize(struct FloatRMat *rm)
Definition: pprz_algebra_float.c:286
float_quat_integrate
void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place quaternion integration with constant rotational velocity
Definition: pprz_algebra_float.c:400
float_quat_comp_inv_norm_shortest
void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
Definition: pprz_algebra_float.c:351
float_rates_of_euler_dot
void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e, struct FloatEulers *edot)
Definition: pprz_algebra_float.c:45
c1
static uint16_t c1
Definition: baro_MS5534A.c:203
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
float_quat_vmult
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
Definition: pprz_algebra_float.c:421
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
VECT3_SUM_SCALED
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:175
float_rmat_of_eulers_321
void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e)
Rotation matrix from 321 Euler angles (float).
Definition: pprz_algebra_float.c:194
float_quat_wrap_shortest
static void float_quat_wrap_shortest(struct FloatQuat *q)
Definition: pprz_algebra_float.h:396
float_quat_of_axis_angle
void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
Quaternion from unit vector and angle.
Definition: pprz_algebra_float.c:551
float_quat_derivative
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
Definition: pprz_algebra_float.c:450
c2
static uint16_t c2
Definition: baro_MS5534A.c:203
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
float_eulers_of_quat_zxy
void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
Definition: pprz_algebra_float.c:717
vect_bound_in_2d
void vect_bound_in_2d(struct FloatVect3 *vect3, float bound)
Definition: pprz_algebra_float.c:977
M_SQRT2
#define M_SQRT2
Definition: pprz_algebra_float.h:46
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
float_quat_of_orientation_vect
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
Definition: pprz_algebra_float.c:560
float_rmat_transp_ratemult
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
Definition: pprz_algebra_float.c:160
float_quat_comp_norm_shortest
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
Definition: pprz_algebra_float.c:344
float_rmat_of_axis_angle
void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
initialises a rotation matrix from unit vector axis and angle
Definition: pprz_algebra_float.c:169
float_rmat_ratemult
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
Definition: pprz_algebra_float.c:150
float_log_n
static float float_log_n(float v, float n)
Definition: pprz_algebra_float.h:107
float_quat_norm
static float float_quat_norm(struct FloatQuat *q)
Definition: pprz_algebra_float.h:375
float_quat_derivative_lagrange
void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
Definition: pprz_algebra_float.c:461
float_quat_inv_comp_norm_shortest
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
Definition: pprz_algebra_float.c:358
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
float_quat_differential
void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt)
Delta rotation quaternion with constant angular rates.
Definition: pprz_algebra_float.c:365
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
Max
#define Max(x, y)
Definition: main_fbw.c:53
float_eulers_of_quat
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
Definition: pprz_algebra_float.c:650
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
float_mat_sum_scaled
static void float_mat_sum_scaled(float **a, float **b, float k, int m, int n)
a += k*b, where k is a scalar value
Definition: pprz_algebra_float.h:795
float_eulers_of_quat_yxz
void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'YXZ' This function calculates from a quaternion the Euler angles with the order YXZ,...
Definition: pprz_algebra_float.c:684
FloatRMat::m
float m[3 *3]
Definition: pprz_algebra_float.h:78
float_quat_of_eulers_yxz
void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e)
quat from euler rotation 'YXZ' This function calculates a quaternion from Euler angles with the order...
Definition: pprz_algebra_float.c:532
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
p
static float p[2][2]
Definition: ins_alt_float.c:268
renorm_factor
static float renorm_factor(float n)
Definition: pprz_algebra_float.c:275
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
float_rmat_inv
void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
Inverse/transpose of a rotation matrix.
Definition: pprz_algebra_float.c:55
float_quat_of_eulers
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
Definition: pprz_algebra_float.c:477
float_quat_comp_inv
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_float.c:328