Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
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
57 bool log_started = false;
58 #endif
59 
60 /*------------- =*= Invariant Observers =*= -------------*
61  *
62  * State vector :
63  *
64  * x = [q0 q1 q2 q3 vx vy vz px py pz wb1 wb2 wb3 hb as]'
65  *
66  * Dynamic model (dim = 15) :
67  *
68  * x_qdot = 0.5 * x_quat * ( x_rates - x_bias );
69  * x_Vdot = A + 1/as (q * am * (q)-1);
70  * x_Xdot = V;
71  * x_bias_dot = 0;
72  * x_asdot = 0;
73  * x_hbdot = 0;
74  *
75  * Observation model (dim = 10):
76  * yv = V;
77  * yx = X;
78  * yh = <X,e3> - hb;
79  * yb = (q)-1 *B * q; (B : magnetometers)
80  *
81  *------------------------------------------------------*/
82 
83 // Default values for the tuning gains
84 // Tuning parameter of speed error on attitude (e-2)
85 #ifndef INS_INV_LV
86 #define INS_INV_LV 2.
87 #endif
88 // Tuning parameter of mag error on attitude (e-2)
89 #ifndef INS_INV_LB
90 #define INS_INV_LB 6.
91 #endif
92 // Tuning parameter of horizontal speed error on speed
93 #ifndef INS_INV_MV
94 #define INS_INV_MV 8.
95 #endif
96 // Tuning parameter of vertical speed error on speed
97 #ifndef INS_INV_MVZ
98 #define INS_INV_MVZ 15.
99 #endif
100 // Tuning parameter of baro error on vertical speed
101 #ifndef INS_INV_MH
102 #define INS_INV_MH 0.2
103 #endif
104 // Tuning parameter of horizontal position error on position
105 #ifndef INS_INV_NX
106 #define INS_INV_NX 0.8
107 #endif
108 // Tuning parameter of vertical position error on position
109 #ifndef INS_INV_NXZ
110 #define INS_INV_NXZ 0.5
111 #endif
112 // Tuning parameter of baro error on vertical position
113 #ifndef INS_INV_NH
114 #define INS_INV_NH 1.2
115 #endif
116 // Tuning parameter of speed error on gyro biases (e-3)
117 #ifndef INS_INV_OV
118 #define INS_INV_OV 1.2
119 #endif
120 // Tuning parameter of mag error on gyro biases (e-3)
121 #ifndef INS_INV_OB
122 #define INS_INV_OB 1.
123 #endif
124 // Tuning parameter of speed error on accel biases (e-2)
125 #ifndef INS_INV_RV
126 #define INS_INV_RV 4.
127 #endif
128 // Tuning parameter of baro error on accel biases (vertical projection) (e-8)
129 #ifndef INS_INV_RH
130 #define INS_INV_RH 8.
131 #endif
132 // Tuning parameter of baro error on baro bias
133 #ifndef INS_INV_SH
134 #define INS_INV_SH 0.01
135 #endif
136 
137 
139 
140 /* earth gravity model */
141 static const struct FloatVect3 A = { 0.f, 0.f, 9.81f };
142 
143 /* earth magnetic model */
144 //static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) };
145 #define B ins_float_inv.mag_h
146 
147 /* barometer */
149 
150 /* gps */
152 
153 /* error computation */
154 static inline void error_output(struct InsFloatInv *_ins);
155 
156 /* propagation model (called by runge-kutta library) */
157 static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m);
158 
159 
163 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
164  struct FloatVect3 *vi);
165 
166 
167 /* init state and measurements */
168 static inline void init_invariant_state(void)
169 {
170  // init state
175  ins_float_inv.state.as = 1.0f;
176  ins_float_inv.state.hb = 0.0f;
177 
178  // init measures
181  ins_float_inv.meas.baro_alt = 0.0f;
182 
183  // init baro
184  ins_baro_initialized = false;
185  ins_gps_fix_once = false;
186 }
187 
188 #if SEND_INVARIANT_FILTER || PERIODIC_TELEMETRY
189 static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
190 {
191  struct FloatEulers eulers;
193  pprz_msg_send_INV_FILTER(trans, dev,
194  AC_ID,
196  &eulers.phi,
197  &eulers.theta,
198  &eulers.psi,
212 }
213 #endif
214 
216 {
217 
218  // init position
219 #if INS_FINV_USE_UTM
220  struct UtmCoor_f utm0;
221  utm0.north = (float)nav_utm_north0;
222  utm0.east = (float)nav_utm_east0;
223  utm0.alt = GROUND_ALT;
224  utm0.zone = nav_utm_zone0;
226  stateSetPositionUtm_f(&utm0);
227 #else
228  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
229  llh_nav0.lat = NAV_LAT0;
230  llh_nav0.lon = NAV_LON0;
231  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
232  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
233  struct EcefCoor_i ecef_nav0;
234  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
235  struct LtpDef_i ltp_def;
236  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
237  ltp_def.hmsl = NAV_ALT0;
238  stateSetLocalOrigin_i(&ltp_def);
239 #endif
240 
241  B.x = INS_H_X;
242  B.y = INS_H_Y;
243  B.z = INS_H_Z;
244 
245  // init state and measurements
247 
248  // init gains
262 
263  ins_float_inv.is_aligned = false;
264  ins_float_inv.reset = false;
265 
266 #if PERIODIC_TELEMETRY
268 #endif
269 }
270 
271 
273 {
274 #if INS_FINV_USE_UTM
275  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
276  // reset state UTM ref
278 #else
279  struct LtpDef_i ltp_def;
280  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
281  ltp_def.hmsl = gps.hmsl;
282  stateSetLocalOrigin_i(&ltp_def);
283 #endif
284 }
285 
287 {
288 #if INS_FINV_USE_UTM
289  struct UtmCoor_f utm = state.utm_origin_f;
290  utm.alt = gps.hmsl / 1000.0f;
292 #else
293  struct LlaCoor_i lla = {
295  .lon = state.ned_origin_i.lla.lon,
296  .alt = gps.lla_pos.alt
297  };
298  struct LtpDef_i ltp_def;
299  ltp_def_from_lla_i(&ltp_def, &lla);
300  ltp_def.hmsl = gps.hmsl;
301  stateSetLocalOrigin_i(&ltp_def);
302 #endif
303 }
304 
306  struct FloatVect3 *lp_accel,
307  struct FloatVect3 *lp_mag)
308 {
309  /* Compute an initial orientation from accel and mag directly as quaternion */
311 
312  /* use average gyro as initial value for bias */
313  ins_float_inv.state.bias = *lp_gyro;
314 
315  /* push initial values to state interface */
317 
318  // ins and ahrs are now running
319  ins_float_inv.is_aligned = true;
320 }
321 
322 void ins_float_invariant_propagate(struct FloatRates* gyro, struct FloatVect3* accel, float dt)
323 {
324  struct FloatRates body_rates;
325 
326  // realign all the filter if needed
327  // a complete init cycle is required
328  if (ins_float_inv.reset) {
329  ins_float_inv.reset = false;
330  ins_float_inv.is_aligned = false;
332  }
333 
334  // fill command vector in body frame
335  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
336  float_rmat_transp_ratemult(&ins_float_inv.cmd.rates, body_to_imu_rmat, gyro);
337  float_rmat_transp_vmult(&ins_float_inv.cmd.accel, body_to_imu_rmat, accel);
338 
339  // update correction gains
341 
342  // propagate model
343  struct inv_state new_state;
344  runge_kutta_4_float((float *)&new_state,
345  (float *)&ins_float_inv.state, INV_STATE_DIM,
346  (float *)&ins_float_inv.cmd, INV_COMMAND_DIM,
347  invariant_model, dt);
348  ins_float_inv.state = new_state;
349 
350  // normalize quaternion
352 
353  // set global state
356  stateSetBodyRates_f(&body_rates);
359  // untilt accel and remove gravity
360  struct FloatQuat q_b2n;
362  struct FloatVect3 accel_n;
363  float_quat_vmult(&accel_n, &q_b2n, &ins_float_inv.cmd.accel);
364  VECT3_SMUL(accel_n, accel_n, 1. / (ins_float_inv.state.as));
365  VECT3_ADD(accel_n, A);
366  stateSetAccelNed_f((struct NedCoor_f *)&accel_n);
367 
368  //------------------------------------------------------------//
369 
370 #if SEND_INVARIANT_FILTER
371  RunOnceEvery(3, send_inv_filter(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
372 #endif
373 
374 #if LOG_INVARIANT_FILTER
375  if (pprzLogFile != -1) {
376  if (!log_started) {
377  // log file header
378  sdLogWriteLog(pprzLogFile,
379  "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");
380  log_started = true;
381  } else {
382  sdLogWriteLog(pprzLogFile,
383  "%.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",
415  }
416  }
417 #endif
418 }
419 
421 {
422 
423  if (gps_s->fix >= GPS_FIX_3D && ins_float_inv.is_aligned) {
424  ins_gps_fix_once = true;
425 
426 #if INS_FINV_USE_UTM
427  if (state.utm_initialized_f) {
428  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
429  // position (local ned)
433  // speed
434  ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f;
435  ins_float_inv.meas.speed_gps.y = gps_s->ned_vel.y / 100.0f;
436  ins_float_inv.meas.speed_gps.z = gps_s->ned_vel.z / 100.0f;
437  }
438 #else
439  if (state.ned_initialized_f) {
440  struct NedCoor_i gps_pos_cm_ned, ned_pos;
441  ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &gps_s->ecef_pos);
444  struct EcefCoor_f ecef_vel;
445  ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel);
447  }
448 #endif
449  }
450 
451 }
452 
453 
455 {
456  static float ins_qfe = 101325.0f;
457  static float alpha = 10.0f;
458  static int32_t i = 1;
459  static float baro_moy = 0.0f;
460  static float baro_prev = 0.0f;
461 
462  if (!ins_baro_initialized) {
463  // try to find a stable qfe
464  // TODO generic function in pprz_isa ?
465  if (i == 1) {
466  baro_moy = pressure;
467  baro_prev = pressure;
468  }
469  baro_moy = (baro_moy * (i - 1) + pressure) / i;
470  alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f);
471  baro_prev = baro_moy;
472  // test stop condition
473  if (fabs(alpha) < 0.005f) {
474  ins_qfe = baro_moy;
475  ins_baro_initialized = true;
476  }
477  if (i == 250) {
478  ins_qfe = pressure;
479  ins_baro_initialized = true;
480  }
481  i++;
482  } else { /* normal update with baro measurement */
483  ins_float_inv.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
484  }
485 }
486 
487 // assume mag is dead when values are not moving anymore
488 #define MAG_FROZEN_COUNT 30
489 
491 {
492  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
493  static int32_t last_mx = 0;
494 
495  if (last_mx == mag->x) {
496  mag_frozen_count--;
497  if (mag_frozen_count == 0) {
498  // if mag is dead, better set measurements to zero
500  mag_frozen_count = MAG_FROZEN_COUNT;
501  }
502  } else {
503  // values are moving
504  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
505  // new values in body frame
506  float_rmat_transp_vmult(&ins_float_inv.meas.mag, body_to_imu_rmat, mag);
507  // reset counter
508  mag_frozen_count = MAG_FROZEN_COUNT;
509  }
510  last_mx = mag->x;
511 }
512 
513 
518 static inline void invariant_model(float *o, const float *x, const int n, const float *u,
519  const int m __attribute__((unused)))
520 {
521 
522 #pragma GCC diagnostic push // require GCC 4.6
523 #pragma GCC diagnostic ignored "-Wcast-qual"
524  struct inv_state *s = (struct inv_state *)x;
525  struct inv_command *c = (struct inv_command *)u;
526 #pragma GCC diagnostic pop // require GCC 4.6
527  struct inv_state s_dot;
528  struct FloatRates rates_unbiased;
529  struct FloatVect3 tmp_vect;
530  struct FloatQuat tmp_quat;
531 
532  // test accel sensitivity
533  if (fabs(s->as) < 0.1) {
534  // too small, return x_dot = 0 to avoid division by 0
535  float_vect_zero(o, n);
536  // TODO set ins state to error
537  return;
538  }
539 
540  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
541  RATES_DIFF(rates_unbiased, c->rates, s->bias);
542  /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
543  float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
544 
545  float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_float_inv.corr.LE);
546  QUAT_ADD(s_dot.quat, tmp_quat);
547 
548  float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat);
549  QUAT_SMUL(tmp_quat, s->quat, norm2_r);
550  QUAT_ADD(s_dot.quat, tmp_quat);
551 
552  /* dot_V = A + (1/as) * (q * am * q-1) + ME */
553  struct FloatQuat q_b2n;
554  float_quat_invert(&q_b2n, &(s->quat));
555  float_quat_vmult((struct FloatVect3 *)&s_dot.speed, &q_b2n, &(c->accel));
556  VECT3_SMUL(s_dot.speed, s_dot.speed, 1. / (s->as));
557  VECT3_ADD(s_dot.speed, A);
559 
560  /* dot_X = V + NE */
561  VECT3_SUM(s_dot.pos, s->speed, ins_float_inv.corr.NE);
562 
563  /* bias_dot = q-1 * (OE) * q */
564  float_quat_vmult(&tmp_vect, &(s->quat), &ins_float_inv.corr.OE);
565  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
566 
567  /* as_dot = as * RE */
568  s_dot.as = (s->as) * (ins_float_inv.corr.RE);
569 
570  /* hb_dot = SE */
571  s_dot.hb = ins_float_inv.corr.SE;
572 
573  // set output
574  memcpy(o, &s_dot, n * sizeof(float));
575 }
576 
581 static inline void error_output(struct InsFloatInv *_ins)
582 {
583 
584  struct FloatVect3 YBt, I, Ev, Eb, Ex, Itemp, Ebtemp, Evtemp;
585  float Eh;
586  float temp;
587 
588  // test accel sensitivity
589  if (fabs(_ins->state.as) < 0.1) {
590  // too small, don't do anything to avoid division by 0
591  return;
592  }
593 
594  /* YBt = q * yB * q-1 */
595  struct FloatQuat q_b2n;
596  float_quat_invert(&q_b2n, &(_ins->state.quat));
597  float_quat_vmult(&YBt, &q_b2n, &(_ins->meas.mag));
598 
599  float_quat_vmult(&I, &q_b2n, &(_ins->cmd.accel));
600  VECT3_SMUL(I, I, 1. / (_ins->state.as));
601 
602  /*--------- E = ( ŷ - y ) ----------*/
603  /* Eb = ( B - YBt ) */
604  VECT3_DIFF(Eb, B, YBt);
605 
606  // pos and speed error only if GPS data are valid
607  // or while waiting first GPS data to prevent diverging
609 #if INS_FINV_USE_UTM
611 #else
613 #endif
614  ) || !ins_gps_fix_once) {
615  /* Ev = (V - YV) */
616  VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
617  /* Ex = (X - YX) */
618  VECT3_DIFF(Ex, _ins->state.pos, _ins->meas.pos_gps);
619  } else {
620  FLOAT_VECT3_ZERO(Ev);
621  FLOAT_VECT3_ZERO(Ex);
622  }
623  /* Eh = < X,e3 > - hb - YH */
624  Eh = _ins->state.pos.z - _ins->state.hb - _ins->meas.baro_alt;
625 
626  /*--------------Gains--------------*/
627 
628  /**** LvEv + LbEb = -lvIa x Ev + lb < B x Eb, Ia > Ia *****/
629  VECT3_SMUL(Itemp, I, -_ins->gains.lv / 100.);
630  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
631 
632  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
633  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
634  temp = (_ins->gains.lb / 100.) * temp;
635 
636  VECT3_SMUL(Ebtemp, I, temp);
637  VECT3_ADD(Evtemp, Ebtemp);
638  VECT3_COPY(_ins->corr.LE, Evtemp);
639 
640  /***** MvEv + MhEh = -mv * Ev + (-mh * <Eh,e3>)********/
641  _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.;
642  _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.;
643  _ins->corr.ME.z = ((-_ins->gains.mvz) * Ev.z) + ((-_ins->gains.mh) * Eh);
644 
645  /****** NxEx + NhEh = -nx * Ex + (-nh * <Eh, e3>) ********/
646  _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.;
647  _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.;
648  _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh);
649 
650  /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/
651  VECT3_SMUL(Itemp, I, _ins->gains.ov / 1000.);
652  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
653 
654  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
655  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
656  temp = (-_ins->gains.ob / 1000.) * temp;
657 
658  VECT3_SMUL(Ebtemp, I, temp);
659  VECT3_ADD(Evtemp, Ebtemp);
660  VECT3_COPY(_ins->corr.OE, Evtemp);
661 
662  /* a scalar */
663  /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/
664  _ins->corr.RE = ((_ins->gains.rv / 100.) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh / 10000.) * Eh);
665 
666  /****** ShEh ******/
667  _ins->corr.SE = (_ins->gains.sh) * Eh;
668 
669 }
670 
671 
672 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
673  struct FloatVect3 *vi)
674 {
675  struct FloatVect3 qvec, v1, v2;
676  float qi;
677 
678  FLOAT_QUAT_EXTRACT(qvec, *q);
679  qi = - VECT3_DOT_PRODUCT(*vi, qvec);
680  VECT3_CROSS_PRODUCT(v1, *vi, qvec);
681  VECT3_SMUL(v2, *vi, q->qi);
682  VECT3_ADD(v2, v1);
683  QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
684 }
685 
687 {
689 
690  if (!ins_float_inv.is_aligned) {
691  /* Set ltp_to_imu so that body is zero */
692  ins_float_inv.state.quat = *q_b2i;
693  }
694 }
#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.
Interface to align the AHRS via low-passed measurements at startup.
#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:547
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
#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:1075
#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
float dt
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
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:576
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:803
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:451
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: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
FileDes pprzLogFile
Definition: sdlog_chibios.c:76
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:592
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:321
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: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:1163
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:984
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:471
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