Paparazzi UAS  v5.12_stable-4-g9b43e9b
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 /* error computation */
153 static inline void error_output(struct InsFloatInv *_ins);
154 
155 /* propagation model (called by runge-kutta library) */
156 static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m);
157 
158 
162 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
163  struct FloatVect3 *vi);
164 
165 
166 /* init state and measurements */
167 static inline void init_invariant_state(void)
168 {
169  // init state
174  ins_float_inv.state.as = 1.0f;
175  ins_float_inv.state.hb = 0.0f;
176 
177  // init measures
180  ins_float_inv.meas.baro_alt = 0.0f;
181 
182  // init baro
183  ins_baro_initialized = false;
184  ins_gps_fix_once = false;
185 }
186 
187 #if SEND_INVARIANT_FILTER || PERIODIC_TELEMETRY
188 static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
189 {
190  struct FloatEulers eulers;
192  pprz_msg_send_INV_FILTER(trans, dev,
193  AC_ID,
195  &eulers.phi,
196  &eulers.theta,
197  &eulers.psi,
211 }
212 #endif
213 
215 {
216 
217  // init position
218 #if INS_FINV_USE_UTM
219  struct UtmCoor_f utm0;
220  utm0.north = (float)nav_utm_north0;
221  utm0.east = (float)nav_utm_east0;
222  utm0.alt = GROUND_ALT;
223  utm0.zone = nav_utm_zone0;
225  stateSetPositionUtm_f(&utm0);
226 #else
227  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
228  llh_nav0.lat = NAV_LAT0;
229  llh_nav0.lon = NAV_LON0;
230  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
231  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
232  struct EcefCoor_i ecef_nav0;
233  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
234  struct LtpDef_i ltp_def;
235  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
236  ltp_def.hmsl = NAV_ALT0;
237  stateSetLocalOrigin_i(&ltp_def);
238 #endif
239 
240  B.x = INS_H_X;
241  B.y = INS_H_Y;
242  B.z = INS_H_Z;
243 
244  // init state and measurements
246 
247  // init gains
261 
262  ins_float_inv.is_aligned = false;
263  ins_float_inv.reset = false;
264 
265 #if PERIODIC_TELEMETRY
267 #endif
268 }
269 
270 
272 {
273 #if INS_FINV_USE_UTM
274  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
275  // reset state UTM ref
277 #else
278  struct LtpDef_i ltp_def;
279  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
280  ltp_def.hmsl = gps.hmsl;
281  stateSetLocalOrigin_i(&ltp_def);
282 #endif
283 }
284 
286 {
287 #if INS_FINV_USE_UTM
288  struct UtmCoor_f utm = state.utm_origin_f;
289  utm.alt = gps.hmsl / 1000.0f;
291 #else
292  struct LlaCoor_i lla = {
294  .lon = state.ned_origin_i.lla.lon,
295  .alt = gps.lla_pos.alt
296  };
297  struct LtpDef_i ltp_def;
298  ltp_def_from_lla_i(&ltp_def, &lla);
299  ltp_def.hmsl = gps.hmsl;
300  stateSetLocalOrigin_i(&ltp_def);
301 #endif
302 }
303 
305  struct FloatVect3 *lp_accel,
306  struct FloatVect3 *lp_mag)
307 {
308  /* Compute an initial orientation from accel and mag directly as quaternion */
310 
311  /* use average gyro as initial value for bias */
312  ins_float_inv.state.bias = *lp_gyro;
313 
314  /* push initial values to state interface */
316 
317  // ins and ahrs are now running
318  ins_float_inv.is_aligned = true;
319 }
320 
321 void ins_float_invariant_propagate(struct FloatRates* gyro, struct FloatVect3* accel, float dt)
322 {
323  struct FloatRates body_rates;
324 
325  // realign all the filter if needed
326  // a complete init cycle is required
327  if (ins_float_inv.reset) {
328  ins_float_inv.reset = false;
329  ins_float_inv.is_aligned = false;
331  }
332 
333  // fill command vector in body frame
334  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
335  float_rmat_transp_ratemult(&ins_float_inv.cmd.rates, body_to_imu_rmat, gyro);
336  float_rmat_transp_vmult(&ins_float_inv.cmd.accel, body_to_imu_rmat, accel);
337 
338  struct Int32Vect3 body_accel_i;
339  ACCELS_BFP_OF_REAL(body_accel_i, ins_float_inv.cmd.accel);
340  stateSetAccelBody_i(&body_accel_i);
341 
342  // update correction gains
344 
345  // propagate model
346  struct inv_state new_state;
347  runge_kutta_4_float((float *)&new_state,
348  (float *)&ins_float_inv.state, INV_STATE_DIM,
349  (float *)&ins_float_inv.cmd, INV_COMMAND_DIM,
350  invariant_model, dt);
351  ins_float_inv.state = new_state;
352 
353  // normalize quaternion
355 
356  // set global state
359  stateSetBodyRates_f(&body_rates);
362  // untilt accel and remove gravity
363  struct FloatQuat q_b2n;
365  struct FloatVect3 accel_n;
366  float_quat_vmult(&accel_n, &q_b2n, &ins_float_inv.cmd.accel);
367  VECT3_SMUL(accel_n, accel_n, 1. / (ins_float_inv.state.as));
368  VECT3_ADD(accel_n, A);
369  stateSetAccelNed_f((struct NedCoor_f *)&accel_n);
370 
371  //------------------------------------------------------------//
372 
373 #if SEND_INVARIANT_FILTER
374  RunOnceEvery(3, send_inv_filter(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
375 #endif
376 
377 #if LOG_INVARIANT_FILTER
378  if (pprzLogFile != -1) {
379  if (!log_started) {
380  // log file header
381  sdLogWriteLog(pprzLogFile,
382  "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");
383  log_started = true;
384  } else {
385  sdLogWriteLog(pprzLogFile,
386  "%.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",
418  }
419  }
420 #endif
421 }
422 
424 {
425 
426  if (gps_s->fix >= GPS_FIX_3D && ins_float_inv.is_aligned) {
427  ins_gps_fix_once = true;
428 
429 #if INS_FINV_USE_UTM
430  if (state.utm_initialized_f) {
431  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
432  // position (local ned)
436  // speed
437  ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f;
438  ins_float_inv.meas.speed_gps.y = gps_s->ned_vel.y / 100.0f;
439  ins_float_inv.meas.speed_gps.z = gps_s->ned_vel.z / 100.0f;
440  }
441 #else
442  if (state.ned_initialized_f) {
443  struct NedCoor_i gps_pos_cm_ned, ned_pos;
444  ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &gps_s->ecef_pos);
447  struct EcefCoor_f ecef_vel;
448  ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel);
450  }
451 #endif
452  }
453 
454 }
455 
456 
458 {
459  static float ins_qfe = 101325.0f;
460  static float alpha = 10.0f;
461  static int32_t i = 1;
462  static float baro_moy = 0.0f;
463  static float baro_prev = 0.0f;
464 
465  if (!ins_baro_initialized) {
466  // try to find a stable qfe
467  // TODO generic function in pprz_isa ?
468  if (i == 1) {
469  baro_moy = pressure;
470  baro_prev = pressure;
471  }
472  baro_moy = (baro_moy * (i - 1) + pressure) / i;
473  alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f);
474  baro_prev = baro_moy;
475  // test stop condition
476  if (fabs(alpha) < 0.005f) {
477  ins_qfe = baro_moy;
478  ins_baro_initialized = true;
479  }
480  if (i == 250) {
481  ins_qfe = pressure;
482  ins_baro_initialized = true;
483  }
484  i++;
485  } else { /* normal update with baro measurement */
486  ins_float_inv.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
487  }
488 }
489 
490 // assume mag is dead when values are not moving anymore
491 #define MAG_FROZEN_COUNT 30
492 
494 {
495  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
496  static int32_t last_mx = 0;
497 
498  if (last_mx == mag->x) {
499  mag_frozen_count--;
500  if (mag_frozen_count == 0) {
501  // if mag is dead, better set measurements to zero
503  mag_frozen_count = MAG_FROZEN_COUNT;
504  }
505  } else {
506  // values are moving
507  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
508  // new values in body frame
509  float_rmat_transp_vmult(&ins_float_inv.meas.mag, body_to_imu_rmat, mag);
510  // reset counter
511  mag_frozen_count = MAG_FROZEN_COUNT;
512  }
513  last_mx = mag->x;
514 }
515 
516 
521 static inline void invariant_model(float *o, const float *x, const int n, const float *u,
522  const int m __attribute__((unused)))
523 {
524 
525 #pragma GCC diagnostic push // require GCC 4.6
526 #pragma GCC diagnostic ignored "-Wcast-qual"
527  struct inv_state *s = (struct inv_state *)x;
528  struct inv_command *c = (struct inv_command *)u;
529 #pragma GCC diagnostic pop // require GCC 4.6
530  struct inv_state s_dot;
531  struct FloatRates rates_unbiased;
532  struct FloatVect3 tmp_vect;
533  struct FloatQuat tmp_quat;
534 
535  // test accel sensitivity
536  if (fabs(s->as) < 0.1) {
537  // too small, return x_dot = 0 to avoid division by 0
538  float_vect_zero(o, n);
539  // TODO set ins state to error
540  return;
541  }
542 
543  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
544  RATES_DIFF(rates_unbiased, c->rates, s->bias);
545  /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
546  float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
547 
548  float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_float_inv.corr.LE);
549  QUAT_ADD(s_dot.quat, tmp_quat);
550 
551  float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat);
552  QUAT_SMUL(tmp_quat, s->quat, norm2_r);
553  QUAT_ADD(s_dot.quat, tmp_quat);
554 
555  /* dot_V = A + (1/as) * (q * am * q-1) + ME */
556  struct FloatQuat q_b2n;
557  float_quat_invert(&q_b2n, &(s->quat));
558  float_quat_vmult((struct FloatVect3 *)&s_dot.speed, &q_b2n, &(c->accel));
559  VECT3_SMUL(s_dot.speed, s_dot.speed, 1. / (s->as));
560  VECT3_ADD(s_dot.speed, A);
562 
563  /* dot_X = V + NE */
564  VECT3_SUM(s_dot.pos, s->speed, ins_float_inv.corr.NE);
565 
566  /* bias_dot = q-1 * (OE) * q */
567  float_quat_vmult(&tmp_vect, &(s->quat), &ins_float_inv.corr.OE);
568  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
569 
570  /* as_dot = as * RE */
571  s_dot.as = (s->as) * (ins_float_inv.corr.RE);
572 
573  /* hb_dot = SE */
574  s_dot.hb = ins_float_inv.corr.SE;
575 
576  // set output
577  memcpy(o, &s_dot, n * sizeof(float));
578 }
579 
584 static inline void error_output(struct InsFloatInv *_ins)
585 {
586 
587  struct FloatVect3 YBt, I, Ev, Eb, Ex, Itemp, Ebtemp, Evtemp;
588  float Eh;
589  float temp;
590 
591  // test accel sensitivity
592  if (fabs(_ins->state.as) < 0.1) {
593  // too small, don't do anything to avoid division by 0
594  return;
595  }
596 
597  /* YBt = q * yB * q-1 */
598  struct FloatQuat q_b2n;
599  float_quat_invert(&q_b2n, &(_ins->state.quat));
600  float_quat_vmult(&YBt, &q_b2n, &(_ins->meas.mag));
601 
602  float_quat_vmult(&I, &q_b2n, &(_ins->cmd.accel));
603  VECT3_SMUL(I, I, 1. / (_ins->state.as));
604 
605  /*--------- E = ( ลท - y ) ----------*/
606  /* Eb = ( B - YBt ) */
607  VECT3_DIFF(Eb, B, YBt);
608 
609  // pos and speed error only if GPS data are valid
610  // or while waiting first GPS data to prevent diverging
612 #if INS_FINV_USE_UTM
614 #else
616 #endif
617  ) || !ins_gps_fix_once) {
618  /* Ev = (V - YV) */
619  VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
620  /* Ex = (X - YX) */
621  VECT3_DIFF(Ex, _ins->state.pos, _ins->meas.pos_gps);
622  } else {
623  FLOAT_VECT3_ZERO(Ev);
624  FLOAT_VECT3_ZERO(Ex);
625  }
626  /* Eh = < X,e3 > - hb - YH */
627  Eh = _ins->state.pos.z - _ins->state.hb - _ins->meas.baro_alt;
628 
629  /*--------------Gains--------------*/
630 
631  /**** LvEv + LbEb = -lvIa x Ev + lb < B x Eb, Ia > Ia *****/
632  VECT3_SMUL(Itemp, I, -_ins->gains.lv / 100.);
633  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
634 
635  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
636  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
637  temp = (_ins->gains.lb / 100.) * temp;
638 
639  VECT3_SMUL(Ebtemp, I, temp);
640  VECT3_ADD(Evtemp, Ebtemp);
641  VECT3_COPY(_ins->corr.LE, Evtemp);
642 
643  /***** MvEv + MhEh = -mv * Ev + (-mh * <Eh,e3>)********/
644  _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.;
645  _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.;
646  _ins->corr.ME.z = ((-_ins->gains.mvz) * Ev.z) + ((-_ins->gains.mh) * Eh);
647 
648  /****** NxEx + NhEh = -nx * Ex + (-nh * <Eh, e3>) ********/
649  _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.;
650  _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.;
651  _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh);
652 
653  /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/
654  VECT3_SMUL(Itemp, I, _ins->gains.ov / 1000.);
655  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
656 
657  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
658  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
659  temp = (-_ins->gains.ob / 1000.) * temp;
660 
661  VECT3_SMUL(Ebtemp, I, temp);
662  VECT3_ADD(Evtemp, Ebtemp);
663  VECT3_COPY(_ins->corr.OE, Evtemp);
664 
665  /* a scalar */
666  /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/
667  _ins->corr.RE = ((_ins->gains.rv / 100.) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh / 10000.) * Eh);
668 
669  /****** ShEh ******/
670  _ins->corr.SE = (_ins->gains.sh) * Eh;
671 
672 }
673 
674 
675 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
676  struct FloatVect3 *vi)
677 {
678  struct FloatVect3 qvec, v1, v2;
679  float qi;
680 
681  FLOAT_QUAT_EXTRACT(qvec, *q);
682  qi = - VECT3_DOT_PRODUCT(*vi, qvec);
683  VECT3_CROSS_PRODUCT(v1, *vi, qvec);
684  VECT3_SMUL(v2, *vi, q->qi);
685  VECT3_ADD(v2, v1);
686  QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
687 }
688 
690 {
692 
693  if (!ins_float_inv.is_aligned) {
694  /* Set ltp_to_imu so that body is zero */
695  ins_float_inv.state.quat = *q_b2i;
696  }
697 }
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:243
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:249
#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:146
struct NedCoor_f speed_gps
Measured gps speed.
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:558
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:139
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:181
float alpha
Definition: textons.c:107
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:329
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:160
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:371
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
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:745
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:88
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.
data structure for GPS information
Definition: gps.h:81
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.
static void float_vect_zero(float *a, const int n)
a = 0
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:85
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:527
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
bool ins_gps_fix_once
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:188
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:393
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.
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:89
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:86
static float pprz_isa_height_of_pressure(float pressure, float ref_p)
Get relative altitude from pressure (using simplified equation).
Definition: pprz_isa.h:95
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:566
#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:99
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
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:90
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:75
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