Paparazzi UAS  v6.0_unstable-92-g17422e4-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 
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 FIXEDWING_FIRMWARE
40 #endif
41 
43 #include "math/pprz_algebra_int.h"
44 #include "math/pprz_rk_float.h"
45 #include "math/pprz_isa.h"
46 
47 #include "state.h"
48 
49 // for debugging
50 #if SEND_INVARIANT_FILTER || PERIODIC_TELEMETRY
52 #endif
53 
54 #if LOG_INVARIANT_FILTER
56 bool log_started = false;
57 #endif
58 
59 /*------------- =*= Invariant Observers =*= -------------*
60  *
61  * State vector :
62  *
63  * x = [q0 q1 q2 q3 vx vy vz px py pz wb1 wb2 wb3 hb as]'
64  *
65  * Dynamic model (dim = 15) :
66  *
67  * x_qdot = 0.5 * x_quat * ( x_rates - x_bias );
68  * x_Vdot = A + 1/as (q * am * (q)-1);
69  * x_Xdot = V;
70  * x_bias_dot = 0;
71  * x_asdot = 0;
72  * x_hbdot = 0;
73  *
74  * Observation model (dim = 10):
75  * yv = V;
76  * yx = X;
77  * yh = <X,e3> - hb;
78  * yb = (q)-1 *B * q; (B : magnetometers)
79  *
80  *------------------------------------------------------*/
81 
82 // Default values for the tuning gains
83 // Tuning parameter of speed error on attitude (e-2)
84 #ifndef INS_INV_LV
85 #define INS_INV_LV 2.f
86 #endif
87 // Tuning parameter of mag error on attitude (e-2)
88 #ifndef INS_INV_LB
89 #define INS_INV_LB 6.f
90 #endif
91 // Tuning parameter of horizontal speed error on speed
92 #ifndef INS_INV_MV
93 #define INS_INV_MV 8.f
94 #endif
95 // Tuning parameter of vertical speed error on speed
96 #ifndef INS_INV_MVZ
97 #define INS_INV_MVZ 15.f
98 #endif
99 // Tuning parameter of baro error on vertical speed
100 #ifndef INS_INV_MH
101 #define INS_INV_MH 0.2f
102 #endif
103 // Tuning parameter of horizontal position error on position
104 #ifndef INS_INV_NX
105 #define INS_INV_NX 0.8f
106 #endif
107 // Tuning parameter of vertical position error on position
108 #ifndef INS_INV_NXZ
109 #define INS_INV_NXZ 0.5f
110 #endif
111 // Tuning parameter of baro error on vertical position
112 #ifndef INS_INV_NH
113 #define INS_INV_NH 1.2f
114 #endif
115 // Tuning parameter of speed error on gyro biases (e-3)
116 #ifndef INS_INV_OV
117 #define INS_INV_OV 1.2f
118 #endif
119 // Tuning parameter of mag error on gyro biases (e-3)
120 #ifndef INS_INV_OB
121 #define INS_INV_OB 1.f
122 #endif
123 // Tuning parameter of speed error on accel biases (e-2)
124 #ifndef INS_INV_RV
125 #define INS_INV_RV 4.f
126 #endif
127 // Tuning parameter of baro error on accel biases (vertical projection) (e-8)
128 #ifndef INS_INV_RH
129 #define INS_INV_RH 8.f
130 #endif
131 // Tuning parameter of baro error on baro bias
132 #ifndef INS_INV_SH
133 #define INS_INV_SH 0.01f
134 #endif
135 
136 
138 
139 /* earth gravity model */
140 static const struct FloatVect3 A = { 0.f, 0.f, 9.81f };
141 
142 /* earth magnetic model */
143 //static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) };
144 #define B ins_float_inv.mag_h
145 
146 /* barometer */
148 
149 /* gps */
151 
152 /* min speed to update heading from GPS when mag are not used */
153 #ifndef INS_INV_HEADING_UPDATE_GPS_MIN_SPEED
154 #define INS_INV_HEADING_UPDATE_GPS_MIN_SPEED 5.f
155 #endif
156 
157 /* error computation */
158 static inline void error_output(struct InsFloatInv *_ins);
159 
160 /* propagation model (called by runge-kutta library) */
161 static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m);
162 
163 
167 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
168  struct FloatVect3 *vi);
169 
170 
171 /* init state and measurements */
172 static inline void init_invariant_state(void)
173 {
174  // init state
179  ins_float_inv.state.as = 1.0f;
180  ins_float_inv.state.hb = 0.0f;
181 
182  // init measures
185  ins_float_inv.meas.baro_alt = 0.0f;
186 
187  // init baro
188  ins_baro_initialized = false;
189  ins_gps_fix_once = false;
190 }
191 
192 #if SEND_INVARIANT_FILTER || PERIODIC_TELEMETRY
193 static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
194 {
195  struct FloatEulers eulers;
197  pprz_msg_send_INV_FILTER(trans, dev,
198  AC_ID,
200  &eulers.phi,
201  &eulers.theta,
202  &eulers.psi,
216 }
217 #endif
218 
220 {
221 
222  // init position
223 #if FIXEDWING_FIRMWARE
224  struct UtmCoor_f utm0;
225  utm0.north = (float)nav_utm_north0;
226  utm0.east = (float)nav_utm_east0;
227  utm0.alt = GROUND_ALT;
228  utm0.zone = nav_utm_zone0;
230  stateSetPositionUtm_f(&utm0);
231 #else
232  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
233  llh_nav0.lat = NAV_LAT0;
234  llh_nav0.lon = NAV_LON0;
235  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
236  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
237  struct EcefCoor_i ecef_nav0;
238  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
239  struct LtpDef_i ltp_def;
240  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
241  ltp_def.hmsl = NAV_ALT0;
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 FIXEDWING_FIRMWARE
285  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
286  // reset state UTM ref
288 #else
289  struct LtpDef_i ltp_def;
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  // reset state position and velocity to zero
297 }
298 
300 {
301 #if FIXEDWING_FIRMWARE
302  struct UtmCoor_f utm = state.utm_origin_f;
303  utm.alt = gps.hmsl / 1000.0f;
305 #else
306  struct LlaCoor_i lla = {
308  .lon = state.ned_origin_i.lla.lon,
309  .alt = gps.lla_pos.alt
310  };
311  struct LtpDef_i ltp_def;
312  ltp_def_from_lla_i(&ltp_def, &lla);
313  ltp_def.hmsl = gps.hmsl;
314  stateSetLocalOrigin_i(&ltp_def);
315 #endif
316  // reset vertical position and velocity to zero
317  ins_float_inv.state.pos.z = 0.f;
318  ins_float_inv.state.speed.z = 0.f;
319 }
320 
322  struct FloatVect3 *lp_accel,
323  struct FloatVect3 *lp_mag __attribute__((unused)))
324 {
325 #if USE_MAGNETOMETER
326  /* Compute an initial orientation from accel and mag directly as quaternion */
328 #else
329  /* Compute an initial orientation from accel only directly as quaternion */
331 #endif
332 
333  /* use average gyro as initial value for bias */
334  ins_float_inv.state.bias = *lp_gyro;
335 
336  /* push initial values to state interface */
338 
339  // ins and ahrs are now running
340  ins_float_inv.is_aligned = true;
341 }
342 
343 void ins_float_invariant_propagate(struct FloatRates* gyro, struct FloatVect3* accel, float dt)
344 {
345  struct FloatRates body_rates;
346 
347  // realign all the filter if needed
348  // a complete init cycle is required
349  if (ins_float_inv.reset) {
350  ins_float_inv.reset = false;
351  ins_float_inv.is_aligned = false;
353  }
354 
355  // fill command vector in body frame
356  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
357  float_rmat_transp_ratemult(&ins_float_inv.cmd.rates, body_to_imu_rmat, gyro);
358  float_rmat_transp_vmult(&ins_float_inv.cmd.accel, body_to_imu_rmat, accel);
359 
360  struct Int32Vect3 body_accel_i;
361  ACCELS_BFP_OF_REAL(body_accel_i, ins_float_inv.cmd.accel);
362  stateSetAccelBody_i(&body_accel_i);
363 
364  // update correction gains
366 
367  // propagate model
368  struct inv_state new_state;
369  runge_kutta_4_float((float *)&new_state,
370  (float *)&ins_float_inv.state, INV_STATE_DIM,
371  (float *)&ins_float_inv.cmd, INV_COMMAND_DIM,
372  invariant_model, dt);
373  ins_float_inv.state = new_state;
374 
375  // normalize quaternion
377 
378  // set global state
381  stateSetBodyRates_f(&body_rates);
384  // untilt accel and remove gravity
385  struct FloatQuat q_b2n;
387  struct FloatVect3 accel_n;
388  float_quat_vmult(&accel_n, &q_b2n, &ins_float_inv.cmd.accel);
389  VECT3_SMUL(accel_n, accel_n, 1.f / (ins_float_inv.state.as));
390  VECT3_ADD(accel_n, A);
391  stateSetAccelNed_f((struct NedCoor_f *)&accel_n);
392 
393  //------------------------------------------------------------//
394 
395 #if SEND_INVARIANT_FILTER
396  RunOnceEvery(3, send_inv_filter(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
397 #endif
398 
399 #if LOG_INVARIANT_FILTER
400  if (pprzLogFile != -1) {
401  if (!log_started) {
402  // log file header
403  sdLogWriteLog(pprzLogFile,
404  "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
405  log_started = true;
406  } else {
407  sdLogWriteLog(pprzLogFile,
408  "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
440  }
441  }
442 #endif
443 }
444 
446 {
447 
448  if (gps_s->fix >= GPS_FIX_3D && ins_float_inv.is_aligned) {
449 #if FIXEDWING_FIRMWARE
450  if (state.utm_initialized_f) {
451  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
452  // position (local ned)
456  // speed
457  ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f;
458  ins_float_inv.meas.speed_gps.y = gps_s->ned_vel.y / 100.0f;
459  ins_float_inv.meas.speed_gps.z = gps_s->ned_vel.z / 100.0f;
460  }
461 #else
462  if (state.ned_initialized_f) {
463  struct NedCoor_i gps_pos_cm_ned, ned_pos;
464  ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &gps_s->ecef_pos);
467  struct EcefCoor_f ecef_vel;
468  ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel);
470  }
471 #endif
472  // when getting first GPS pos (or after reset), set state to current pos and speed
473  if (!ins_gps_fix_once) {
476  ins_gps_fix_once = true;
477  }
478  }
479 
480 #if !USE_MAGNETOMETER
481 #if INS_INV_USE_GPS_HEADING
482  // got a 3d fix, ground speed > INS_INV_HEADING_UPDATE_GPS_MIN_SPEED (default 5.0 m/s)
483  // and course accuracy is better than 10deg
484  static const uint16_t gps_min_speed = INS_INV_HEADING_UPDATE_GPS_MIN_SPEED * 100;
485  static const uint32_t max_cacc = RadOfDeg(10 * 1e7);
486  if (gps_s->fix >= GPS_FIX_3D &&
487  gps_s->gspeed >= gps_min_speed &&
488  gps_s->cacc <= max_cacc) {
489  // gps_s->course is in rad * 1e7
490  struct FloatVect3 pseudo_mag = {
491  cosf((float)gps_s->course / 1e7f),
492  -sinf((float)gps_s->course / 1e7f),
493  0.f
494  };
495  ins_float_invariant_update_mag(&pseudo_mag);
496  }
497  else {
498  // if speed is tool low, better set measurements to zero
500  }
501 #else // else use GPS velocity (only for fixedwing)
502  // Use pseudo-mag rebuilt from GPS horizontal velocity
504  float vel_norm = float_vect2_norm(&vel);
505  if (vel_norm > INS_INV_HEADING_UPDATE_GPS_MIN_SPEED) {
506  struct FloatVect3 pseudo_mag = {
507  vel.x / vel_norm,
508  -vel.y / vel_norm,
509  0.f
510  };
511  ins_float_invariant_update_mag(&pseudo_mag);
512  }
513  else {
514  // if speed is tool low, better set measurements to zero
516  }
517 #endif
518 #endif
519 
520 }
521 
522 
524 {
525  static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE;
526  static float alpha = 10.0f;
527  static int32_t i = 1;
528  static float baro_moy = 0.0f;
529  static float baro_prev = 0.0f;
530 
531  if (!ins_baro_initialized) {
532  // try to find a stable qfe
533  // TODO generic function in pprz_isa ?
534  if (i == 1) {
535  baro_moy = pressure;
536  baro_prev = pressure;
537  }
538  baro_moy = (baro_moy * (i - 1) + pressure) / i;
539  alpha = (10.f*alpha + (baro_moy - baro_prev)) / (11.0f);
540  baro_prev = baro_moy;
541  // test stop condition
542  if (fabs(alpha) < 0.1f) {
543  ins_qfe = baro_moy;
544  ins_baro_initialized = true;
545  }
546  if (i == 250) {
547  ins_qfe = pressure;
548  ins_baro_initialized = true;
549  }
550  i++;
551  } else { /* normal update with baro measurement */
552  ins_float_inv.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
553  }
554 }
555 
556 // assume mag is dead when values are not moving anymore
557 #define MAG_FROZEN_COUNT 30
558 
560 {
561  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
562  static int32_t last_mx = 0;
563 
564  if (last_mx == mag->x) {
565  mag_frozen_count--;
566  if (mag_frozen_count == 0) {
567  // if mag is dead, better set measurements to zero
569  mag_frozen_count = MAG_FROZEN_COUNT;
570  }
571  } else {
572  // values are moving
573  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_float_inv.body_to_imu);
574  // new values in body frame
575  float_rmat_transp_vmult(&ins_float_inv.meas.mag, body_to_imu_rmat, mag);
576  // reset counter
577  mag_frozen_count = MAG_FROZEN_COUNT;
578  }
579  last_mx = mag->x;
580 }
581 
582 
587 static inline void invariant_model(float *o, const float *x, const int n, const float *u,
588  const int m __attribute__((unused)))
589 {
590 
591 #pragma GCC diagnostic push // require GCC 4.6
592 #pragma GCC diagnostic ignored "-Wcast-qual"
593  struct inv_state *s = (struct inv_state *)x;
594  struct inv_command *c = (struct inv_command *)u;
595 #pragma GCC diagnostic pop // require GCC 4.6
596  struct inv_state s_dot;
597  struct FloatRates rates_unbiased;
598  struct FloatVect3 tmp_vect;
599  struct FloatQuat tmp_quat;
600 
601  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
602  RATES_DIFF(rates_unbiased, c->rates, s->bias);
603  /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
604  float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
605 
606  float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_float_inv.corr.LE);
607  QUAT_ADD(s_dot.quat, tmp_quat);
608 
609  float norm2_r = 1.f - FLOAT_QUAT_NORM2(s->quat);
610  QUAT_SMUL(tmp_quat, s->quat, norm2_r);
611  QUAT_ADD(s_dot.quat, tmp_quat);
612 
613  /* dot_V = A + (1/as) * (q * am * q-1) + ME */
614  struct FloatQuat q_b2n;
615  float_quat_invert(&q_b2n, &(s->quat));
616  float_quat_vmult((struct FloatVect3 *)&s_dot.speed, &q_b2n, &(c->accel));
617  VECT3_SMUL(s_dot.speed, s_dot.speed, 1.f / (s->as));
618  VECT3_ADD(s_dot.speed, A);
620 
621  /* dot_X = V + NE */
622  VECT3_SUM(s_dot.pos, s->speed, ins_float_inv.corr.NE);
623 
624  /* bias_dot = q-1 * (OE) * q */
625  float_quat_vmult(&tmp_vect, &(s->quat), &ins_float_inv.corr.OE);
626  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
627 
628  /* as_dot = as * RE */
629  s_dot.as = (s->as) * (ins_float_inv.corr.RE);
630  // keep as in a reasonable range, so 50% around the nominal value
631  if ( ((s->as < 0.5f) && (s_dot.as < 0.f)) || ((s->as > 1.5f) && (s_dot.as > 0.f)) ) {
632  s_dot.as = 0.f;
633  }
634 
635  /* hb_dot = SE */
636  s_dot.hb = ins_float_inv.corr.SE;
637 
638  // set output
639  memcpy(o, &s_dot, n * sizeof(float));
640 }
641 
646 static inline void error_output(struct InsFloatInv *_ins)
647 {
648 
649  struct FloatVect3 YBt, I, Ev, Eb, Ex, Itemp, Ebtemp, Evtemp;
650  float Eh;
651  float temp;
652 
653  /* YBt = q * yB * q-1 */
654  struct FloatQuat q_b2n;
655  float_quat_invert(&q_b2n, &(_ins->state.quat));
656  float_quat_vmult(&YBt, &q_b2n, &(_ins->meas.mag));
657 
658  float_quat_vmult(&I, &q_b2n, &(_ins->cmd.accel));
659  VECT3_SMUL(I, I, 1.f / (_ins->state.as));
660 
661  /*--------- E = ( ŷ - y ) ----------*/
662  /* Eb = ( B - YBt ) */
663  VECT3_DIFF(Eb, B, YBt);
664 
665  // pos and speed error only if GPS data are valid
666  // or while waiting first GPS data to prevent diverging
667  if ((gps.fix >= GPS_FIX_3D && _ins->is_aligned
668 #if FIXEDWING_FIRMWARE
670 #else
672 #endif
673  ) || !ins_gps_fix_once) {
674  /* Ev = (V - YV) */
675  VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
676  /* Ex = (X - YX) */
677  VECT3_DIFF(Ex, _ins->state.pos, _ins->meas.pos_gps);
678  } else {
679  FLOAT_VECT3_ZERO(Ev);
680  FLOAT_VECT3_ZERO(Ex);
681  }
682  /* Eh = < X,e3 > - hb - YH */
683  Eh = _ins->state.pos.z - _ins->state.hb - _ins->meas.baro_alt;
684 
685  /*--------------Gains--------------*/
686 
687  /**** LvEv + LbEb = -lvIa x Ev + lb < B x Eb, Ia > Ia *****/
688  VECT3_SMUL(Itemp, I, -_ins->gains.lv / 100.f);
689  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
690 
691  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
692  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
693  temp = (_ins->gains.lb / 100.f) * temp;
694 
695  VECT3_SMUL(Ebtemp, I, temp);
696  VECT3_ADD(Evtemp, Ebtemp);
697  VECT3_COPY(_ins->corr.LE, Evtemp);
698 
699  /***** MvEv + MhEh = -mv * Ev + (-mh * <Eh,e3>)********/
700  _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.f;
701  _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.f;
702  _ins->corr.ME.z = ((-_ins->gains.mvz) * Ev.z) + ((-_ins->gains.mh) * Eh);
703 
704  /****** NxEx + NhEh = -nx * Ex + (-nh * <Eh, e3>) ********/
705  _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.f;
706  _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.f;
707  _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh);
708 
709  /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/
710  VECT3_SMUL(Itemp, I, _ins->gains.ov / 1000.f);
711  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
712 
713  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
714  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
715  temp = (-_ins->gains.ob / 1000.f) * temp;
716 
717  VECT3_SMUL(Ebtemp, I, temp);
718  VECT3_ADD(Evtemp, Ebtemp);
719  VECT3_COPY(_ins->corr.OE, Evtemp);
720 
721  /* a scalar */
722  /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/
723  _ins->corr.RE = ((_ins->gains.rv / 100.f) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh / 10000.f) * Eh);
724 
725  /****** ShEh ******/
726  _ins->corr.SE = (_ins->gains.sh) * Eh;
727 
728 }
729 
730 
731 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
732  struct FloatVect3 *vi)
733 {
734  struct FloatVect3 qvec, v1, v2;
735  float qi;
736 
737  FLOAT_QUAT_EXTRACT(qvec, *q);
738  qi = - VECT3_DOT_PRODUCT(*vi, qvec);
739  VECT3_CROSS_PRODUCT(v1, *vi, qvec);
740  VECT3_SMUL(v2, *vi, q->qi);
741  VECT3_ADD(v2, v1);
742  QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
743 }
744 
746 {
748 
749  if (!ins_float_inv.is_aligned) {
750  /* Set ltp_to_imu so that body is zero */
751  ins_float_inv.state.quat = *q_b2i;
752  }
753 }
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
#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.
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:104
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.
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
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
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:86
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
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
Definition: wedgebug.c:204
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
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.
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:390
API to get/set the generic vehicle states.
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
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
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
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.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
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
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
struct inv_correction_gains corr
correction gains
bool ins_baro_initialized