Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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 
33 
34 #include "subsystems/ins.h"
35 #include "subsystems/gps.h"
36 
37 #include "generated/airframe.h"
38 #include "generated/flight_plan.h"
39 #if INS_FINV_USE_UTM
41 #endif
42 
44 #include "math/pprz_algebra_int.h"
45 #include "math/pprz_rk_float.h"
46 #include "math/pprz_isa.h"
47 
48 #include "state.h"
49 
50 // for debugging
51 #if SEND_INVARIANT_FILTER || PERIODIC_TELEMETRY
53 #endif
54 
55 #if LOG_INVARIANT_FILTER
56 #include "sdLog.h"
57 #include "subsystems/chibios-libopencm3/chibios_sdlog.h"
58 bool_t log_started = FALSE;
59 #endif
60 
61 /*------------- =*= Invariant Observers =*= -------------*
62  *
63  * State vector :
64  *
65  * x = [q0 q1 q2 q3 vx vy vz px py pz wb1 wb2 wb3 hb as]'
66  *
67  * Dynamic model (dim = 15) :
68  *
69  * x_qdot = 0.5 * x_quat * ( x_rates - x_bias );
70  * x_Vdot = A + 1/as (q * am * (q)-1);
71  * x_Xdot = V;
72  * x_bias_dot = 0;
73  * x_asdot = 0;
74  * x_hbdot = 0;
75  *
76  * Observation model (dim = 10):
77  * yv = V;
78  * yx = X;
79  * yh = <X,e3> - hb;
80  * yb = (q)-1 *B * q; (B : magnetometers)
81  *
82  *------------------------------------------------------*/
83 
84 // Default values for the tuning gains
85 // Tuning parameter of speed error on attitude (e-2)
86 #ifndef INS_INV_LV
87 #define INS_INV_LV 2.
88 #endif
89 // Tuning parameter of mag error on attitude (e-2)
90 #ifndef INS_INV_LB
91 #define INS_INV_LB 6.
92 #endif
93 // Tuning parameter of horizontal speed error on speed
94 #ifndef INS_INV_MV
95 #define INS_INV_MV 8.
96 #endif
97 // Tuning parameter of vertical speed error on speed
98 #ifndef INS_INV_MVZ
99 #define INS_INV_MVZ 15.
100 #endif
101 // Tuning parameter of baro error on vertical speed
102 #ifndef INS_INV_MH
103 #define INS_INV_MH 0.2
104 #endif
105 // Tuning parameter of horizontal position error on position
106 #ifndef INS_INV_NX
107 #define INS_INV_NX 0.8
108 #endif
109 // Tuning parameter of vertical position error on position
110 #ifndef INS_INV_NXZ
111 #define INS_INV_NXZ 0.5
112 #endif
113 // Tuning parameter of baro error on vertical position
114 #ifndef INS_INV_NH
115 #define INS_INV_NH 1.2
116 #endif
117 // Tuning parameter of speed error on gyro biases (e-3)
118 #ifndef INS_INV_OV
119 #define INS_INV_OV 1.2
120 #endif
121 // Tuning parameter of mag error on gyro biases (e-3)
122 #ifndef INS_INV_OB
123 #define INS_INV_OB 1.
124 #endif
125 // Tuning parameter of speed error on accel biases (e-2)
126 #ifndef INS_INV_RV
127 #define INS_INV_RV 4.
128 #endif
129 // Tuning parameter of baro error on accel biases (vertical projection) (e-8)
130 #ifndef INS_INV_RH
131 #define INS_INV_RH 8.
132 #endif
133 // Tuning parameter of baro error on baro bias
134 #ifndef INS_INV_SH
135 #define INS_INV_SH 0.01
136 #endif
137 
138 
140 
141 /* earth gravity model */
142 static const struct FloatVect3 A = { 0.f, 0.f, 9.81f };
143 
144 /* earth magnetic model */
145 //static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) };
146 #define B ins_float_inv.mag_h
147 
148 /* barometer */
150 
151 /* gps */
153 
154 /* error computation */
155 static inline void error_output(struct InsFloatInv *_ins);
156 
157 /* propagation model (called by runge-kutta library) */
158 static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m);
159 
160 
164 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
165  struct FloatVect3 *vi);
166 
167 
168 /* init state and measurements */
169 static inline void init_invariant_state(void)
170 {
171  // init state
176  ins_float_inv.state.as = 1.0f;
177  ins_float_inv.state.hb = 0.0f;
178 
179  // init measures
182  ins_float_inv.meas.baro_alt = 0.0f;
183 
184  // init baro
187 }
188 
189 #if SEND_INVARIANT_FILTER || PERIODIC_TELEMETRY
190 static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
191 {
192  struct FloatEulers eulers;
194  pprz_msg_send_INV_FILTER(trans, dev,
195  AC_ID,
197  &eulers.phi,
198  &eulers.theta,
199  &eulers.psi,
213 }
214 #endif
215 
217 {
218 
219  // init position
220 #if INS_FINV_USE_UTM
221  struct UtmCoor_f utm0;
222  utm0.north = (float)nav_utm_north0;
223  utm0.east = (float)nav_utm_east0;
224  utm0.alt = GROUND_ALT;
225  utm0.zone = nav_utm_zone0;
227  stateSetPositionUtm_f(&utm0);
228 #else
229  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
230  llh_nav0.lat = NAV_LAT0;
231  llh_nav0.lon = NAV_LON0;
232  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
233  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
234  struct EcefCoor_i ecef_nav0;
235  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
236  struct LtpDef_i ltp_def;
237  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
238  ltp_def.hmsl = NAV_ALT0;
239  stateSetLocalOrigin_i(&ltp_def);
240 #endif
241 
242  B.x = INS_H_X;
243  B.y = INS_H_Y;
244  B.z = INS_H_Z;
245 
246  // init state and measurements
248 
249  // init gains
263 
266 
267 #if PERIODIC_TELEMETRY
268  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INV_FILTER, send_inv_filter);
269 #endif
270 }
271 
272 
274 {
275 #if INS_FINV_USE_UTM
276  struct UtmCoor_f utm;
277 #ifdef GPS_USE_LATLONG
278  /* Recompute UTM coordinates in this zone */
279  struct LlaCoor_f lla;
281  utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
282  utm_of_lla_f(&utm, &lla);
283 #else
284  utm.zone = gps.utm_pos.zone;
285  utm.east = gps.utm_pos.east / 100.0f;
286  utm.north = gps.utm_pos.north / 100.0f;
287 #endif
288  // ground_alt
289  utm.alt = gps.hmsl / 1000.0f;
290  // reset state UTM ref
292 #else
293  struct LtpDef_i ltp_def;
294  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
295  ltp_def.hmsl = gps.hmsl;
296  stateSetLocalOrigin_i(&ltp_def);
297 #endif
298 }
299 
301 {
302 #if INS_FINV_USE_UTM
303  struct UtmCoor_f utm = state.utm_origin_f;
304  utm.alt = gps.hmsl / 1000.0f;
306 #else
307  struct LlaCoor_i lla = {
309  .lon = state.ned_origin_i.lla.lon,
310  .alt = gps.lla_pos.alt
311  };
312  struct LtpDef_i ltp_def;
313  ltp_def_from_lla_i(&ltp_def, &lla);
314  ltp_def.hmsl = gps.hmsl;
315  stateSetLocalOrigin_i(&ltp_def);
316 #endif
317 }
318 
320  struct Int32Vect3 *lp_accel,
321  struct Int32Vect3 *lp_mag)
322 {
323  /* Compute an initial orientation from accel and mag directly as quaternion */
325 
326  /* use average gyro as initial value for bias */
327  struct FloatRates bias0;
328  RATES_COPY(bias0, *lp_gyro);
330 
331  /* push initial values to state interface */
333 
334  // ins and ahrs are now running
336 }
337 
338 void ins_float_invariant_propagate(struct Int32Rates* gyro, struct Int32Vect3* accel, float dt)
339 {
340  struct FloatRates body_rates;
341 
342  // realign all the filter if needed
343  // a complete init cycle is required
344  if (ins_float_inv.reset) {
348  }
349 
350  // fill command vector
351  struct Int32Rates gyro_meas_body;
352  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_float_inv.body_to_imu);
353  int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro);
354  RATES_FLOAT_OF_BFP(ins_float_inv.cmd.rates, gyro_meas_body);
355  struct Int32Vect3 accel_meas_body;
356  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
357  ACCELS_FLOAT_OF_BFP(ins_float_inv.cmd.accel, accel_meas_body);
358 
359  // update correction gains
361 
362  // propagate model
363  struct inv_state new_state;
364  runge_kutta_4_float((float *)&new_state,
365  (float *)&ins_float_inv.state, INV_STATE_DIM,
366  (float *)&ins_float_inv.cmd, INV_COMMAND_DIM,
367  invariant_model, dt);
368  ins_float_inv.state = new_state;
369 
370  // normalize quaternion
372 
373  // set global state
376  stateSetBodyRates_f(&body_rates);
379  // untilt accel and remove gravity
380  struct FloatQuat q_b2n;
382  struct FloatVect3 accel_n;
383  float_quat_vmult(&accel_n, &q_b2n, &ins_float_inv.cmd.accel);
384  VECT3_SMUL(accel_n, accel_n, 1. / (ins_float_inv.state.as));
385  VECT3_ADD(accel_n, A);
386  stateSetAccelNed_f((struct NedCoor_f *)&accel_n);
387 
388  //------------------------------------------------------------//
389 
390 #if SEND_INVARIANT_FILTER
391  RunOnceEvery(3, send_inv_filter(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
392 #endif
393 
394 #if LOG_INVARIANT_FILTER
395  if (pprzLogFile != -1) {
396  if (!log_started) {
397  // log file header
398  sdLogWriteLog(pprzLogFile,
399  "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");
400  log_started = TRUE;
401  } else {
402  sdLogWriteLog(pprzLogFile,
403  "%.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",
435  }
436  }
437 #endif
438 }
439 
441 {
442 
443  if (gps_s->fix >= GPS_FIX_3D && ins_float_inv.is_aligned) {
445 
446 #if INS_FINV_USE_UTM
447  if (state.utm_initialized_f) {
448  // position (local ned)
450  ins_float_inv.meas.pos_gps.y = (gps_s->utm_pos.east / 100.0f) - state.utm_origin_f.east;
451  ins_float_inv.meas.pos_gps.z = state.utm_origin_f.alt - (gps_s->hmsl / 1000.0f);
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 EcefCoor_f ecef_pos, ecef_vel;
460  ECEF_FLOAT_OF_BFP(ecef_pos, gps_s->ecef_pos);
462  ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel);
464  }
465 #endif
466  }
467 
468 }
469 
470 
472 {
473  static float ins_qfe = 101325.0f;
474  static float alpha = 10.0f;
475  static int32_t i = 1;
476  static float baro_moy = 0.0f;
477  static float baro_prev = 0.0f;
478 
479  if (!ins_baro_initialized) {
480  // try to find a stable qfe
481  // TODO generic function in pprz_isa ?
482  if (i == 1) {
483  baro_moy = pressure;
484  baro_prev = pressure;
485  }
486  baro_moy = (baro_moy * (i - 1) + pressure) / i;
487  alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f);
488  baro_prev = baro_moy;
489  // test stop condition
490  if (fabs(alpha) < 0.005f) {
491  ins_qfe = baro_moy;
493  }
494  if (i == 250) {
495  ins_qfe = pressure;
497  }
498  i++;
499  } else { /* normal update with baro measurement */
500  ins_float_inv.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
501  }
502 }
503 
504 // assume mag is dead when values are not moving anymore
505 #define MAG_FROZEN_COUNT 30
506 
508 {
509  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
510  static int32_t last_mx = 0;
511 
512  if (last_mx == mag->x) {
513  mag_frozen_count--;
514  if (mag_frozen_count == 0) {
515  // if mag is dead, better set measurements to zero
517  mag_frozen_count = MAG_FROZEN_COUNT;
518  }
519  } else {
520  // values are moving
521  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ins_float_inv.body_to_imu);
522  struct Int32Vect3 mag_meas_body;
523  // new values in body frame
524  int32_rmat_transp_vmult(&mag_meas_body, body_to_imu_rmat, mag);
525  MAGS_FLOAT_OF_BFP(ins_float_inv.meas.mag, mag_meas_body);
526  // reset counter
527  mag_frozen_count = MAG_FROZEN_COUNT;
528  }
529  last_mx = mag->x;
530 }
531 
532 
537 static inline void invariant_model(float *o, const float *x, const int n, const float *u,
538  const int m __attribute__((unused)))
539 {
540 
541 #pragma GCC diagnostic push // require GCC 4.6
542 #pragma GCC diagnostic ignored "-Wcast-qual"
543  struct inv_state *s = (struct inv_state *)x;
544  struct inv_command *c = (struct inv_command *)u;
545 #pragma GCC diagnostic pop // require GCC 4.6
546  struct inv_state s_dot;
547  struct FloatRates rates_unbiased;
548  struct FloatVect3 tmp_vect;
549  struct FloatQuat tmp_quat;
550 
551  // test accel sensitivity
552  if (fabs(s->as) < 0.1) {
553  // too small, return x_dot = 0 to avoid division by 0
554  float_vect_zero(o, n);
555  // TODO set ins state to error
556  return;
557  }
558 
559  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
560  RATES_DIFF(rates_unbiased, c->rates, s->bias);
561  /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
562  float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
563 
564  float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_float_inv.corr.LE);
565  QUAT_ADD(s_dot.quat, tmp_quat);
566 
567  float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat);
568  QUAT_SMUL(tmp_quat, s->quat, norm2_r);
569  QUAT_ADD(s_dot.quat, tmp_quat);
570 
571  /* dot_V = A + (1/as) * (q * am * q-1) + ME */
572  struct FloatQuat q_b2n;
573  float_quat_invert(&q_b2n, &(s->quat));
574  float_quat_vmult((struct FloatVect3 *)&s_dot.speed, &q_b2n, &(c->accel));
575  VECT3_SMUL(s_dot.speed, s_dot.speed, 1. / (s->as));
576  VECT3_ADD(s_dot.speed, A);
578 
579  /* dot_X = V + NE */
580  VECT3_SUM(s_dot.pos, s->speed, ins_float_inv.corr.NE);
581 
582  /* bias_dot = q-1 * (OE) * q */
583  float_quat_vmult(&tmp_vect, &(s->quat), &ins_float_inv.corr.OE);
584  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
585 
586  /* as_dot = as * RE */
587  s_dot.as = (s->as) * (ins_float_inv.corr.RE);
588 
589  /* hb_dot = SE */
590  s_dot.hb = ins_float_inv.corr.SE;
591 
592  // set output
593  memcpy(o, &s_dot, n * sizeof(float));
594 }
595 
600 static inline void error_output(struct InsFloatInv *_ins)
601 {
602 
603  struct FloatVect3 YBt, I, Ev, Eb, Ex, Itemp, Ebtemp, Evtemp;
604  float Eh;
605  float temp;
606 
607  // test accel sensitivity
608  if (fabs(_ins->state.as) < 0.1) {
609  // too small, don't do anything to avoid division by 0
610  return;
611  }
612 
613  /* YBt = q * yB * q-1 */
614  struct FloatQuat q_b2n;
615  float_quat_invert(&q_b2n, &(_ins->state.quat));
616  float_quat_vmult(&YBt, &q_b2n, &(_ins->meas.mag));
617 
618  float_quat_vmult(&I, &q_b2n, &(_ins->cmd.accel));
619  VECT3_SMUL(I, I, 1. / (_ins->state.as));
620 
621  /*--------- E = ( ŷ - y ) ----------*/
622  /* Eb = ( B - YBt ) */
623  VECT3_DIFF(Eb, B, YBt);
624 
625  // pos and speed error only if GPS data are valid
626  // or while waiting first GPS data to prevent diverging
628 #if INS_FINV_USE_UTM
630 #else
632 #endif
633  ) || !ins_gps_fix_once) {
634  /* Ev = (V - YV) */
635  VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
636  /* Ex = (X - YX) */
637  VECT3_DIFF(Ex, _ins->state.pos, _ins->meas.pos_gps);
638  } else {
639  FLOAT_VECT3_ZERO(Ev);
640  FLOAT_VECT3_ZERO(Ex);
641  }
642  /* Eh = < X,e3 > - hb - YH */
643  Eh = _ins->state.pos.z - _ins->state.hb - _ins->meas.baro_alt;
644 
645  /*--------------Gains--------------*/
646 
647  /**** LvEv + LbEb = -lvIa x Ev + lb < B x Eb, Ia > Ia *****/
648  VECT3_SMUL(Itemp, I, -_ins->gains.lv / 100.);
649  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
650 
651  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
652  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
653  temp = (_ins->gains.lb / 100.) * temp;
654 
655  VECT3_SMUL(Ebtemp, I, temp);
656  VECT3_ADD(Evtemp, Ebtemp);
657  VECT3_COPY(_ins->corr.LE, Evtemp);
658 
659  /***** MvEv + MhEh = -mv * Ev + (-mh * <Eh,e3>)********/
660  _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.;
661  _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.;
662  _ins->corr.ME.z = ((-_ins->gains.mvz) * Ev.z) + ((-_ins->gains.mh) * Eh);
663 
664  /****** NxEx + NhEh = -nx * Ex + (-nh * <Eh, e3>) ********/
665  _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.;
666  _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.;
667  _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh);
668 
669  /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/
670  VECT3_SMUL(Itemp, I, _ins->gains.ov / 1000.);
671  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
672 
673  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
674  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
675  temp = (-_ins->gains.ob / 1000.) * temp;
676 
677  VECT3_SMUL(Ebtemp, I, temp);
678  VECT3_ADD(Evtemp, Ebtemp);
679  VECT3_COPY(_ins->corr.OE, Evtemp);
680 
681  /* a scalar */
682  /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/
683  _ins->corr.RE = ((_ins->gains.rv / 100.) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh / 10000.) * Eh);
684 
685  /****** ShEh ******/
686  _ins->corr.SE = (_ins->gains.sh) * Eh;
687 
688 }
689 
690 
691 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
692  struct FloatVect3 *vi)
693 {
694  struct FloatVect3 qvec, v1, v2;
695  float qi;
696 
697  FLOAT_QUAT_EXTRACT(qvec, *q);
698  qi = - VECT3_DOT_PRODUCT(*vi, qvec);
699  VECT3_CROSS_PRODUCT(v1, *vi, qvec);
700  VECT3_SMUL(v2, *vi, q->qi);
701  VECT3_ADD(v2, v1);
702  QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
703 }
704 
706 {
708 
709  if (!ins_float_inv.is_aligned) {
710  /* Set ltp_to_imu so that body is zero */
711  ins_float_inv.state.quat = *q_b2i;
712  }
713 }
void ins_float_invariant_update_mag(struct Int32Vect3 *mag)
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:243
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:249
float nxz
Tuning parameter of vertical position error on position.
Interface to align the AHRS via low-passed measurements at startup.
int32_t north
in centimeters
#define INV_STATE_DIM
Invariant filter state dimension.
angular rates
void ins_float_invariant_propagate(struct Int32Rates *gyro, struct Int32Vect3 *accel, float dt)
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
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
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 OE
Correction gains on magnetometer sensitivity.
definition of the local (flat earth) coordinate system
static const struct FloatVect3 A
#define B
Generic transmission transport header.
Definition: transport.h:89
#define INS_INV_RV
float north
in meters
bool_t ins_baro_initialized
float alt
in meters above WGS84 reference ellipsoid
#define FLOAT_QUAT_NORMALIZE(_q)
bool_t reset
flag to request reset/reinit the filter
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 ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
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:547
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:216
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
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
float r
in rad/s
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:181
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:329
bool_t log_started
#define FLOAT_RATES_ZERO(_r)
bool_t ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:219
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1064
#define INS_INV_LB
uint8_t nav_utm_zone0
Definition: common_nav.c:44
Integrated Navigation System interface.
#define VECT3_SUM(_c, _a, _b)
Definition: pprz_algebra.h:160
bool_t ins_gps_fix_once
position in UTM coordinates Units: meters
int32_t east
in centimeters
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:43
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.
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:70
#define INS_INV_NXZ
int32_t hmsl
Height above mean sea level in mm.
struct FloatVect3 LE
Correction gains on attitude.
#define FALSE
Definition: std.h:5
int32_t alt
in millimeters above WGS84 reference ellipsoid
static void error_output(struct InsFloatInv *_ins)
Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error ) ...
euler angles
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:565
uint8_t zone
UTM zone number.
vector in Latitude, Longitude and Altitude
#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:792
float mvz
Tuning parameter of vertical speed error on speed.
#define TRUE
Definition: std.h:4
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:71
#define INS_INV_LV
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:692
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:67
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.
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:440
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:68
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:516
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
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
#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:581
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:228
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:728
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.
#define FLOAT_EULERS_OF_QUAT(_e, _q)
Utility functions for fixed point AHRS implementations.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:162
void ins_float_invariant_update_baro(float pressure)
#define MAG_FROZEN_COUNT
bool_t utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:231
#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)
struct inv_measures meas
measurement vector
API to get/set the generic vehicle states.
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.
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
#define INS_INV_MV
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:740
rotation matrix
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
void ins_float_invariant_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:72
float rv
Tuning parameter of speed error on accel biases.
INS using invariant filter.
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:336
float hb
Estimates barometers bias.
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:69
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:555
#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:1152
void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
rotate anglular rates by transposed rotation matrix.
struct NedCoor_f pos
Estimates position.
#define INS_INV_RH
struct InsFloatInv ins_float_inv
uint8_t fix
status of fix
Definition: gps.h:82
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:73
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:973
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:460
Invariant filter state.
struct GpsState gps
global GPS state
Definition: gps.c:41
void ins_float_invariant_init(void)
angular rates
float as
Estimated accelerometer sensitivity.
#define LLA_FLOAT_OF_BFP(_o, _i)
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
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
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