Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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;
242  stateSetLocalOrigin_i(&ltp_def);
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;
290  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
291  ltp_def.hmsl = gps.hmsl;
292  stateSetLocalOrigin_i(&ltp_def);
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;
309  ltp_def_from_lla_i(&ltp_def, &lla);
310  ltp_def.hmsl = gps.hmsl;
311  stateSetLocalOrigin_i(&ltp_def);
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);
648  FLOAT_VECT3_ZERO(Ex);
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 }
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:244
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:250
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
float nxz
Tuning parameter of vertical position error on position.
#define INV_STATE_DIM
Invariant filter state dimension.
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
struct VehicleInterface vi
Definition: vi.c:30
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
#define INV_COMMAND_DIM
Invariant filter command vector dimension.
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
struct NedCoor_f speed_gps
Measured gps speed.
static uint32_t s
struct FloatVect3 ME
Correction gains on gyro biases.
float mv
Tuning parameter of horizontal speed error on speed.
float y
in meters
float east
in meters
float phi
in radians
float OE
Correction gains on magnetometer sensitivity.
definition of the local (flat earth) coordinate system
static const struct FloatVect3 A
#define B
#define INS_INV_RV
float north
in meters
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
float ov
Tuning parameter of speed error on gyro biases.
#define INS_INV_OB
Periodic telemetry system header (includes downlink utility and generated code).
void ins_float_invariant_update_mag(struct FloatVect3 *mag)
#define NED_FLOAT_OF_BFP(_o, _i)
float rh
Tuning parameter of baro error on accel biases (vertical projection)
vector in EarthCenteredEarthFixed coordinates
vector in EarthCenteredEarthFixed coordinates
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:611
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
bool reset
flag to request reset/reinit the filter
struct FloatVect3 accel
Input accelerometers.
float RE
Correction gains on accel bias.
struct inv_command cmd
command vector
struct FloatQuat quat
Estimated attitude (quaternion)
#define INS_INV_SH
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
float r
in rad/s
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
float alpha
Definition: textons.c:107
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
#define FLOAT_RATES_ZERO(_r)
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
#define INS_INV_LB
uint8_t nav_utm_zone0
Definition: common_nav.c:44
float psi
in radians
Integrated Navigation System interface.
#define VECT3_SUM(_c, _a, _b)
Definition: pprz_algebra.h:161
position in UTM coordinates Units: meters
float mh
Tuning parameter of baro error on vertical speed.
Invariant filter command vector.
vector in Latitude, Longitude and Altitude
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
float q
in rad/s
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
float p
in rad/s
int32_t z
Down.
void ins_float_invariant_update_gps(struct GpsState *gps_s)
float baro_alt
Measured barometric altitude.
void ins_reset_altitude_ref(void)
INS altitude reference reset.
#define INS_INV_NXZ
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.
int32_t hmsl
Height above mean sea level in mm.
struct FloatVect3 LE
Correction gains on attitude.
int32_t alt
in millimeters above WGS84 reference ellipsoid
bool log_started
static void error_output(struct InsFloatInv *_ins)
Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error ) ...
euler angles
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
float z
in meters
Roation quaternion.
int32_t y
East.
struct OrientationReps body_to_imu
body_to_imu rotation
float ob
Tuning parameter of mag error on gyro biases.
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
float theta
in radians
#define FLOAT_VECT3_ZERO(_v)
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
float mvz
Tuning parameter of vertical speed error on speed.
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
#define INS_INV_LV
int32_t nav_utm_north0
Definition: common_nav.c:43
vector in North East Down coordinates Units: meters
Paparazzi floating point algebra.
static float float_vect2_norm(struct FloatVect2 *v)
data structure for GPS information
Definition: gps.h:87
float NE
Correction gains on accel bias.
static void invariant_model(float *o, const float *x, const int n, const float *u, const int m)
Compute dynamic mode.
#define INT32_POS_OF_CM_NUM
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
Device independent GPS code (interface)
#define INS_INV_MH
Paparazzi atmospheric pressure conversion utilities.
#define FLOAT_QUAT_NORM2(_q)
static void init_invariant_state(void)
struct inv_state state
state vector
int32_t x
North.
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:580
struct FloatRates rates
Input gyro rates.
void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, struct FloatVect3 *vi)
Right multiplication by a quaternion.
#define INS_INV_NX
float lb
Tuning parameter of mag error on attitude.
struct LlaCoor_i lla
Reference point in lla.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int32_t lon
in degrees*1e7
FileDes pprzLogFile
Definition: sdlog_chibios.c:85
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
static void float_quat_normalize(struct FloatQuat *q)
#define INS_INV_MVZ
static void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
struct inv_gains gains
tuning gains
float lv
Tuning parameter of speed error on attitude.
uint8_t zone
UTM zone number.
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
struct FloatVect3 mag
Measured magnetic field.
#define INS_INV_NH
signed long int32_t
Definition: types.h:19
struct FloatRates bias
Estimated gyro biases.
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
float m[3 *3]
Utility functions for fixed point AHRS implementations.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
void ins_float_invariant_update_baro(float pressure)
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
#define MAG_FROZEN_COUNT
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition: pprz_isa.h:50
bool ins_gps_fix_once
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
#define FLOAT_QUAT_EXTRACT(_vo, _qi)
#define INT32_POS_OF_CM_DEN
struct inv_measures meas
measurement vector
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:389
API to get/set the generic vehicle states.
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
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.
vector in North East Down coordinates
float sh
Tuning parameter of baro error on baro bias.
float nh
Tuning parameter of baro error on vertical position.
int32_t nav_utm_east0
Definition: common_nav.c:42
struct NedCoor_f speed
Estimates speed.
#define INS_INV_OV
float SE
Correction gains on barometer bias.
#define INS_INV_HEADING_UPDATE_GPS_MIN_SPEED
rotation matrix
#define INS_INV_MV
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
float rv
Tuning parameter of speed error on accel biases.
INS using invariant filter.
void ins_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
float hb
Estimates barometers bias.
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
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
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:619
#define ECEF_FLOAT_OF_BFP(_o, _i)
Invariant filter structure.
void ins_reset_local_origin(void)
INS local origin reset.
Runge-Kutta library (float version)
int32_t lat
in degrees*1e7
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
struct NedCoor_f pos
Estimates position.
#define INS_INV_RH
struct InsFloatInv ins_float_inv
uint8_t fix
status of fix
Definition: gps.h:107
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
void ins_float_invariant_propagate(struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
Invariant filter state.
struct GpsState gps
global GPS state
Definition: gps.c:69
void ins_float_invariant_init(void)
angular rates
float as
Estimated accelerometer sensitivity.
struct NedCoor_f pos_gps
Measured gps position.
float nx
Tuning parameter of horizontal position error on position.
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
struct State state
Definition: state.c:36
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
Paparazzi fixed point algebra.
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.
float x
in meters
struct inv_correction_gains corr
correction gains
bool ins_baro_initialized