Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ins_float_invariant.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013 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 "subsystems/ins.h"
34 #include "subsystems/gps.h"
35 
36 #include "generated/airframe.h"
37 #include "generated/flight_plan.h"
38 #if INS_FINV_USE_UTM
40 #endif
41 
43 #include "math/pprz_algebra_int.h"
44 #include "math/pprz_rk_float.h"
45 #include "math/pprz_isa.h"
46 
47 #include "state.h"
48 
49 // for debugging
50 #if SEND_INVARIANT_FILTER || PERIODIC_TELEMETRY
52 #endif
53 
54 #if LOG_INVARIANT_FILTER
56 bool log_started = false;
57 #endif
58 
59 /*------------- =*= Invariant Observers =*= -------------*
60  *
61  * State vector :
62  *
63  * x = [q0 q1 q2 q3 vx vy vz px py pz wb1 wb2 wb3 hb as]'
64  *
65  * Dynamic model (dim = 15) :
66  *
67  * x_qdot = 0.5 * x_quat * ( x_rates - x_bias );
68  * x_Vdot = A + 1/as (q * am * (q)-1);
69  * x_Xdot = V;
70  * x_bias_dot = 0;
71  * x_asdot = 0;
72  * x_hbdot = 0;
73  *
74  * Observation model (dim = 10):
75  * yv = V;
76  * yx = X;
77  * yh = <X,e3> - hb;
78  * yb = (q)-1 *B * q; (B : magnetometers)
79  *
80  *------------------------------------------------------*/
81 
82 // Default values for the tuning gains
83 // Tuning parameter of speed error on attitude (e-2)
84 #ifndef INS_INV_LV
85 #define INS_INV_LV 2.
86 #endif
87 // Tuning parameter of mag error on attitude (e-2)
88 #ifndef INS_INV_LB
89 #define INS_INV_LB 6.
90 #endif
91 // Tuning parameter of horizontal speed error on speed
92 #ifndef INS_INV_MV
93 #define INS_INV_MV 8.
94 #endif
95 // Tuning parameter of vertical speed error on speed
96 #ifndef INS_INV_MVZ
97 #define INS_INV_MVZ 15.
98 #endif
99 // Tuning parameter of baro error on vertical speed
100 #ifndef INS_INV_MH
101 #define INS_INV_MH 0.2
102 #endif
103 // Tuning parameter of horizontal position error on position
104 #ifndef INS_INV_NX
105 #define INS_INV_NX 0.8
106 #endif
107 // Tuning parameter of vertical position error on position
108 #ifndef INS_INV_NXZ
109 #define INS_INV_NXZ 0.5
110 #endif
111 // Tuning parameter of baro error on vertical position
112 #ifndef INS_INV_NH
113 #define INS_INV_NH 1.2
114 #endif
115 // Tuning parameter of speed error on gyro biases (e-3)
116 #ifndef INS_INV_OV
117 #define INS_INV_OV 1.2
118 #endif
119 // Tuning parameter of mag error on gyro biases (e-3)
120 #ifndef INS_INV_OB
121 #define INS_INV_OB 1.
122 #endif
123 // Tuning parameter of speed error on accel biases (e-2)
124 #ifndef INS_INV_RV
125 #define INS_INV_RV 4.
126 #endif
127 // Tuning parameter of baro error on accel biases (vertical projection) (e-8)
128 #ifndef INS_INV_RH
129 #define INS_INV_RH 8.
130 #endif
131 // Tuning parameter of baro error on baro bias
132 #ifndef INS_INV_SH
133 #define INS_INV_SH 0.01
134 #endif
135 
136 
138 
139 /* earth gravity model */
140 static const struct FloatVect3 A = { 0.f, 0.f, 9.81f };
141 
142 /* earth magnetic model */
143 //static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) };
144 #define B ins_float_inv.mag_h
145 
146 /* barometer */
148 
149 /* gps */
151 
152 /* min speed to update heading from GPS when mag are not used */
153 #ifndef INS_INV_HEADING_UPDATE_GPS_MIN_SPEED
154 #define INS_INV_HEADING_UPDATE_GPS_MIN_SPEED 5.f
155 #endif
156 
157 /* error computation */
158 static inline void error_output(struct InsFloatInv *_ins);
159 
160 /* propagation model (called by runge-kutta library) */
161 static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m);
162 
163 
167 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
168  struct FloatVect3 *vi);
169 
170 
171 /* init state and measurements */
172 static inline void init_invariant_state(void)
173 {
174  // init state
179  ins_float_inv.state.as = 1.0f;
180  ins_float_inv.state.hb = 0.0f;
181 
182  // init measures
185  ins_float_inv.meas.baro_alt = 0.0f;
186 
187  // init baro
188  ins_baro_initialized = false;
189  ins_gps_fix_once = false;
190 }
191 
192 #if SEND_INVARIANT_FILTER || PERIODIC_TELEMETRY
193 static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
194 {
195  struct FloatEulers eulers;
197  pprz_msg_send_INV_FILTER(trans, dev,
198  AC_ID,
200  &eulers.phi,
201  &eulers.theta,
202  &eulers.psi,
216 }
217 #endif
218 
220 {
221 
222  // init position
223 #if INS_FINV_USE_UTM
224  struct UtmCoor_f utm0;
225  utm0.north = (float)nav_utm_north0;
226  utm0.east = (float)nav_utm_east0;
227  utm0.alt = GROUND_ALT;
228  utm0.zone = nav_utm_zone0;
230  stateSetPositionUtm_f(&utm0);
231 #else
232  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
233  llh_nav0.lat = NAV_LAT0;
234  llh_nav0.lon = NAV_LON0;
235  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
236  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
237  struct EcefCoor_i ecef_nav0;
238  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
239  struct LtpDef_i ltp_def;
240  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
241  ltp_def.hmsl = NAV_ALT0;
243 #endif
244 
245 #if USE_MAGNETOMETER
246  B.x = INS_H_X;
247  B.y = INS_H_Y;
248  B.z = INS_H_Z;
249 #else
250  B.x = 1.f; // when using GPS as magnetometer, mag field is true north
251  B.y = 0.f;
252  B.z = 0.f;
253 #endif
254 
255  // init state and measurements
257 
258  // init gains
272 
273  ins_float_inv.is_aligned = false;
274  ins_float_inv.reset = false;
275 
276 #if PERIODIC_TELEMETRY
278 #endif
279 }
280 
281 
283 {
284 #if INS_FINV_USE_UTM
285  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
286  // reset state UTM ref
288 #else
289  struct LtpDef_i ltp_def;
291  ltp_def.hmsl = gps.hmsl;
293 #endif
294 }
295 
297 {
298 #if INS_FINV_USE_UTM
299  struct UtmCoor_f utm = state.utm_origin_f;
300  utm.alt = gps.hmsl / 1000.0f;
302 #else
303  struct LlaCoor_i lla = {
305  .lon = state.ned_origin_i.lla.lon,
306  .alt = gps.lla_pos.alt
307  };
308  struct LtpDef_i ltp_def;
310  ltp_def.hmsl = gps.hmsl;
312 #endif
313 }
314 
316  struct FloatVect3 *lp_accel,
317  struct FloatVect3 *lp_mag __attribute__((unused)))
318 {
319 #if USE_MAGNETOMETER
320  /* Compute an initial orientation from accel and mag directly as quaternion */
322 #else
323  /* Compute an initial orientation from accel only directly as quaternion */
325 #endif
326 
327  /* use average gyro as initial value for bias */
328  ins_float_inv.state.bias = *lp_gyro;
329 
330  /* push initial values to state interface */
332 
333  // ins and ahrs are now running
334  ins_float_inv.is_aligned = true;
335 }
336 
337 void ins_float_invariant_propagate(struct FloatRates* gyro, struct FloatVect3* accel, float dt)
338 {
339  struct FloatRates body_rates;
340 
341  // realign all the filter if needed
342  // a complete init cycle is required
343  if (ins_float_inv.reset) {
344  ins_float_inv.reset = false;
345  ins_float_inv.is_aligned = false;
347  }
348 
349  // fill command vector in body frame
350  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
351  float_rmat_transp_ratemult(&ins_float_inv.cmd.rates, body_to_imu_rmat, gyro);
352  float_rmat_transp_vmult(&ins_float_inv.cmd.accel, body_to_imu_rmat, accel);
353 
354  struct Int32Vect3 body_accel_i;
355  ACCELS_BFP_OF_REAL(body_accel_i, ins_float_inv.cmd.accel);
356  stateSetAccelBody_i(&body_accel_i);
357 
358  // update correction gains
360 
361  // propagate model
362  struct inv_state new_state;
363  runge_kutta_4_float((float *)&new_state,
364  (float *)&ins_float_inv.state, INV_STATE_DIM,
365  (float *)&ins_float_inv.cmd, INV_COMMAND_DIM,
366  invariant_model, dt);
367  ins_float_inv.state = new_state;
368 
369  // normalize quaternion
371 
372  // set global state
375  stateSetBodyRates_f(&body_rates);
378  // untilt accel and remove gravity
379  struct FloatQuat q_b2n;
381  struct FloatVect3 accel_n;
382  float_quat_vmult(&accel_n, &q_b2n, &ins_float_inv.cmd.accel);
383  VECT3_SMUL(accel_n, accel_n, 1. / (ins_float_inv.state.as));
384  VECT3_ADD(accel_n, A);
385  stateSetAccelNed_f((struct NedCoor_f *)&accel_n);
386 
387  //------------------------------------------------------------//
388 
389 #if SEND_INVARIANT_FILTER
390  RunOnceEvery(3, send_inv_filter(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
391 #endif
392 
393 #if LOG_INVARIANT_FILTER
394  if (pprzLogFile != -1) {
395  if (!log_started) {
396  // log file header
397  sdLogWriteLog(pprzLogFile,
398  "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
399  log_started = true;
400  } else {
401  sdLogWriteLog(pprzLogFile,
402  "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
434  }
435  }
436 #endif
437 }
438 
440 {
441 
442  if (gps_s->fix >= GPS_FIX_3D && ins_float_inv.is_aligned) {
443  ins_gps_fix_once = true;
444 
445 #if INS_FINV_USE_UTM
446  if (state.utm_initialized_f) {
447  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
448  // position (local ned)
452  // speed
453  ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f;
454  ins_float_inv.meas.speed_gps.y = gps_s->ned_vel.y / 100.0f;
455  ins_float_inv.meas.speed_gps.z = gps_s->ned_vel.z / 100.0f;
456  }
457 #else
458  if (state.ned_initialized_f) {
459  struct NedCoor_i gps_pos_cm_ned, ned_pos;
460  ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &gps_s->ecef_pos);
463  struct EcefCoor_f ecef_vel;
464  ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel);
466  }
467 #endif
468  }
469 
470 #if !USE_MAGNETOMETER
471  // Use pseudo-mag rebuilt from GPS horizontal velocity
473  float vel_norm = float_vect2_norm(&vel);
474  if (vel_norm > INS_INV_HEADING_UPDATE_GPS_MIN_SPEED) {
475  struct FloatVect3 pseudo_mag = {
476  vel.x / vel_norm,
477  -vel.y / vel_norm,
478  0.f
479  };
480  ins_float_invariant_update_mag(&pseudo_mag);
481  }
482  else {
483  // if speed is tool low, better set measurements to zero
485  }
486 #endif
487 
488 }
489 
490 
492 {
493  static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE;
494  static float alpha = 10.0f;
495  static int32_t i = 1;
496  static float baro_moy = 0.0f;
497  static float baro_prev = 0.0f;
498 
499  if (!ins_baro_initialized) {
500  // try to find a stable qfe
501  // TODO generic function in pprz_isa ?
502  if (i == 1) {
503  baro_moy = pressure;
504  baro_prev = pressure;
505  }
506  baro_moy = (baro_moy * (i - 1) + pressure) / i;
507  alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f);
508  baro_prev = baro_moy;
509  // test stop condition
510  if (fabs(alpha) < 0.1f) {
511  ins_qfe = baro_moy;
512  ins_baro_initialized = true;
513  }
514  if (i == 250) {
515  ins_qfe = pressure;
516  ins_baro_initialized = true;
517  }
518  i++;
519  } else { /* normal update with baro measurement */
520  ins_float_inv.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
521  }
522 }
523 
524 // assume mag is dead when values are not moving anymore
525 #define MAG_FROZEN_COUNT 30
526 
528 {
529  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
530  static int32_t last_mx = 0;
531 
532  if (last_mx == mag->x) {
533  mag_frozen_count--;
534  if (mag_frozen_count == 0) {
535  // if mag is dead, better set measurements to zero
537  mag_frozen_count = MAG_FROZEN_COUNT;
538  }
539  } else {
540  // values are moving
541  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
542  // new values in body frame
543  float_rmat_transp_vmult(&ins_float_inv.meas.mag, body_to_imu_rmat, mag);
544  // reset counter
545  mag_frozen_count = MAG_FROZEN_COUNT;
546  }
547  last_mx = mag->x;
548 }
549 
550 
555 static inline void invariant_model(float *o, const float *x, const int n, const float *u,
556  const int m __attribute__((unused)))
557 {
558 
559 #pragma GCC diagnostic push // require GCC 4.6
560 #pragma GCC diagnostic ignored "-Wcast-qual"
561  struct inv_state *s = (struct inv_state *)x;
562  struct inv_command *c = (struct inv_command *)u;
563 #pragma GCC diagnostic pop // require GCC 4.6
564  struct inv_state s_dot;
565  struct FloatRates rates_unbiased;
566  struct FloatVect3 tmp_vect;
567  struct FloatQuat tmp_quat;
568 
569  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
570  RATES_DIFF(rates_unbiased, c->rates, s->bias);
571  /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
572  float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
573 
574  float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_float_inv.corr.LE);
575  QUAT_ADD(s_dot.quat, tmp_quat);
576 
577  float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat);
578  QUAT_SMUL(tmp_quat, s->quat, norm2_r);
579  QUAT_ADD(s_dot.quat, tmp_quat);
580 
581  /* dot_V = A + (1/as) * (q * am * q-1) + ME */
582  struct FloatQuat q_b2n;
583  float_quat_invert(&q_b2n, &(s->quat));
584  float_quat_vmult((struct FloatVect3 *)&s_dot.speed, &q_b2n, &(c->accel));
585  VECT3_SMUL(s_dot.speed, s_dot.speed, 1. / (s->as));
586  VECT3_ADD(s_dot.speed, A);
588 
589  /* dot_X = V + NE */
590  VECT3_SUM(s_dot.pos, s->speed, ins_float_inv.corr.NE);
591 
592  /* bias_dot = q-1 * (OE) * q */
593  float_quat_vmult(&tmp_vect, &(s->quat), &ins_float_inv.corr.OE);
594  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
595 
596  /* as_dot = as * RE */
597  s_dot.as = (s->as) * (ins_float_inv.corr.RE);
598  // keep as in a reasonable range, so 50% around the nominal value
599  if (s->as < 0.5f || s->as > 1.5f) {
600  s_dot.as = 0.f;
601  }
602 
603  /* hb_dot = SE */
604  s_dot.hb = ins_float_inv.corr.SE;
605 
606  // set output
607  memcpy(o, &s_dot, n * sizeof(float));
608 }
609 
614 static inline void error_output(struct InsFloatInv *_ins)
615 {
616 
617  struct FloatVect3 YBt, I, Ev, Eb, Ex, Itemp, Ebtemp, Evtemp;
618  float Eh;
619  float temp;
620 
621  /* YBt = q * yB * q-1 */
622  struct FloatQuat q_b2n;
623  float_quat_invert(&q_b2n, &(_ins->state.quat));
624  float_quat_vmult(&YBt, &q_b2n, &(_ins->meas.mag));
625 
626  float_quat_vmult(&I, &q_b2n, &(_ins->cmd.accel));
627  VECT3_SMUL(I, I, 1. / (_ins->state.as));
628 
629  /*--------- E = ( ŷ - y ) ----------*/
630  /* Eb = ( B - YBt ) */
631  VECT3_DIFF(Eb, B, YBt);
632 
633  // pos and speed error only if GPS data are valid
634  // or while waiting first GPS data to prevent diverging
636 #if INS_FINV_USE_UTM
638 #else
640 #endif
641  ) || !ins_gps_fix_once) {
642  /* Ev = (V - YV) */
643  VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
644  /* Ex = (X - YX) */
645  VECT3_DIFF(Ex, _ins->state.pos, _ins->meas.pos_gps);
646  } else {
647  FLOAT_VECT3_ZERO(Ev);
649  }
650  /* Eh = < X,e3 > - hb - YH */
651  Eh = _ins->state.pos.z - _ins->state.hb - _ins->meas.baro_alt;
652 
653  /*--------------Gains--------------*/
654 
655  /**** LvEv + LbEb = -lvIa x Ev + lb < B x Eb, Ia > Ia *****/
656  VECT3_SMUL(Itemp, I, -_ins->gains.lv / 100.);
657  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
658 
659  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
660  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
661  temp = (_ins->gains.lb / 100.) * temp;
662 
663  VECT3_SMUL(Ebtemp, I, temp);
664  VECT3_ADD(Evtemp, Ebtemp);
665  VECT3_COPY(_ins->corr.LE, Evtemp);
666 
667  /***** MvEv + MhEh = -mv * Ev + (-mh * <Eh,e3>)********/
668  _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.;
669  _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.;
670  _ins->corr.ME.z = ((-_ins->gains.mvz) * Ev.z) + ((-_ins->gains.mh) * Eh);
671 
672  /****** NxEx + NhEh = -nx * Ex + (-nh * <Eh, e3>) ********/
673  _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.;
674  _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.;
675  _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh);
676 
677  /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/
678  VECT3_SMUL(Itemp, I, _ins->gains.ov / 1000.);
679  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
680 
681  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
682  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
683  temp = (-_ins->gains.ob / 1000.) * temp;
684 
685  VECT3_SMUL(Ebtemp, I, temp);
686  VECT3_ADD(Evtemp, Ebtemp);
687  VECT3_COPY(_ins->corr.OE, Evtemp);
688 
689  /* a scalar */
690  /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/
691  _ins->corr.RE = ((_ins->gains.rv / 100.) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh / 10000.) * Eh);
692 
693  /****** ShEh ******/
694  _ins->corr.SE = (_ins->gains.sh) * Eh;
695 
696 }
697 
698 
699 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
700  struct FloatVect3 *vi)
701 {
702  struct FloatVect3 qvec, v1, v2;
703  float qi;
704 
705  FLOAT_QUAT_EXTRACT(qvec, *q);
706  qi = - VECT3_DOT_PRODUCT(*vi, qvec);
707  VECT3_CROSS_PRODUCT(v1, *vi, qvec);
708  VECT3_SMUL(v2, *vi, q->qi);
709  VECT3_ADD(v2, v1);
710  QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
711 }
712 
714 {
716 
717  if (!ins_float_inv.is_aligned) {
718  /* Set ltp_to_imu so that body is zero */
719  ins_float_inv.state.quat = *q_b2i;
720  }
721 }
inv_gains::mvz
float mvz
Tuning parameter of vertical speed error on speed.
Definition: ins_float_invariant.h:92
inv_gains::nxz
float nxz
Tuning parameter of vertical position error on position.
Definition: ins_float_invariant.h:95
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
ins_baro_initialized
bool ins_baro_initialized
Definition: ins_float_invariant.c:147
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
InsFloatInv::reset
bool reset
flag to request reset/reinit the filter
Definition: ins_float_invariant.h:113
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: ins_float_invariant.c:699
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
send_inv_filter
static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
Definition: ins_float_invariant.c:193
ins.h
pprzLogFile
FileDes pprzLogFile
Definition: sdlog_chibios.c:86
NedCoor_f
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_float.h:63
inv_correction_gains::ME
struct FloatVect3 ME
Correction gains on gyro biases.
Definition: ahrs_float_invariant.h:72
inv_gains::rh
float rh
Tuning parameter of baro error on accel biases (vertical projection)
Definition: ins_float_invariant.h:100
inv_gains::sh
float sh
Tuning parameter of baro error on baro bias.
Definition: ins_float_invariant.h:101
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
INT32_VECT3_SCALE_2
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
Definition: pprz_algebra_int.h:290
stateSetSpeedNed_f
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
ahrs_int_utils.h
ltp_def_from_ecef_i
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
Definition: pprz_geodetic_int.c:60
VECT3_SMUL
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
float_quat_identity
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
Definition: pprz_algebra_float.h:365
UtmCoor_f::north
float north
in meters
Definition: pprz_geodetic_float.h:82
invariant_model
static void invariant_model(float *o, const float *x, const int n, const float *u, const int m)
Compute dynamic mode.
Definition: ins_float_invariant.c:555
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
InsFloatInv::cmd
struct inv_command cmd
command vector
Definition: ins_float_invariant.h:109
INS_INV_HEADING_UPDATE_GPS_MIN_SPEED
#define INS_INV_HEADING_UPDATE_GPS_MIN_SPEED
Definition: ins_float_invariant.c:154
NedCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:66
INS_INV_MV
#define INS_INV_MV
Definition: ins_float_invariant.c:93
vi
struct VehicleInterface vi
Definition: vi.c:30
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
nav_utm_east0
int32_t nav_utm_east0
Definition: common_nav.c:42
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
INS_INV_NH
#define INS_INV_NH
Definition: ins_float_invariant.c:113
LtpDef_i
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_int.h:98
VECT3_CROSS_PRODUCT
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:244
B
#define B
Definition: ins_float_invariant.c:144
inv_state::speed
struct NedCoor_f speed
Estimates speed.
Definition: ins_float_invariant.h:44
InsFloatInv::corr
struct inv_correction_gains corr
correction gains
Definition: ins_float_invariant.h:110
ned_of_ecef_vect_f
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
Definition: pprz_geodetic_float.c:105
GpsState
data structure for GPS information
Definition: gps.h:87
s
static uint32_t s
Definition: light_scheduler.c:33
VECT3_SUM
#define VECT3_SUM(_c, _a, _b)
Definition: pprz_algebra.h:161
inv_state
Invariant filter state.
Definition: ahrs_float_invariant.h:40
INS_INV_MVZ
#define INS_INV_MVZ
Definition: ins_float_invariant.c:97
UtmCoor_f::east
float east
in meters
Definition: pprz_geodetic_float.h:83
uint32_t
unsigned long uint32_t
Definition: types.h:18
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
ecef_of_lla_i
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
Definition: pprz_geodetic_int.c:392
alpha
float alpha
Definition: textons.c:107
ins_float_invariant.h
init_invariant_state
static void init_invariant_state(void)
Definition: ins_float_invariant.c:172
stateSetPositionUtm_f
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
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
stateSetAccelBody_i
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
nav_utm_north0
int32_t nav_utm_north0
Definition: common_nav.c:43
INT32_POS_OF_CM_NUM
#define INT32_POS_OF_CM_NUM
Definition: pprz_algebra_int.h:70
inv_correction_gains::RE
float RE
Correction gains on accel bias.
Definition: ins_float_invariant.h:82
pprz_rk_float.h
Runge-Kutta library (float version)
INS_INV_MH
#define INS_INV_MH
Definition: ins_float_invariant.c:101
GpsState::ned_vel
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
INT32_POS_OF_CM_DEN
#define INT32_POS_OF_CM_DEN
Definition: pprz_algebra_int.h:71
orientationGetRMat_f
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Definition: pprz_orientation_conversion.h:234
inv_gains::ov
float ov
Tuning parameter of speed error on gyro biases.
Definition: ins_float_invariant.h:97
inv_gains::lv
float lv
Tuning parameter of speed error on attitude.
Definition: ins_float_invariant.h:89
pprz_algebra_int.h
Paparazzi fixed point algebra.
inv_gains::mv
float mv
Tuning parameter of horizontal speed error on speed.
Definition: ins_float_invariant.h:91
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
sdlog_chibios.h
InsFloatInv
Invariant filter structure.
Definition: ins_float_invariant.h:106
inv_state::pos
struct NedCoor_f pos
Estimates position.
Definition: ins_float_invariant.h:45
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
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
NED_FLOAT_OF_BFP
#define NED_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:196
telemetry.h
ins_reset_altitude_ref
void ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins_float_invariant.c:296
error_output
static void error_output(struct InsFloatInv *_ins)
Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error )
Definition: ins_float_invariant.c:614
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
RATES_ASSIGN
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
stateSetLocalUtmOrigin_f
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
INV_STATE_DIM
#define INV_STATE_DIM
Invariant filter state dimension.
Definition: ahrs_float_invariant.h:36
float_vect2_norm
static float float_vect2_norm(struct FloatVect2 *v)
Definition: pprz_algebra_float.h:139
ACCELS_BFP_OF_REAL
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
I
static float I
Definition: trilateration.c:35
InsFloatInv::gains
struct inv_gains gains
tuning gains
Definition: ins_float_invariant.h:111
INS_INV_NX
#define INS_INV_NX
Definition: ins_float_invariant.c:105
gps.h
Device independent GPS code (interface)
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:107
Ex
static float Ex[3]
Definition: trilateration.c:33
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
ins_float_invariant_init
void ins_float_invariant_init(void)
Definition: ins_float_invariant.c:219
UtmCoor_f::alt
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
Definition: pprz_geodetic_float.h:84
RATES_DIFF
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
UtmCoor_f::zone
uint8_t zone
UTM zone number.
Definition: pprz_geodetic_float.h:85
inv_correction_gains::SE
float SE
Correction gains on barometer bias.
Definition: ins_float_invariant.h:83
FLOAT_VECT3_ZERO
#define FLOAT_VECT3_ZERO(_v)
Definition: pprz_algebra_float.h:161
log_started
bool log_started
Definition: meteo_france_DAQ.c:47
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
EcefCoor_i
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_int.h:50
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
stateSetBodyRates_f
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
ECEF_FLOAT_OF_BFP
#define ECEF_FLOAT_OF_BFP(_o, _i)
Definition: pprz_geodetic_int.h:160
State::utm_initialized_f
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
float_quat_normalize
static void float_quat_normalize(struct FloatQuat *q)
Definition: pprz_algebra_float.h:380
Int32Vect3
Definition: pprz_algebra_int.h:88
float_quat_invert
static void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
Definition: pprz_algebra_float.h:391
InsFloatInv::meas
struct inv_measures meas
measurement vector
Definition: ins_float_invariant.h:108
InsFloatInv::state
struct inv_state state
state vector
Definition: ins_float_invariant.h:107
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
inv_gains::nh
float nh
Tuning parameter of baro error on vertical position.
Definition: ins_float_invariant.h:96
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
State::ned_origin_f
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
FLOAT_QUAT_NORM2
#define FLOAT_QUAT_NORM2(_q)
Definition: pprz_algebra_float.h:373
inv_measures::pos_gps
struct NedCoor_f pos_gps
Measured gps position.
Definition: ins_float_invariant.h:58
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
stateSetLocalOrigin_i
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
ins_float_invariant_update_mag
void ins_float_invariant_update_mag(struct FloatVect3 *mag)
Definition: ins_float_invariant.c:527
inv_correction_gains::NE
float NE
Correction gains on accel bias.
Definition: ahrs_float_invariant.h:73
ins_float_invariant_update_gps
void ins_float_invariant_update_gps(struct GpsState *gps_s)
Definition: ins_float_invariant.c:439
inv_gains::lb
float lb
Tuning parameter of mag error on attitude.
Definition: ins_float_invariant.h:90
inv_gains::mh
float mh
Tuning parameter of baro error on vertical speed.
Definition: ins_float_invariant.h:93
inv_measures::speed_gps
struct NedCoor_f speed_gps
Measured gps speed.
Definition: ins_float_invariant.h:59
ltp_def_from_lla_i
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
Definition: pprz_geodetic_int.c:72
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
pprz_isa_height_of_pressure
static float pprz_isa_height_of_pressure(float pressure, float ref_p)
Get relative altitude from pressure (using simplified equation).
Definition: pprz_isa.h:102
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
stateSetAccelNed_f
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
QUAT_SMUL
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:611
ins_gps_fix_once
bool ins_gps_fix_once
Definition: ins_float_invariant.c:150
INS_INV_RH
#define INS_INV_RH
Definition: ins_float_invariant.c:129
v1
@ v1
Definition: discrete_ekf_no_north.c:34
INS_INV_LV
#define INS_INV_LV
Definition: ins_float_invariant.c:85
GpsState::ecef_pos
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
inv_command
Invariant filter command vector.
Definition: ahrs_float_invariant.h:64
INS_INV_SH
#define INS_INV_SH
Definition: ins_float_invariant.c:133
inv_correction_gains::OE
float OE
Correction gains on magnetometer sensitivity.
Definition: ahrs_float_invariant.h:74
utm_float_from_gps
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
Convenience functions to get utm position from GPS state.
Definition: gps.c:390
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
nav.h
ahrs_float_get_quat_from_accel
static void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct FloatVect3 *accel)
Compute a quaternion representing roll and pitch from an accelerometer measurement.
Definition: ahrs_float_utils.h:61
ins_float_invariant_align
void ins_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
Definition: ins_float_invariant.c:315
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
QUAT_ADD
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:619
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
InsFloatInv::is_aligned
bool is_aligned
Definition: ins_float_invariant.h:119
int32_t
signed long int32_t
Definition: types.h:19
ins_reset_local_origin
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_float_invariant.c:282
EcefCoor_f
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_float.h:45
QUAT_ASSIGN
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:580
stateSetPositionNed_f
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
UtmCoor_f
position in UTM coordinates Units: meters
Definition: pprz_geodetic_float.h:81
FLOAT_QUAT_EXTRACT
#define FLOAT_QUAT_EXTRACT(_vo, _qi)
Definition: pprz_algebra_float.h:403
inv_command::accel
struct FloatVect3 accel
Input accelerometers.
Definition: ins_float_invariant.h:72
nav_utm_zone0
uint8_t nav_utm_zone0
Definition: common_nav.c:44
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
INS_INV_NXZ
#define INS_INV_NXZ
Definition: ins_float_invariant.c:109
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
MAG_FROZEN_COUNT
#define MAG_FROZEN_COUNT
Definition: ins_float_invariant.c:525
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
A
static const struct FloatVect3 A
Definition: ins_float_invariant.c:140
InsFloatInv::body_to_imu
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ins_float_invariant.h:116
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
State::utm_origin_f
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
inv_state::as
float as
Estimated accelerometer sensitivity.
Definition: ahrs_float_invariant.h:44
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
State::ned_initialized_f
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
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
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
INS_INV_OB
#define INS_INV_OB
Definition: ins_float_invariant.c:121
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
INS_INV_OV
#define INS_INV_OV
Definition: ins_float_invariant.c:117
ins_float_invariant_update_baro
void ins_float_invariant_update_baro(float pressure)
Definition: ins_float_invariant.c:491
ins_float_inv
struct InsFloatInv ins_float_inv
Definition: ins_float_invariant.c:137
ned_of_ecef_point_i
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local NED.
Definition: pprz_geodetic_int.c:116
inv_gains::nx
float nx
Tuning parameter of horizontal position error on position.
Definition: ins_float_invariant.h:94
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
inv_measures::mag
struct FloatVect3 mag
Measured magnetic field.
Definition: ahrs_float_invariant.h:55
ins_float_invariant_propagate
void ins_float_invariant_propagate(struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
Definition: ins_float_invariant.c:337
state.h
INS_INV_LB
#define INS_INV_LB
Definition: ins_float_invariant.c:89
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
GpsState::ecef_vel
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
FloatRMat::m
float m[3 *3]
Definition: pprz_algebra_float.h:78
INS_INV_RV
#define INS_INV_RV
Definition: ins_float_invariant.c:125
state
struct State state
Definition: state.c:36
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
ins_float_inv_set_body_to_imu_quat
void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ins_float_invariant.c:713
stateSetNedToBodyQuat_f
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
PPRZ_ISA_SEA_LEVEL_PRESSURE
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition: pprz_isa.h:50
inv_state::quat
struct FloatQuat quat
Estimated attitude (quaternion)
Definition: ahrs_float_invariant.h:41
inv_gains::rv
float rv
Tuning parameter of speed error on accel biases.
Definition: ins_float_invariant.h:99
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
pprz_isa.h
Paparazzi atmospheric pressure conversion utilities.
inv_state::hb
float hb
Estimates barometers bias.
Definition: ins_float_invariant.h:46
inv_measures::baro_alt
float baro_alt
Measured barometric altitude.
Definition: ins_float_invariant.h:61
inv_gains::ob
float ob
Tuning parameter of mag error on gyro biases.
Definition: ins_float_invariant.h:98