Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_float_invariant.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Jean-Philippe Condomines, Gautier Hattenberger
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 
30 
32 
33 #include "generated/airframe.h"
34 
36 #include "math/pprz_algebra_int.h"
37 #include "math/pprz_rk_float.h"
38 
39 #if SEND_INVARIANT_FILTER
41 #endif
42 
43 /*---------Invariant Observers-----------
44  *
45  * State vector :
46  * x = [q0 q1 q2 q3 wb1 wb2 wb3 as cs]
47  *
48  *
49  * Dynamic model (dim = 9) :
50  * x_qdot = 0.5 * x_quat * ( x_rates - x_bias );
51  * x_bias_dot = 0;
52  * x_asdot = 0;
53  * x_csdot = 0;
54  *
55  * Observation model (dim = 6) :
56  * ya = as * (q)-1 * A * q; (A : accelerometers)
57  * yc = cs * (q)-1 * C * q; (C = A x B (cross product), B : magnetometers)
58  *
59  *------------------------------------------*/
60 
61 // Default values for the tuning gains
62 // Tuning parameter of accel and mag on attitude
63 #ifndef AHRS_INV_LX
64 #define AHRS_INV_LX 2. * (0.06 + 0.1)
65 #endif
66 // Tuning parameter of accel and mag on attitude
67 #ifndef AHRS_INV_LY
68 #define AHRS_INV_LY 2. * (0.06 + 0.06)
69 #endif
70 // Tuning parameter of accel and mag on attitude
71 #ifndef AHRS_INV_LZ
72 #define AHRS_INV_LZ 2. * (0.1 + 0.06)
73 #endif
74 // Tuning parameter of accel and mag on gyro bias
75 #ifndef AHRS_INV_MX
76 #define AHRS_INV_MX 2. * 0.05 * (0.06 + 0.1)
77 #endif
78 // Tuning parameter of accel and mag on gyro bias
79 #ifndef AHRS_INV_MY
80 #define AHRS_INV_MY 2. * 0.05 * (0.06 + 0.06)
81 #endif
82 // Tuning parameter of accel and mag on gyro bias
83 #ifndef AHRS_INV_MZ
84 #define AHRS_INV_MZ 2. * 0.05 * (0.1 + 0.06)
85 #endif
86 // Tuning parameter of accel and mag on accel bias
87 #ifndef AHRS_INV_N
88 #define AHRS_INV_N 0.25
89 #endif
90 // Tuning parameter of accel and mag on mag bias
91 #ifndef AHRS_INV_O
92 #define AHRS_INV_O 0.5
93 #endif
94 
96 
97 /* earth gravity model */
98 static const struct FloatVect3 A = { 0.f, 0.f, -9.81f };
99 
100 /* earth magnetic model */
101 #define B ahrs_float_inv.mag_h
102 
103 /* error computation */
104 static inline void error_output(struct AhrsFloatInv *_ins);
105 
106 /* propagation model (called by runge-kutta library) */
107 static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m);
108 
109 
113 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
114  struct FloatVect3 *vi);
115 
116 /* init state and measurements */
117 static inline void init_invariant_state(void)
118 {
119  // init state
122  ahrs_float_inv.state.as = 1.0f;
123  ahrs_float_inv.state.cs = 1.0f;
124 
125  // init measures
128 
129 }
130 
132 {
133  // init magnetometers
134 
135  ahrs_float_inv.mag_h.x = AHRS_H_X;
136  ahrs_float_inv.mag_h.y = AHRS_H_Y;
137  ahrs_float_inv.mag_h.z = AHRS_H_Z;
138 
139  // init state and measurements
141 
142  // init gains
151 
152  ahrs_float_inv.is_aligned = false;
153  ahrs_float_inv.reset = false;
154 }
155 
157  struct FloatVect3 *lp_accel,
158  struct FloatVect3 *lp_mag)
159 {
160  /* Compute an initial orientation from accel and mag directly as quaternion */
162 
163  /* use average gyro as initial value for bias */
164  ahrs_float_inv.state.bias = *lp_gyro;
165 
166  // ins and ahrs are now running
167  ahrs_float_inv.is_aligned = true;
168 }
169 
170 void ahrs_float_invariant_propagate(struct FloatRates* gyro, float dt)
171 {
172  // realign all the filter if needed
173  // a complete init cycle is required
174  if (ahrs_float_inv.reset) {
175  ahrs_float_inv.reset = false;
176  ahrs_float_inv.is_aligned = false;
178  }
179 
180  // fill command vector
181  struct FloatRates gyro_meas_body;
182  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
183  float_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro);
184  ahrs_float_inv.cmd.rates = gyro_meas_body;
185 
186  // update correction gains
188 
189  // propagate model
190  struct inv_state new_state;
191  runge_kutta_4_float((float *)&new_state,
194  invariant_model, dt);
195  ahrs_float_inv.state = new_state;
196 
197  // normalize quaternion
199 
200  //------------------------------------------------------------//
201 
202 #if SEND_INVARIANT_FILTER
203  struct FloatEulers eulers;
204  float foo = 0.f;
206  RunOnceEvery(3,
207  pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device,
208  AC_ID,
210  &eulers.phi,
211  &eulers.theta,
212  &eulers.psi,
213  &foo,
214  &foo,
215  &foo,
216  &foo,
217  &foo,
218  &foo,
224  &foo,
225  &foo);
226  );
227 #endif
228 
229 }
230 
232 {
233  ahrs_float_inv.meas.accel = *accel;
234 }
235 
236 // assume mag is dead when values are not moving anymore
237 #define MAG_FROZEN_COUNT 30
238 
240 {
241  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
242  static int32_t last_mx = 0;
243 
244  if (last_mx == mag->x) {
245  mag_frozen_count--;
246  if (mag_frozen_count == 0) {
247  // if mag is dead, better set measurements to zero
249  mag_frozen_count = MAG_FROZEN_COUNT;
250  }
251  } else {
252  // values are moving
253  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
254  // new values in body frame
255  float_rmat_transp_vmult(&ahrs_float_inv.meas.mag, body_to_imu_rmat, mag);
256  // reset counter
257  mag_frozen_count = MAG_FROZEN_COUNT;
258  }
259  last_mx = mag->x;
260 }
261 
266 static inline void invariant_model(float *o, const float *x, const int n, const float *u,
267  const int m __attribute__((unused)))
268 {
269 
270 #pragma GCC diagnostic push // require GCC 4.6
271 #pragma GCC diagnostic ignored "-Wcast-qual"
272  struct inv_state *s = (struct inv_state *)x;
273  struct inv_command *c = (struct inv_command *)u;
274 #pragma GCC diagnostic pop // require GCC 4.6
275  struct inv_state s_dot;
276  struct FloatRates rates_unbiased;
277  struct FloatVect3 tmp_vect;
278  struct FloatQuat tmp_quat;
279 
280  // test accel sensitivity
281  if (fabs(s->as) < 0.1) {
282  // too small, return x_dot = 0 to avoid division by 0
283  float_vect_zero(o, n);
284  // TODO set ins state to error
285  return;
286  }
287 
288  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
289  RATES_DIFF(rates_unbiased, c->rates, s->bias);
290  /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
291  float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
292 
293  float_quat_vmul_right(&tmp_quat, &(s->quat), &ahrs_float_inv.corr.LE);
294  QUAT_ADD(s_dot.quat, tmp_quat);
295 
296  float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat);
297  QUAT_SMUL(tmp_quat, s->quat, norm2_r);
298  QUAT_ADD(s_dot.quat, tmp_quat);
299 
300  /* bias_dot = q-1 * (ME) * q */
301  float_quat_vmult(&tmp_vect, &(s->quat), &ahrs_float_inv.corr.ME);
302  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
303 
304  /* as_dot = as * NE */
305  s_dot.as = (s->as) * (ahrs_float_inv.corr.NE);
306 
307  /* cs_dot = OE */
308  s_dot.cs = ahrs_float_inv.corr.OE;
309 
310  // set output
311  memcpy(o, &s_dot, n * sizeof(float));
312 }
313 
318 static inline void error_output(struct AhrsFloatInv *_ahrs)
319 {
320 
321  struct FloatVect3 YAt, C, YC, YCt, Ec, Ea;
322 
323  // test accel sensitivity
324  if (fabs(_ahrs->state.as) < 0.1) {
325  // too small, don't do anything to avoid division by 0
326  return;
327  }
328 
329  /* C = A X B Cross product */
330  VECT3_CROSS_PRODUCT(C, A, B);
331  /* YC = YA X YB Cross product */
332  VECT3_CROSS_PRODUCT(YC, _ahrs->meas.accel, _ahrs->meas.mag);
333  /* YCt = (1 / cs) * q * YC * q-1 => invariant output on magnetometers */
334  struct FloatQuat q_b2n;
335  float_quat_invert(&q_b2n, &(_ahrs->state.quat));
336  float_quat_vmult(&YCt, &q_b2n, &YC);
337  VECT3_SMUL(YCt, YCt, 1. / (_ahrs->state.cs));
338 
339  /* YAt = q * yA * q-1 => invariant output on accelerometers */
340  float_quat_vmult(&YAt, &q_b2n, &(_ahrs->meas.accel));
341  VECT3_SMUL(YAt, YAt, 1. / (_ahrs->state.as));
342 
343  /*--------- E = ( ŷ - y ) ----------*/
344 
345  /* EC = ( C - YCt ) */
346  VECT3_DIFF(Ec, C, YCt);
347  /* EA = ( A - YAt ) */
348  VECT3_DIFF(Ea, A, YAt);
349 
350  /*-------------- Gains --------------*/
351 
352  /**** LaEa + LcEc *****/
353  _ahrs->corr.LE.x = (-_ahrs->gains.lx * Ea.y) / (2.f * A.z);
354  _ahrs->corr.LE.y = (_ahrs->gains.ly * Ea.x) / (2.f * A.z);
355  _ahrs->corr.LE.z = (-_ahrs->gains.lz * Ec.x) / (B.x * 2.f * A.z);
356 
357  /***** MaEa + McEc ********/
358  _ahrs->corr.ME.x = (_ahrs->gains.mx * Ea.y) / (2.f * A.z);
359  _ahrs->corr.ME.y = (-_ahrs->gains.my * Ea.x)/(2.f * A.z);
360  _ahrs->corr.ME.z = (_ahrs->gains.mz * Ec.x) / (B.x * 2.f * A.z);
361 
362  /****** NaEa + NcEc ********/
363  _ahrs->corr.NE = (-_ahrs->gains.n * Ea.z) / A.z;
364 
365  /****** OaEa + OcEc ********/
366  _ahrs->corr.OE = (-_ahrs->gains.o * Ec.y) / (B.x * A.z);
367 }
368 
369 
370 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
371  struct FloatVect3 *vi)
372 {
373  struct FloatVect3 qvec, v1, v2;
374  float qi;
375 
376  FLOAT_QUAT_EXTRACT(qvec, *q);
377  qi = - VECT3_DOT_PRODUCT(*vi, qvec);
378  VECT3_CROSS_PRODUCT(v1, *vi, qvec);
379  VECT3_SMUL(v2, *vi, q->qi);
380  VECT3_ADD(v2, v1);
381  QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
382 }
383 
385 {
387 
388  if (!ahrs_float_inv.is_aligned) {
389  /* Set ltp_to_imu so that body is zero */
390  ahrs_float_inv.state.quat = *q_b2i;
391  }
392 }
393 
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
inv_correction_gains::ME
struct FloatVect3 ME
Correction gains on gyro biases.
Definition: ahrs_float_invariant.h:72
AHRS_INV_O
#define AHRS_INV_O
Definition: ahrs_float_invariant.c:92
ahrs_float_invariant_update_mag
void ahrs_float_invariant_update_mag(struct FloatVect3 *mag)
Definition: ahrs_float_invariant.c:239
MAG_FROZEN_COUNT
#define MAG_FROZEN_COUNT
Definition: ahrs_float_invariant.c:237
ahrs_float_inv_set_body_to_imu_quat
void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ahrs_float_invariant.c:384
float_quat_vmul_right
void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, struct FloatVect3 *vi)
Right multiplication by a quaternion.
Definition: ahrs_float_invariant.c:370
ahrs_int_utils.h
A
static const struct FloatVect3 A
Definition: ahrs_float_invariant.c:98
VECT3_SMUL
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
AhrsFloatInv::gains
struct inv_gains gains
tuning gains
Definition: ahrs_float_invariant.h:97
float_quat_identity
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
Definition: pprz_algebra_float.h:365
ahrs_float_invariant_propagate
void ahrs_float_invariant_propagate(struct FloatRates *gyro, float dt)
Definition: ahrs_float_invariant.c:170
FLOAT_RATES_ZERO
#define FLOAT_RATES_ZERO(_r)
Definition: pprz_algebra_float.h:191
inv_command::rates
struct FloatRates rates
Input gyro rates.
Definition: ahrs_float_invariant.h:65
VECT3_CROSS_PRODUCT
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:244
s
static uint32_t s
Definition: light_scheduler.c:33
inv_state
Invariant filter state.
Definition: ahrs_float_invariant.h:40
VECT3_DIFF
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
v2
@ v2
Definition: discrete_ekf_no_north.c:34
VECT3_DOT_PRODUCT
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:250
ahrs_float_invariant.h
inv_measures::accel
struct FloatVect3 accel
Measured accelerometers.
Definition: ahrs_float_invariant.h:54
pprz_algebra_float.h
Paparazzi floating point algebra.
inv_state::bias
struct FloatRates bias
Estimated gyro biases.
Definition: ahrs_float_invariant.h:42
VECT3_ADD
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
AHRS_INV_MX
#define AHRS_INV_MX
Definition: ahrs_float_invariant.c:76
error_output
static void error_output(struct AhrsFloatInv *_ins)
Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error )
Definition: ahrs_float_invariant.c:318
pprz_rk_float.h
Runge-Kutta library (float version)
AHRS_INV_LY
#define AHRS_INV_LY
Definition: ahrs_float_invariant.c:68
foo
uint16_t foo
Definition: main_demo5.c:58
orientationGetRMat_f
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Definition: pprz_orientation_conversion.h:234
pprz_algebra_int.h
Paparazzi fixed point algebra.
ahrs_float_invariant_update_accel
void ahrs_float_invariant_update_accel(struct FloatVect3 *accel)
Definition: ahrs_float_invariant.c:231
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
inv_gains::ly
float ly
Tuning parameter of accel and mag on attitude (lateral subsystem)
Definition: ahrs_float_invariant.h:81
init_invariant_state
static void init_invariant_state(void)
Definition: ahrs_float_invariant.c:117
AHRS_INV_N
#define AHRS_INV_N
Definition: ahrs_float_invariant.c:88
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
AHRS_INV_MZ
#define AHRS_INV_MZ
Definition: ahrs_float_invariant.c:84
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
RATES_ASSIGN
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
INV_STATE_DIM
#define INV_STATE_DIM
Invariant filter state dimension.
Definition: ahrs_float_invariant.h:36
inv_gains::lx
float lx
Tuning parameter of accel and mag on attitude (longitudinal subsystem)
Definition: ahrs_float_invariant.h:80
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
AhrsFloatInv::state
struct inv_state state
state vector
Definition: ahrs_float_invariant.h:93
RATES_DIFF
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
FLOAT_VECT3_ZERO
#define FLOAT_VECT3_ZERO(_v)
Definition: pprz_algebra_float.h:161
orientationSetQuat_f
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
Definition: pprz_orientation_conversion.h:173
inv_gains::n
float n
Tuning parameter of accel and mag on accel bias (scaling subsystem)
Definition: ahrs_float_invariant.h:86
float_vect_zero
static void float_vect_zero(float *a, const int n)
a = 0
Definition: pprz_algebra_float.h:542
AhrsFloatInv::reset
bool reset
flag to request reset/reinit the filter
Definition: ahrs_float_invariant.h:99
inv_gains::my
float my
Tuning parameter of accel and mag on gyro bias (lateral subsystem)
Definition: ahrs_float_invariant.h:84
float_quat_normalize
static void float_quat_normalize(struct FloatQuat *q)
Definition: pprz_algebra_float.h:380
float_quat_invert
static void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
Definition: pprz_algebra_float.h:391
runge_kutta_4_float
static void runge_kutta_4_float(float *xo, const float *x, const int n, const float *u, const int m, void(*f)(float *o, const float *x, const int n, const float *u, const int m), const float dt)
Fourth-Order Runge-Kutta.
Definition: pprz_rk_float.h:132
FLOAT_QUAT_NORM2
#define FLOAT_QUAT_NORM2(_q)
Definition: pprz_algebra_float.h:373
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
inv_correction_gains::NE
float NE
Correction gains on accel bias.
Definition: ahrs_float_invariant.h:73
AhrsFloatInv::is_aligned
bool is_aligned
Definition: ahrs_float_invariant.h:105
AHRS_INV_MY
#define AHRS_INV_MY
Definition: ahrs_float_invariant.c:80
AhrsFloatInv::mag_h
struct FloatVect3 mag_h
Definition: ahrs_float_invariant.h:104
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
AhrsFloatInv::body_to_imu
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ahrs_float_invariant.h:102
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
AHRS_INV_LZ
#define AHRS_INV_LZ
Definition: ahrs_float_invariant.c:72
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
QUAT_SMUL
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:611
v1
@ v1
Definition: discrete_ekf_no_north.c:34
inv_command
Invariant filter command vector.
Definition: ahrs_float_invariant.h:64
inv_correction_gains::OE
float OE
Correction gains on magnetometer sensitivity.
Definition: ahrs_float_invariant.h:74
INV_COMMAND_DIM
#define INV_COMMAND_DIM
Invariant filter command vector dimension.
Definition: ahrs_float_invariant.h:60
inv_correction_gains::LE
struct FloatVect3 LE
Correction gains on attitude.
Definition: ahrs_float_invariant.h:71
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
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
B
#define B
Definition: ahrs_float_invariant.c:101
QUAT_ADD
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:619
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
invariant_model
static void invariant_model(float *o, const float *x, const int n, const float *u, const int m)
Compute dynamic mode.
Definition: ahrs_float_invariant.c:266
inv_gains::o
float o
Tuning parameter of accel and mag on mag bias (scaling subsystem)
Definition: ahrs_float_invariant.h:87
QUAT_ASSIGN
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:580
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
inv_gains::mx
float mx
Tuning parameter of accel and mag on gyro bias (longitudinal subsystem)
Definition: ahrs_float_invariant.h:83
FLOAT_QUAT_EXTRACT
#define FLOAT_QUAT_EXTRACT(_vo, _qi)
Definition: pprz_algebra_float.h:403
ahrs_float_inv
struct AhrsFloatInv ahrs_float_inv
Definition: ahrs_float_invariant.c:95
AHRS_INV_LX
#define AHRS_INV_LX
Definition: ahrs_float_invariant.c:64
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
inv_state::cs
float cs
Estimates magnetometer sensitivity.
Definition: ahrs_float_invariant.h:43
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
inv_state::as
float as
Estimated accelerometer sensitivity.
Definition: ahrs_float_invariant.h:44
ahrs_float_get_quat_from_accel_mag
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
Definition: ahrs_float_utils.h:85
AhrsFloatInv::cmd
struct inv_command cmd
command vector
Definition: ahrs_float_invariant.h:95
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
inv_measures::mag
struct FloatVect3 mag
Measured magnetic field.
Definition: ahrs_float_invariant.h:55
AhrsFloatInv::meas
struct inv_measures meas
measurement vector
Definition: ahrs_float_invariant.h:94
float_eulers_of_quat
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
Definition: pprz_algebra_float.c:688
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
AhrsFloatInv
Invariant filter structure.
Definition: ahrs_float_invariant.h:92
FloatRMat::m
float m[3 *3]
Definition: pprz_algebra_float.h:78
inv_gains::lz
float lz
Tuning parameter of accel and mag on attitude (heading subsystem)
Definition: ahrs_float_invariant.h:82
ahrs_float_invariant_align
void ahrs_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
Definition: ahrs_float_invariant.c:156
ahrs_float_invariant_init
void ahrs_float_invariant_init(void)
Definition: ahrs_float_invariant.c:131
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
inv_gains::mz
float mz
Tuning parameter of accel and mag on gyro bias (heading subsystem)
Definition: ahrs_float_invariant.h:85
AhrsFloatInv::corr
struct inv_correction_gains corr
correction gains
Definition: ahrs_float_invariant.h:96
inv_state::quat
struct FloatQuat quat
Estimated attitude (quaternion)
Definition: ahrs_float_invariant.h:41
FloatRates
angular rates
Definition: pprz_algebra_float.h:93