Paparazzi UAS  v6.2_unstable
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 "modules/ins/ins.h"
34 #include "modules/gps/gps.h"
35 
36 #include "generated/airframe.h"
37 #include "generated/flight_plan.h"
38 #if FIXEDWING_FIRMWARE
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.f
86 #endif
87 // Tuning parameter of mag error on attitude (e-2)
88 #ifndef INS_INV_LB
89 #define INS_INV_LB 6.f
90 #endif
91 // Tuning parameter of horizontal speed error on speed
92 #ifndef INS_INV_MV
93 #define INS_INV_MV 8.f
94 #endif
95 // Tuning parameter of vertical speed error on speed
96 #ifndef INS_INV_MVZ
97 #define INS_INV_MVZ 15.f
98 #endif
99 // Tuning parameter of baro error on vertical speed
100 #ifndef INS_INV_MH
101 #define INS_INV_MH 0.2f
102 #endif
103 // Tuning parameter of horizontal position error on position
104 #ifndef INS_INV_NX
105 #define INS_INV_NX 0.8f
106 #endif
107 // Tuning parameter of vertical position error on position
108 #ifndef INS_INV_NXZ
109 #define INS_INV_NXZ 0.5f
110 #endif
111 // Tuning parameter of baro error on vertical position
112 #ifndef INS_INV_NH
113 #define INS_INV_NH 1.2f
114 #endif
115 // Tuning parameter of speed error on gyro biases (e-3)
116 #ifndef INS_INV_OV
117 #define INS_INV_OV 1.2f
118 #endif
119 // Tuning parameter of mag error on gyro biases (e-3)
120 #ifndef INS_INV_OB
121 #define INS_INV_OB 1.f
122 #endif
123 // Tuning parameter of speed error on accel biases (e-2)
124 #ifndef INS_INV_RV
125 #define INS_INV_RV 4.f
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.f
130 #endif
131 // Tuning parameter of baro error on baro bias
132 #ifndef INS_INV_SH
133 #define INS_INV_SH 0.01f
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 FIXEDWING_FIRMWARE
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 FIXEDWING_FIRMWARE
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  // reset state position and velocity to zero
297 }
298 
300 {
301 #if FIXEDWING_FIRMWARE
302  struct UtmCoor_f utm = state.utm_origin_f;
303  utm.alt = gps.hmsl / 1000.0f;
305 #else
306  struct LlaCoor_i lla = {
308  .lon = state.ned_origin_i.lla.lon,
309  .alt = gps.lla_pos.alt
310  };
311  struct LtpDef_i ltp_def;
313  ltp_def.hmsl = gps.hmsl;
315 #endif
316  // reset vertical position and velocity to zero
317  ins_float_inv.state.pos.z = 0.f;
318  ins_float_inv.state.speed.z = 0.f;
319 }
320 
322  struct FloatVect3 *lp_accel,
323  struct FloatVect3 *lp_mag __attribute__((unused)))
324 {
325 #if USE_MAGNETOMETER
326  /* Compute an initial orientation from accel and mag directly as quaternion */
328 #else
329  /* Compute an initial orientation from accel only directly as quaternion */
331 #endif
332 
333  /* use average gyro as initial value for bias */
334  ins_float_inv.state.bias = *lp_gyro;
335 
336  /* push initial values to state interface */
338 
339  // ins and ahrs are now running
340  ins_float_inv.is_aligned = true;
341 }
342 
343 void ins_float_invariant_propagate(struct FloatRates* gyro, struct FloatVect3* accel, float dt)
344 {
345  struct FloatRates body_rates;
346 
347  // realign all the filter if needed
348  // a complete init cycle is required
349  if (ins_float_inv.reset) {
350  ins_float_inv.reset = false;
351  ins_float_inv.is_aligned = false;
353  }
354 
355  // fill command vector in body frame
356  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
357  float_rmat_transp_ratemult(&ins_float_inv.cmd.rates, body_to_imu_rmat, gyro);
358  float_rmat_transp_vmult(&ins_float_inv.cmd.accel, body_to_imu_rmat, accel);
359 
360  struct Int32Vect3 body_accel_i;
361  ACCELS_BFP_OF_REAL(body_accel_i, ins_float_inv.cmd.accel);
362  stateSetAccelBody_i(&body_accel_i);
363 
364  // update correction gains
366 
367  // propagate model
368  struct inv_state new_state;
369  runge_kutta_4_float((float *)&new_state,
370  (float *)&ins_float_inv.state, INV_STATE_DIM,
371  (float *)&ins_float_inv.cmd, INV_COMMAND_DIM,
372  invariant_model, dt);
373  ins_float_inv.state = new_state;
374 
375  // normalize quaternion
377 
378  // set global state
381  stateSetBodyRates_f(&body_rates);
384  // untilt accel and remove gravity
385  struct FloatQuat q_b2n;
387  struct FloatVect3 accel_n;
388  float_quat_vmult(&accel_n, &q_b2n, &ins_float_inv.cmd.accel);
389  VECT3_SMUL(accel_n, accel_n, 1.f / (ins_float_inv.state.as));
390  VECT3_ADD(accel_n, A);
391  stateSetAccelNed_f((struct NedCoor_f *)&accel_n);
392 
393  //------------------------------------------------------------//
394 
395 #if SEND_INVARIANT_FILTER
396  RunOnceEvery(3, send_inv_filter(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
397 #endif
398 
399 #if LOG_INVARIANT_FILTER
400  if (pprzLogFile != -1) {
401  if (!log_started) {
402  // log file header
403  sdLogWriteLog(pprzLogFile,
404  "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");
405  log_started = true;
406  } else {
407  sdLogWriteLog(pprzLogFile,
408  "%.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",
440  }
441  }
442 #endif
443 }
444 
446 {
447 
448  if (gps_s->fix >= GPS_FIX_3D && ins_float_inv.is_aligned) {
449 #if FIXEDWING_FIRMWARE
450  if (state.utm_initialized_f) {
451  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
452  // position (local ned)
456  // speed
457  ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f;
458  ins_float_inv.meas.speed_gps.y = gps_s->ned_vel.y / 100.0f;
459  ins_float_inv.meas.speed_gps.z = gps_s->ned_vel.z / 100.0f;
460  }
461 #else
462  if (state.ned_initialized_f) {
463  struct NedCoor_i gps_pos_cm_ned, ned_pos;
464  ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &gps_s->ecef_pos);
467  struct EcefCoor_f ecef_vel;
468  ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel);
470  }
471 #endif
472  // when getting first GPS pos (or after reset), set state to current pos and speed
473  if (!ins_gps_fix_once) {
476  ins_gps_fix_once = true;
477  }
478  }
479 
480 #if !USE_MAGNETOMETER
481  struct FloatVect3 pseudo_mag = { 0.f, 0.f, 0.f };
482  bool update_mag = false;
483 #if INS_INV_USE_GPS_HEADING
484  // got a 3d fix, ground speed > INS_INV_HEADING_UPDATE_GPS_MIN_SPEED (default 5.0 m/s)
485  // and course accuracy is better than 10deg
486  static const uint16_t gps_min_speed = INS_INV_HEADING_UPDATE_GPS_MIN_SPEED * 100;
487  static const uint32_t max_cacc = RadOfDeg(10 * 1e7);
488  if (gps_s->fix >= GPS_FIX_3D &&
489  gps_s->gspeed >= gps_min_speed &&
490  gps_s->cacc <= max_cacc) {
491  // gps_s->course is in rad * 1e7
492  float course = (float)gps_s->course / 1e7f;
493  pseudo_mag.x = cosf(course);
494  pseudo_mag.y = -sinf(course);
495  update_mag = true;
496  }
497 #else // else use GPS velocity (only for fixedwing)
498  // Use pseudo-mag rebuilt from GPS horizontal velocity
500  float vel_norm = float_vect2_norm(&vel);
501  if (vel_norm > INS_INV_HEADING_UPDATE_GPS_MIN_SPEED) {
502  pseudo_mag.x = vel.x / vel_norm;
503  pseudo_mag.y = -vel.y / vel_norm;
504  update_mag = true;
505  }
506 #endif
507  if (update_mag) {
508  // create quat corresponding to psi
509  // quat_psi = [ cos(psi/2) 0 0 sin(psi/2) ]'
510  struct FloatEulers angles;
512  struct FloatQuat quat_psi = { cosf(angles.psi / 2.f), 0.f, 0.f, sinf(angles.psi / 2.f) };
513  // build quat corresponding to roll and pitch (from quat psi and current quat)
514  struct FloatQuat quat_horiz;
515  float_quat_inv_comp_norm_shortest(&quat_horiz, &quat_psi, &ins_float_inv.state.quat);
516  // rotate pseudo mag around horiz quaternion
517  struct FloatVect3 mag_final;
518  float_quat_vmult(&mag_final, &quat_horiz, &pseudo_mag);
519  ins_float_invariant_update_mag(&mag_final);
520  } else {
521  // if speed is tool low, better set measurements to zero
523  }
524 #endif
525 
526 }
527 
528 
530 {
531  static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE;
532  static float alpha = 10.0f;
533  static int32_t i = 1;
534  static float baro_moy = 0.0f;
535  static float baro_prev = 0.0f;
536 
537  if (!ins_baro_initialized) {
538  // try to find a stable qfe
539  // TODO generic function in pprz_isa ?
540  if (i == 1) {
541  baro_moy = pressure;
542  baro_prev = pressure;
543  }
544  baro_moy = (baro_moy * (i - 1) + pressure) / i;
545  alpha = (10.f*alpha + (baro_moy - baro_prev)) / (11.0f);
546  baro_prev = baro_moy;
547  // test stop condition
548  if (fabs(alpha) < 0.1f) {
549  ins_qfe = baro_moy;
550  ins_baro_initialized = true;
551  }
552  if (i == 250) {
553  ins_qfe = pressure;
554  ins_baro_initialized = true;
555  }
556  i++;
557  } else { /* normal update with baro measurement */
558  ins_float_inv.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
559  }
560 }
561 
562 // assume mag is dead when values are not moving anymore
563 #define MAG_FROZEN_COUNT 30
564 
566 {
567  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
568  static int32_t last_mx = 0;
569 
570  if (last_mx == mag->x) {
571  mag_frozen_count--;
572  if (mag_frozen_count == 0) {
573  // if mag is dead, better set measurements to zero
575  mag_frozen_count = MAG_FROZEN_COUNT;
576  }
577  } else {
578  // values are moving
579  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
580  // new values in body frame
581  float_rmat_transp_vmult(&ins_float_inv.meas.mag, body_to_imu_rmat, mag);
582  // reset counter
583  mag_frozen_count = MAG_FROZEN_COUNT;
584  }
585  last_mx = mag->x;
586 }
587 
588 
593 static inline void invariant_model(float *o, const float *x, const int n, const float *u,
594  const int m __attribute__((unused)))
595 {
596 
597 #pragma GCC diagnostic push // require GCC 4.6
598 #pragma GCC diagnostic ignored "-Wcast-qual"
599  struct inv_state *s = (struct inv_state *)x;
600  struct inv_command *c = (struct inv_command *)u;
601 #pragma GCC diagnostic pop // require GCC 4.6
602  struct inv_state s_dot;
603  struct FloatRates rates_unbiased;
604  struct FloatVect3 tmp_vect;
605  struct FloatQuat tmp_quat;
606 
607  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
608  RATES_DIFF(rates_unbiased, c->rates, s->bias);
609  /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
610  float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
611 
612  float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_float_inv.corr.LE);
613  QUAT_ADD(s_dot.quat, tmp_quat);
614 
615  float norm2_r = 1.f - FLOAT_QUAT_NORM2(s->quat);
616  QUAT_SMUL(tmp_quat, s->quat, norm2_r);
617  QUAT_ADD(s_dot.quat, tmp_quat);
618 
619  /* dot_V = A + (1/as) * (q * am * q-1) + ME */
620  struct FloatQuat q_b2n;
621  float_quat_invert(&q_b2n, &(s->quat));
622  float_quat_vmult((struct FloatVect3 *)&s_dot.speed, &q_b2n, &(c->accel));
623  VECT3_SMUL(s_dot.speed, s_dot.speed, 1.f / (s->as));
624  VECT3_ADD(s_dot.speed, A);
626 
627  /* dot_X = V + NE */
628  VECT3_SUM(s_dot.pos, s->speed, ins_float_inv.corr.NE);
629 
630  /* bias_dot = q-1 * (OE) * q */
631  float_quat_vmult(&tmp_vect, &(s->quat), &ins_float_inv.corr.OE);
632  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
633 
634  /* as_dot = as * RE */
635  s_dot.as = (s->as) * (ins_float_inv.corr.RE);
636  // keep as in a reasonable range, so 50% around the nominal value
637  if ( ((s->as < 0.5f) && (s_dot.as < 0.f)) || ((s->as > 1.5f) && (s_dot.as > 0.f)) ) {
638  s_dot.as = 0.f;
639  }
640 
641  /* hb_dot = SE */
642  s_dot.hb = ins_float_inv.corr.SE;
643 
644  // set output
645  memcpy(o, &s_dot, n * sizeof(float));
646 }
647 
652 static inline void error_output(struct InsFloatInv *_ins)
653 {
654 
655  struct FloatVect3 YBt, I, Ev, Eb, Ex, Itemp, Ebtemp, Evtemp;
656  float Eh;
657  float temp;
658 
659  /* YBt = q * yB * q-1 */
660  struct FloatQuat q_b2n;
661  float_quat_invert(&q_b2n, &(_ins->state.quat));
662  float_quat_vmult(&YBt, &q_b2n, &(_ins->meas.mag));
663 
664  float_quat_vmult(&I, &q_b2n, &(_ins->cmd.accel));
665  VECT3_SMUL(I, I, 1.f / (_ins->state.as));
666 
667  /*--------- E = ( ŷ - y ) ----------*/
668  /* Eb = ( B - YBt ) */
669  VECT3_DIFF(Eb, B, YBt);
670 
671  // pos and speed error only if GPS data are valid
672  // or while waiting first GPS data to prevent diverging
673  if ((gps.fix >= GPS_FIX_3D && _ins->is_aligned
674 #if FIXEDWING_FIRMWARE
676 #else
678 #endif
679  ) || !ins_gps_fix_once) {
680  /* Ev = (V - YV) */
681  VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
682  /* Ex = (X - YX) */
683  VECT3_DIFF(Ex, _ins->state.pos, _ins->meas.pos_gps);
684  } else {
685  FLOAT_VECT3_ZERO(Ev);
687  }
688  /* Eh = < X,e3 > - hb - YH */
689  Eh = _ins->state.pos.z - _ins->state.hb - _ins->meas.baro_alt;
690 
691  /*--------------Gains--------------*/
692 
693  /**** LvEv + LbEb = -lvIa x Ev + lb < B x Eb, Ia > Ia *****/
694  VECT3_SMUL(Itemp, I, -_ins->gains.lv / 100.f);
695  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
696 
697  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
698  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
699  temp = (_ins->gains.lb / 100.f) * temp;
700 
701  VECT3_SMUL(Ebtemp, I, temp);
702  VECT3_ADD(Evtemp, Ebtemp);
703  VECT3_COPY(_ins->corr.LE, Evtemp);
704 
705  /***** MvEv + MhEh = -mv * Ev + (-mh * <Eh,e3>)********/
706  _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.f;
707  _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.f;
708  _ins->corr.ME.z = ((-_ins->gains.mvz) * Ev.z) + ((-_ins->gains.mh) * Eh);
709 
710  /****** NxEx + NhEh = -nx * Ex + (-nh * <Eh, e3>) ********/
711  _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.f;
712  _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.f;
713  _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh);
714 
715  /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/
716  VECT3_SMUL(Itemp, I, _ins->gains.ov / 1000.f);
717  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
718 
719  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
720  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
721  temp = (-_ins->gains.ob / 1000.f) * temp;
722 
723  VECT3_SMUL(Ebtemp, I, temp);
724  VECT3_ADD(Evtemp, Ebtemp);
725  VECT3_COPY(_ins->corr.OE, Evtemp);
726 
727  /* a scalar */
728  /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/
729  _ins->corr.RE = ((_ins->gains.rv / 100.f) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh / 10000.f) * Eh);
730 
731  /****** ShEh ******/
732  _ins->corr.SE = (_ins->gains.sh) * Eh;
733 
734 }
735 
736 
737 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
738  struct FloatVect3 *vi)
739 {
740  struct FloatVect3 qvec, v1, v2;
741  float qi;
742 
743  FLOAT_QUAT_EXTRACT(qvec, *q);
744  qi = - VECT3_DOT_PRODUCT(*vi, qvec);
745  VECT3_CROSS_PRODUCT(v1, *vi, qvec);
746  VECT3_SMUL(v2, *vi, q->qi);
747  VECT3_ADD(v2, v1);
748  QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
749 }
750 
752 {
754 
755  if (!ins_float_inv.is_aligned) {
756  /* Set ltp_to_imu so that body is zero */
757  ins_float_inv.state.quat = *q_b2i;
758  }
759 }
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
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:737
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
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
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:291
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:593
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
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:43
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
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:44
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:299
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:652
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
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
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:51
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:565
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:445
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
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
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
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
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:411
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
course
static int16_t course[3]
Definition: airspeed_uADC.c:58
nav.h
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
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:321
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
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:45
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:563
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
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
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:529
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
GpsState::cacc
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:104
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:343
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:688
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
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
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:751
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