Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_mekf_wind.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Marton Brossard <martin.brossard@mines-paristech.fr>
3  * Gautier Hattenberger <gautier.hattenberger@enac.fr>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
34 #include "generated/airframe.h"
35 
36 #ifndef SITL
37 // Redifine Eigen assert so it doesn't use memory allocation
38 #define eigen_assert(_cond) { if (!(_cond)) { while(1) ; } }
39 #endif
40 
41 // Eigen headers
42 #pragma GCC diagnostic ignored "-Wint-in-bool-context"
43 #pragma GCC diagnostic ignored "-Wshadow"
44 #include <Eigen/Dense>
45 #pragma GCC diagnostic pop
46 
47 using namespace Eigen;
48 
60 };
61 
62 typedef Matrix<float, MEKF_WIND_COV_SIZE, MEKF_WIND_COV_SIZE> MEKFWCov;
63 
74 };
75 
76 typedef Matrix<float, MEKF_WIND_PROC_NOISE_SIZE, MEKF_WIND_PROC_NOISE_SIZE> MEKFWPNoise;
77 
87 };
88 
89 typedef Matrix<float, MEKF_WIND_MEAS_NOISE_SIZE, MEKF_WIND_MEAS_NOISE_SIZE> MEKFWMNoise;
90 
93 struct MekfWindState {
94  Quaternionf quat;
95  Vector3f speed;
96  Vector3f pos;
97  Vector3f accel;
98  Vector3f rates_bias;
99  Vector3f accel_bias;
100  float baro_bias;
101  Vector3f wind;
102 };
103 
107  Vector3f rates;
108  Vector3f accel;
109 };
110 
114  Vector3f speed;
115  Vector3f pos;
116  Vector3f mag;
117  float baro_alt;
118  float airspeed;
119  float aoa;
120  float aos;
121 };
122 
127  struct MekfWindInputs inputs;
128  struct MekfWindMeasurements measurements;
129 
133 
134  /* earth magnetic model */
135  Vector3f mag_h;
136 };
137 
138 
139 // Initial covariance parameters
140 #ifndef INS_MEKF_WIND_P0_QUAT
141 #define INS_MEKF_WIND_P0_QUAT 0.007615f
142 #endif
143 #ifndef INS_MEKF_WIND_P0_SPEED
144 #define INS_MEKF_WIND_P0_SPEED 1.E+2f
145 #endif
146 #ifndef INS_MEKF_WIND_P0_POS
147 #define INS_MEKF_WIND_P0_POS 1.E+1f
148 #endif
149 #ifndef INS_MEKF_WIND_P0_RATES_BIAS
150 #define INS_MEKF_WIND_P0_RATES_BIAS 1.E-3f
151 #endif
152 #ifndef INS_MEKF_WIND_P0_ACCEL_BIAS
153 #define INS_MEKF_WIND_P0_ACCEL_BIAS 1.E-3f
154 #endif
155 #ifndef INS_MEKF_WIND_P0_BARO_BIAS
156 #define INS_MEKF_WIND_P0_BARO_BIAS 1.E-3f
157 #endif
158 #ifndef INS_MEKF_WIND_P0_WIND
159 #define INS_MEKF_WIND_P0_WIND 1.E-0f
160 #endif
161 
162 // Initial process noise parameters
163 #ifndef INS_MEKF_WIND_Q_GYRO
164 #define INS_MEKF_WIND_Q_GYRO 1.E-2f
165 #endif
166 #ifndef INS_MEKF_WIND_Q_ACCEL
167 #define INS_MEKF_WIND_Q_ACCEL 1.E-2f
168 #endif
169 #ifndef INS_MEKF_WIND_Q_RATES_BIAS
170 #define INS_MEKF_WIND_Q_RATES_BIAS 1.E-6f
171 #endif
172 #ifndef INS_MEKF_WIND_Q_ACCEL_BIAS
173 #define INS_MEKF_WIND_Q_ACCEL_BIAS 1.E-6f
174 #endif
175 #ifndef INS_MEKF_WIND_Q_BARO_BIAS
176 #define INS_MEKF_WIND_Q_BARO_BIAS 1.E-3f
177 #endif
178 #ifndef INS_MEKF_WIND_Q_WIND
179 #define INS_MEKF_WIND_Q_WIND 1.E+1f
180 #endif
181 
182 // Initial measurements noise parameters
183 #ifndef INS_MEKF_WIND_R_SPEED
184 #define INS_MEKF_WIND_R_SPEED 0.1f
185 #endif
186 #ifndef INS_MEKF_WIND_R_SPEED_Z
187 #define INS_MEKF_WIND_R_SPEED_Z 0.2f
188 #endif
189 #ifndef INS_MEKF_WIND_R_POS
190 #define INS_MEKF_WIND_R_POS 2.0f
191 #endif
192 #ifndef INS_MEKF_WIND_R_POS_Z
193 #define INS_MEKF_WIND_R_POS_Z 4.0f
194 #endif
195 #ifndef INS_MEKF_WIND_R_MAG
196 #define INS_MEKF_WIND_R_MAG 1.f
197 #endif
198 #ifndef INS_MEKF_WIND_R_BARO
199 #define INS_MEKF_WIND_R_BARO 2.f
200 #endif
201 #ifndef INS_MEKF_WIND_R_AIRSPEED
202 #define INS_MEKF_WIND_R_AIRSPEED 0.1f
203 #endif
204 #ifndef INS_MEKF_WIND_R_AOA
205 #define INS_MEKF_WIND_R_AOA 0.1f
206 #endif
207 #ifndef INS_MEKF_WIND_R_AOS
208 #define INS_MEKF_WIND_R_AOS 0.1f
209 #endif
210 
211 // Disable wind estimation by default
212 #ifndef INS_MEKF_WIND_DISABLE_WIND
213 #define INS_MEKF_WIND_DISABLE_WIND true
214 #endif
215 
216 // paramters
218 
219 // internal structure
221 // short name
222 #define mwp mekf_wind_private
223 
224 /* earth gravity model */
225 static const Vector3f gravity( 0.f, 0.f, 9.81f );
226 
227 
228 /* init state and measurements */
229 static void init_mekf_state(void)
230 {
231  // init state
232  mekf_wind_private.state.quat = Quaternionf::Identity();
233  mekf_wind_private.state.speed = Vector3f::Zero();
234  mekf_wind_private.state.pos = Vector3f::Zero();
235  mekf_wind_private.state.rates_bias = Vector3f::Zero();
236  mekf_wind_private.state.accel_bias = Vector3f::Zero();
238  mekf_wind_private.state.wind = Vector3f::Zero();
239 
240  // init measures
241  mekf_wind_private.measurements.speed = Vector3f::Zero();
242  mekf_wind_private.measurements.pos = Vector3f::Zero();
243  mekf_wind_private.measurements.mag = Vector3f::Zero();
248 
249  // init input
250  mekf_wind_private.inputs.rates = Vector3f::Zero();
251  mekf_wind_private.inputs.accel = Vector3f::Zero();
252 
253  // init state covariance
254  mekf_wind_private.P = MEKFWCov::Zero();
274 
275  // init process and measurements noise
277 }
278 
279 // Some matrix and quaternion utility functions
280 static Quaternionf quat_add(const Quaternionf& q1, const Quaternionf& q2) {
281  return Quaternionf(q1.w() + q2.w(), q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z());
282 }
283 
284 static Quaternionf quat_smul(const Quaternionf& q1, float scal) {
285  return Quaternionf(q1.w() * scal, q1.x() * scal, q1.y() * scal, q1.z() * scal);
286 }
287 
294 static Matrix3f skew_sym(const Vector3f& v) {
295  Matrix3f m = Matrix3f::Zero();
296  m(0,1) = -v(2);
297  m(0,2) = v(1);
298  m(1,0) = v(2);
299  m(1,2) = -v(0);
300  m(2,0) = -v(1);
301  m(2,1) = v(0);
302  return m;
303 }
304 
309 {
310  // init parameters
327 
328  // init state and measurements
329  init_mekf_state();
330 
331  // init local earth magnetic field
332  mekf_wind_private.mag_h = Vector3f(1.0f, 0.f, 0.f);
333 }
334 
336 {
337  // update local earth magnetic field
338  mekf_wind_private.mag_h(0) = mag_h->x;
339  mekf_wind_private.mag_h(1) = mag_h->y;
340  mekf_wind_private.mag_h(2) = mag_h->z;
341 }
342 
344 {
345  init_mekf_state();
346 }
347 
350 void ins_mekf_wind_propagate(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
351 {
352  Quaternionf q_tmp;
353 
354  mekf_wind_private.inputs.rates = Vector3f(gyro->p, gyro->q, gyro->r);
355  mekf_wind_private.inputs.accel = Vector3f(acc->x, acc->y, acc->z);
356 
357  const Vector3f gyro_unbiased = mwp.inputs.rates - mwp.state.rates_bias;
358  const Vector3f accel_unbiased = mwp.inputs.accel - mwp.state.accel_bias;
359  // propagate state
360  // q_dot = 1/2 q * (rates - rates_bias)
361  q_tmp.w() = 0.f;
362  q_tmp.vec() = gyro_unbiased;
363  const Quaternionf q_d = quat_smul(mwp.state.quat * q_tmp, 0.5f);
364  // speed_d = q * (accel - accel_bias) * q^-1 + g
365  q_tmp.vec() = accel_unbiased;
366  // store NED accel
367  mwp.state.accel = (mwp.state.quat * q_tmp * mwp.state.quat.inverse()).vec() + gravity;
368 
369  // Euler integration
370 
371  //mwp.state.quat = (mwp.state.quat + q_d * dt).normalize();
372  mwp.state.quat = quat_add(mwp.state.quat, quat_smul(q_d, dt));
373  mwp.state.quat.normalize();
374  mwp.state.speed = mwp.state.speed + mwp.state.accel * dt;
375  mwp.state.pos = mwp.state.pos + mwp.state.speed * dt;
376 
377  // propagate covariance
378  const Matrix3f Rq = mwp.state.quat.toRotationMatrix();
379  const Matrix3f Rqdt = Rq * dt;
380  const Matrix3f RqA = skew_sym(Rq * accel_unbiased);
381  const Matrix3f RqAdt = RqA * dt;
382  const Matrix3f RqAdt2 = RqAdt * dt;
383 
384  MEKFWCov A = MEKFWCov::Identity();
385  A.block<3,3>(MEKF_WIND_qx,MEKF_WIND_rbp) = -Rqdt;
386  A.block<3,3>(MEKF_WIND_vx,MEKF_WIND_qx) = -RqAdt;
387  A.block<3,3>(MEKF_WIND_vx,MEKF_WIND_rbp) = RqAdt2;
388  A.block<3,3>(MEKF_WIND_vx,MEKF_WIND_abx) = -Rqdt;
389  A.block<3,3>(MEKF_WIND_px,MEKF_WIND_qx) = -RqAdt2;
390  A.block<3,3>(MEKF_WIND_px,MEKF_WIND_vx) = Matrix3f::Identity() * dt;
391  A.block<3,3>(MEKF_WIND_px,MEKF_WIND_rbp) = RqAdt2 * dt;
392  A.block<3,3>(MEKF_WIND_px,MEKF_WIND_abx) = -Rqdt * dt;
393 
394  Matrix<float, MEKF_WIND_COV_SIZE, MEKF_WIND_PROC_NOISE_SIZE> An;
395  An.setZero();
396  An.block<3,3>(MEKF_WIND_qx,MEKF_WIND_qgp) = Rq;
397  An.block<3,3>(MEKF_WIND_vx,MEKF_WIND_qax) = Rq;
398  An.block<3,3>(MEKF_WIND_rbp,MEKF_WIND_qrbp) = Matrix3f::Identity();
399  An.block<3,3>(MEKF_WIND_abx,MEKF_WIND_qabx) = Matrix3f::Identity();
400  An(MEKF_WIND_bb,MEKF_WIND_qbb) = 1.0f;
401  An.block<3,3>(MEKF_WIND_wx,MEKF_WIND_qwx) = Matrix3f::Identity();
402 
403  MEKFWCov At(A);
404  At.transposeInPlace();
405  Matrix<float, MEKF_WIND_PROC_NOISE_SIZE, MEKF_WIND_COV_SIZE> Ant;
406  Ant = An.transpose();
407 
408  mwp.P = A * mwp.P * At + An * mwp.Q * Ant * dt;
409 
411  mwp.P.block<3,MEKF_WIND_COV_SIZE>(MEKF_WIND_wx,0) = Matrix<float,3,MEKF_WIND_COV_SIZE>::Zero();
412  mwp.P.block<MEKF_WIND_COV_SIZE-3,3>(0,MEKF_WIND_wx) = Matrix<float,MEKF_WIND_COV_SIZE-3,3>::Zero();
416  mwp.state.wind = Vector3f::Zero();
417  }
418 }
419 
422 void ins_mekf_wind_propagate_ahrs(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
423 {
424  Quaternionf q_tmp;
425 
426  mekf_wind_private.inputs.rates = Vector3f(gyro->p, gyro->q, gyro->r);
427  mekf_wind_private.inputs.accel = Vector3f(acc->x, acc->y, acc->z);
428 
429  const Vector3f gyro_unbiased = mwp.inputs.rates - mwp.state.rates_bias;
430  const Vector3f accel_unbiased = mwp.inputs.accel - mwp.state.accel_bias;
431  // propagate state
432  // q_dot = 1/2 q * (rates - rates_bias)
433  q_tmp.w() = 0.f;
434  q_tmp.vec() = gyro_unbiased;
435  const Quaternionf q_d = quat_smul(mwp.state.quat * q_tmp, 0.5f);
436 
437  // Euler integration
438 
439  //mwp.state.quat = (mwp.state.quat + q_d * dt).normalize();
440  mwp.state.quat = quat_add(mwp.state.quat, quat_smul(q_d, dt));
441  mwp.state.quat.normalize();
442 
443  // propagate covariance
444  const Matrix3f Rq = mwp.state.quat.toRotationMatrix();
445  const Matrix3f Rqdt = Rq * dt;
446 
447  MEKFWCov A = MEKFWCov::Zero();
448  A.block<3,3>(MEKF_WIND_qx,MEKF_WIND_qx) = Matrix3f::Identity();
449  A.block<3,3>(MEKF_WIND_qx,MEKF_WIND_rbp) = -Rqdt;
450  A.block<3,3>(MEKF_WIND_rbp,MEKF_WIND_rbp) = Matrix3f::Identity();
451 
452  Matrix<float, MEKF_WIND_COV_SIZE, MEKF_WIND_PROC_NOISE_SIZE> An;
453  An.setZero();
454  An.block<3,3>(MEKF_WIND_qx,MEKF_WIND_qgp) = Rq;
455  An.block<3,3>(MEKF_WIND_rbp,MEKF_WIND_qrbp) = Matrix3f::Identity(); // TODO check index
456 
457  MEKFWCov At(A);
458  At.transposeInPlace();
459  Matrix<float, MEKF_WIND_PROC_NOISE_SIZE, MEKF_WIND_COV_SIZE> Ant;
460  Ant = An.transpose();
461 
462  mwp.P = A * mwp.P * At + An * mwp.Q * Ant * dt;
463 
464  // correction from accel measurements
465  const Matrix3f Rqt = Rq.transpose();
466  Matrix<float, 3, MEKF_WIND_COV_SIZE> H = Matrix<float, 3, MEKF_WIND_COV_SIZE>::Zero();
467  H.block<3,3>(0,0) = - Rqt * skew_sym(gravity);
468  Matrix<float, MEKF_WIND_COV_SIZE, 3> Ht = H.transpose();
469  // S = H*P*Ht + Hn*N*Hnt
470  Matrix3f S = H * mwp.P * Ht + mwp.R.block<3,3>(MEKF_WIND_rmx,MEKF_WIND_rmx); // FIXME currently abusing mag noise ????
471  // K = P*Ht*S^-1
472  Matrix<float, MEKF_WIND_COV_SIZE, 3> K = mwp.P * Ht * S.inverse();
473  // Residual z_a - h(z)
474  Vector3f res = accel_unbiased + (Rqt * gravity);
475  // Update state
476  q_tmp.w() = 1.f;
477  q_tmp.vec() = 0.5f * K.block<3,3>(MEKF_WIND_qx,0) * res;
478  q_tmp.normalize();
479  mwp.state.quat = q_tmp * mwp.state.quat;
480  mwp.state.quat.normalize();
481  mwp.state.rates_bias += K.block<3,3>(MEKF_WIND_rbp,0) * res;
482  // Update covariance
483  mwp.P = (MEKFWCov::Identity() - K * H) * mwp.P;
484 }
485 
486 
487 void ins_mekf_wind_align(struct FloatRates *gyro_bias, struct FloatQuat *quat)
488 {
489  /* Compute an initial orientation from accel and mag directly as quaternion */
490  mwp.state.quat.w() = quat->qi;
491  mwp.state.quat.x() = quat->qx;
492  mwp.state.quat.y() = quat->qy;
493  mwp.state.quat.z() = quat->qz;
494 
495  /* use average gyro as initial value for bias */
496  mwp.state.rates_bias(0) = gyro_bias->p;
497  mwp.state.rates_bias(1) = gyro_bias->q;
498  mwp.state.rates_bias(2) = gyro_bias->r;
499 }
500 
501 void ins_mekf_wind_update_mag(struct FloatVect3* mag, bool attitude_only)
502 {
503  mwp.measurements.mag(0) = mag->x;
504  mwp.measurements.mag(1) = mag->y;
505  mwp.measurements.mag(2) = mag->z;
506 
507  // H and Ht matrices
508  const Matrix3f Rqt = mwp.state.quat.toRotationMatrix().transpose();
509  Matrix<float, 3, MEKF_WIND_COV_SIZE> H = Matrix<float, 3, MEKF_WIND_COV_SIZE>::Zero();
510  H.block<3,3>(0,0) = Rqt * skew_sym(mwp.mag_h);
511  Matrix<float, MEKF_WIND_COV_SIZE, 3> Ht = H.transpose();
512  // S = H*P*Ht + Hn*N*Hnt
513  Matrix3f S = H * mwp.P * Ht + mwp.R.block<3,3>(MEKF_WIND_rmx,MEKF_WIND_rmx);
514  // K = P*Ht*S^-1
515  Matrix<float, MEKF_WIND_COV_SIZE, 3> K = mwp.P * Ht * S.inverse();
516  // Residual z_m - h(z)
517  Vector3f res = mwp.measurements.mag - (Rqt * mwp.mag_h);
518  // Update state
519  Quaternionf q_tmp;
520  q_tmp.w() = 1.f;
521  q_tmp.vec() = 0.5f * K.block<3,3>(MEKF_WIND_qx,0) * res;
522  q_tmp.normalize();
523  mwp.state.quat = q_tmp * mwp.state.quat;
524  mwp.state.quat.normalize();
525  if (attitude_only) {
526  mwp.state.rates_bias += K.block<3,3>(MEKF_WIND_rbp,0) * res;
527  } else {
528  mwp.state.speed += K.block<3,3>(MEKF_WIND_vx,0) * res;
529  mwp.state.pos += K.block<3,3>(MEKF_WIND_px,0) * res;
530  mwp.state.rates_bias += K.block<3,3>(MEKF_WIND_rbp,0) * res;
531  mwp.state.accel_bias += K.block<3,3>(MEKF_WIND_abx,0) * res;
532  mwp.state.baro_bias += K.block<1,3>(MEKF_WIND_bb,0) * res;
534  mwp.state.wind += K.block<3,3>(MEKF_WIND_wx,0) * res;
535  }
536  }
537  // Update covariance
538  mwp.P = (MEKFWCov::Identity() - K * H) * mwp.P;
539 }
540 
542 {
543  mwp.measurements.baro_alt = baro_alt;
544 
545  // H and Ht matrices
546  Matrix<float, 1, MEKF_WIND_COV_SIZE> H = Matrix<float, 1, MEKF_WIND_COV_SIZE>::Zero();
547  H(0,MEKF_WIND_pz) = 1.0f; // TODO check index
548  H(0,MEKF_WIND_bb) = -1.0f;
549  Matrix<float, MEKF_WIND_COV_SIZE, 1> Ht = H.transpose();
550  // S = H*P*Ht + Hn*N*Hnt -> only pos.z component
552  // K = P*Ht*S^-1
553  Matrix<float, MEKF_WIND_COV_SIZE, 1> K = mwp.P * Ht / S;
554  // Residual z_m - h(z)
555  float res = mwp.measurements.baro_alt - (mwp.state.pos(2) - mwp.state.baro_bias);
556  // Update state
557  Quaternionf q_tmp;
558  q_tmp.w() = 1.f;
559  q_tmp.vec() = 0.5f * K.block<3,1>(MEKF_WIND_qx,0) * res;
560  q_tmp.normalize();
561  mwp.state.quat = q_tmp * mwp.state.quat;
562  mwp.state.quat.normalize();
563  mwp.state.speed += K.block<3,1>(MEKF_WIND_vx,0) * res;
564  mwp.state.pos += K.block<3,1>(MEKF_WIND_px,0) * res;
565  mwp.state.rates_bias += K.block<3,1>(MEKF_WIND_rbp,0) * res;
566  mwp.state.accel_bias += K.block<3,1>(MEKF_WIND_abx,0) * res;
567  mwp.state.baro_bias += K(MEKF_WIND_bb,0) * res;
569  mwp.state.wind += K.block<3,1>(MEKF_WIND_wx,0) * res;
570  }
571  // Update covariance
572  mwp.P = (MEKFWCov::Identity() - K * H) * mwp.P;
573 }
574 
575 void ins_mekf_wind_update_pos_speed(struct FloatVect3 *pos, struct FloatVect3 *speed)
576 {
577  mwp.measurements.pos(0) = pos->x;
578  mwp.measurements.pos(1) = pos->y;
579  mwp.measurements.pos(2) = pos->z;
580  mwp.measurements.speed(0) = speed->x;
581  mwp.measurements.speed(1) = speed->y;
582  mwp.measurements.speed(2) = speed->z;
583 
584  // H and Ht matrices
585  Matrix<float, 6, MEKF_WIND_COV_SIZE> H = Matrix<float, 6, MEKF_WIND_COV_SIZE>::Zero();
586  H.block<6,6>(0,MEKF_WIND_vx) = Matrix<float,6,6>::Identity();
587  Matrix<float, MEKF_WIND_COV_SIZE, 6> Ht = H.transpose();
588  // S = H*P*Ht + Hn*N*Hnt
589  Matrix<float, 6, 6> S = mwp.P.block<6,6>(MEKF_WIND_vx,MEKF_WIND_vx) + mwp.R.block<6,6>(MEKF_WIND_rvx,MEKF_WIND_rvx);
590  // K = P*Ht*S^-1
591  Matrix<float, MEKF_WIND_COV_SIZE, 6> K = mwp.P * Ht * S.inverse();
592  // Residual z_m - h(z)
593  Matrix<float, 6, 1> res = Matrix<float, 6, 1>::Zero();
594  res.block<3,1>(0,0) = mwp.measurements.speed - mwp.state.speed;
595  res.block<3,1>(3,0) = mwp.measurements.pos - mwp.state.pos;
596  // Update state
597  Quaternionf q_tmp;
598  q_tmp.w() = 1.f;
599  q_tmp.vec() = 0.5f * K.block<3,6>(MEKF_WIND_qx,0) * res;
600  q_tmp.normalize();
601  mwp.state.quat = q_tmp * mwp.state.quat;
602  mwp.state.speed += K.block<3,6>(MEKF_WIND_vx,0) * res;
603  mwp.state.pos += K.block<3,6>(MEKF_WIND_px,0) * res;
604  mwp.state.rates_bias += K.block<3,6>(MEKF_WIND_rbp,0) * res;
605  mwp.state.accel_bias += K.block<3,6>(MEKF_WIND_abx,0) * res;
606  mwp.state.baro_bias += K.block<1,6>(MEKF_WIND_bb,0) * res;
608  mwp.state.wind += K.block<3,6>(MEKF_WIND_wx,0) * res;
609  }
610  // Update covariance
611  mwp.P = (MEKFWCov::Identity() - K * H) * mwp.P;
612 }
613 
614 void ins_mekf_wind_update_airspeed(float airspeed)
615 {
616  mwp.measurements.airspeed = airspeed;
617 
619  // H and Ht matrices
620  const RowVector3f IuRqt = mwp.state.quat.toRotationMatrix().transpose().block<1,3>(0,0);
621  const Vector3f va = mwp.state.speed - mwp.state.wind;
622  Matrix<float, 1, MEKF_WIND_COV_SIZE> H = Matrix<float, 1, MEKF_WIND_COV_SIZE>::Zero();
623  H.block<1,3>(0,MEKF_WIND_qx) = IuRqt * skew_sym(va);
624  H.block<1,3>(0,MEKF_WIND_vx) = IuRqt;
625  H.block<1,3>(0,MEKF_WIND_wx) = -IuRqt;
626  Matrix<float, MEKF_WIND_COV_SIZE, 1> Ht = H.transpose();
627  // S = H*P*Ht + Hn*N*Hnt
628  float S = H * mwp.P * Ht + mwp.R(MEKF_WIND_ras,MEKF_WIND_ras);
629  // K = P*Ht*S^-1
630  Matrix<float, MEKF_WIND_COV_SIZE, 1> K = mwp.P * Ht / S;
631  // Residual z_m - h(z)
632  float res = mwp.measurements.airspeed - IuRqt * va;
633  // Update state
634  Quaternionf q_tmp;
635  q_tmp.w() = 1.f;
636  q_tmp.vec() = 0.5f * K.block<3,1>(MEKF_WIND_qx,0) * res;
637  q_tmp.normalize();
638  mwp.state.quat = q_tmp * mwp.state.quat;
639  mwp.state.quat.normalize();
640  mwp.state.speed += K.block<3,1>(MEKF_WIND_vx,0) * res;
641  mwp.state.pos += K.block<3,1>(MEKF_WIND_px,0) * res;
642  mwp.state.rates_bias += K.block<3,1>(MEKF_WIND_rbp,0) * res;
643  mwp.state.accel_bias += K.block<3,1>(MEKF_WIND_abx,0) * res;
644  mwp.state.baro_bias += K(MEKF_WIND_bb,0) * res;
646  mwp.state.wind += K.block<3,1>(MEKF_WIND_wx,0) * res;
647  }
648  // Update covariance
649  mwp.P = (MEKFWCov::Identity() - K * H) * mwp.P;
650 }
651 
652 void ins_mekf_wind_update_incidence(float aoa, float aos)
653 {
654  mwp.measurements.aoa = aoa;
655  mwp.measurements.aos = aos;
656 
658  // H and Ht matrices
659  const Matrix3f Rqt = mwp.state.quat.toRotationMatrix().transpose();
660  const Vector3f va = Rqt * (mwp.state.speed - mwp.state.wind); // airspeed in body frame
661  // check if data in valid range
662  const float van = va.norm();
663  //const float va_bound = 0.2f * van;
664  if (van < 5.f /*|| va(0) < 0.8f * van
665  || va(1) < - va_bound || va(1) > va_bound
666  || va(2) < - va_bound || va(2) > va_bound*/
667  || mwp.state.pos(2) > -10.f) {
668  // filter doesn't work at zero airspeed
669  return;
670  }
671  const RowVector3f C(sinf(aoa), 0.f, -cosf(aoa));
672  const RowVector3f CRqt = C * Rqt;
673  const float s_aos = sinf(aos);
674  const float c_aos = cosf(aos);
675  const Matrix3f B = Vector3f(s_aos * s_aos, - c_aos * c_aos, 0.f).asDiagonal();
676  const RowVector3f vBRqt = 2.f * va.transpose() * B * Rqt;
677  Matrix<float, 2, MEKF_WIND_COV_SIZE> H = Matrix<float, 2, MEKF_WIND_COV_SIZE>::Zero();
678  H.block<1,3>(0,MEKF_WIND_qx) = CRqt * skew_sym(mwp.state.speed - mwp.state.wind);
679  H.block<1,3>(0,MEKF_WIND_vx) = CRqt;
680  H.block<1,3>(0,MEKF_WIND_wx) = -CRqt;
681  H.block<1,3>(1,MEKF_WIND_qx) = vBRqt * skew_sym(mwp.state.speed - mwp.state.wind);
682  H.block<1,3>(1,MEKF_WIND_vx) = vBRqt;
683  H.block<1,3>(1,MEKF_WIND_wx) = -vBRqt;
684  Matrix<float, MEKF_WIND_COV_SIZE, 2> Ht = H.transpose();
685  // Hn and Hnt matrices
686  Matrix2f Hn = Matrix2f::Identity();
687  Hn(0,0) = C(2) * va(0) - C(0) * va(2);
688  const float s_2aos = sinf(2.0f * aos);
689  Hn(1,1) = (RowVector3f(-s_2aos, 0.f, s_2aos) * va.asDiagonal()) * va;
690  Matrix2f Hnt = Hn.transpose();
691  // S = H*P*Ht + Hn*N*Hnt
692  Matrix2f S = H * mwp.P * Ht + Hn * mwp.R.block<2,2>(MEKF_WIND_raoa,MEKF_WIND_raoa) * Hnt;
693  // K = P*Ht*S^-1
694  Matrix<float, MEKF_WIND_COV_SIZE, 2> K = mwp.P * Ht * S.inverse();
695  // Residual z_m - h(z)
696  Vector2f res = Vector2f::Zero();
697  res(0) = - C * va;
698  res(1) = - va.transpose() * B * va;
699  // Update state
700  Quaternionf q_tmp;
701  q_tmp.w() = 1.f;
702  q_tmp.vec() = 0.5f * K.block<3,2>(MEKF_WIND_qx,0) * res;
703  q_tmp.normalize();
704  mwp.state.quat = q_tmp * mwp.state.quat;
705  mwp.state.quat.normalize();
706  mwp.state.speed += K.block<3,2>(MEKF_WIND_vx,0) * res;
707  mwp.state.pos += K.block<3,2>(MEKF_WIND_px,0) * res;
708  mwp.state.rates_bias += K.block<3,2>(MEKF_WIND_rbp,0) * res;
709  mwp.state.accel_bias += K.block<3,2>(MEKF_WIND_abx,0) * res;
710  mwp.state.baro_bias += K.block<1,2>(MEKF_WIND_bb,0) * res;
712  mwp.state.wind += K.block<3,2>(MEKF_WIND_wx,0) * res;
713  }
714  // Update covariance
715  mwp.P = (MEKFWCov::Identity() - K * H) * mwp.P;
716 }
717 
722 {
723  const struct NedCoor_f p = {
724  .x = mwp.state.pos(0),
725  .y = mwp.state.pos(1),
726  .z = mwp.state.pos(2)
727  };
728  return p;
729 }
730 
732 {
733  mwp.state.pos(0) = p->x;
734  mwp.state.pos(1) = p->y;
735  mwp.state.pos(2) = p->z;
736 }
737 
739 {
740  const struct NedCoor_f s = {
741  .x = mwp.state.speed(0),
742  .y = mwp.state.speed(1),
743  .z = mwp.state.speed(2)
744  };
745  return s;
746 }
747 
749 {
750  mwp.state.speed(0) = s->x;
751  mwp.state.speed(1) = s->y;
752  mwp.state.speed(2) = s->z;
753 }
754 
756 {
757  const struct NedCoor_f a = {
758  .x = mwp.state.accel(0),
759  .y = mwp.state.accel(1),
760  .z = mwp.state.accel(2)
761  };
762  return a;
763 }
764 
766 {
767  const struct FloatQuat q = {
768  .qi = mwp.state.quat.w(),
769  .qx = mwp.state.quat.x(),
770  .qy = mwp.state.quat.y(),
771  .qz = mwp.state.quat.z()
772  };
773  return q;
774 }
775 
777 {
778  mwp.state.quat.w() = quat->qi;
779  mwp.state.quat.x() = quat->qx;
780  mwp.state.quat.y() = quat->qy;
781  mwp.state.quat.z() = quat->qz;
782 }
783 
785 {
786  const struct FloatRates r = {
787  .p = mwp.inputs.rates(0) - mwp.state.rates_bias(0),
788  .q = mwp.inputs.rates(1) - mwp.state.rates_bias(1),
789  .r = mwp.inputs.rates(2) - mwp.state.rates_bias(2)
790  };
791  return r;
792 }
793 
795 {
796  const struct NedCoor_f w = {
797  .x = mwp.state.wind(0),
798  .y = mwp.state.wind(1),
799  .z = mwp.state.wind(2)
800  };
801  return w;
802 }
803 
805 {
806  const Matrix3f Rqt = mwp.state.quat.toRotationMatrix().transpose();
807  const Vector3f va = Rqt * (mwp.state.speed - mwp.state.wind); // airspeed in body frame
808  const struct NedCoor_f a = {
809  .x = va(0),
810  .y = va(1),
811  .z = va(2)
812  };
813  return a;
814 }
815 
817 {
818  return (mwp.state.speed - mwp.state.wind).norm();
819 }
820 
822 {
823  const struct FloatVect3 ab = {
824  .x = mwp.state.accel_bias(0),
825  .y = mwp.state.accel_bias(1),
826  .z = mwp.state.accel_bias(2)
827  };
828  return ab;
829 }
830 
832 {
833  const struct FloatRates rb = {
834  .p = mwp.state.rates_bias(0),
835  .q = mwp.state.rates_bias(1),
836  .r = mwp.state.rates_bias(2)
837  };
838  return rb;
839 }
840 
842 {
843  return mwp.state.baro_bias;
844 }
845 
847 {
848  Matrix<float, MEKF_WIND_PROC_NOISE_SIZE, 1> vp;
855  mekf_wind_private.Q = vp.asDiagonal();
856 
857  Matrix<float, MEKF_WIND_MEAS_NOISE_SIZE, 1> vm;
867  mekf_wind_private.R = vm.asDiagonal();
868 }
869 
MEKF_WIND_wz
@ MEKF_WIND_wz
Definition: ins_mekf_wind.cpp:58
skew_sym
static Matrix3f skew_sym(const Vector3f &v)
build skew symetric matrix from vector m = [ 0, -v(2), v(1) ] [ v(2), 0, -v(0) ] [ -v(1),...
Definition: ins_mekf_wind.cpp:294
ins_mekf_wind_parameters::Q_baro_bias
float Q_baro_bias
baro bias process noise
Definition: ins_mekf_wind.h:47
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
InsMekfWindPrivate::P
MEKFWCov P
Definition: ins_mekf_wind.cpp:130
MekfWindState::accel_bias
Vector3f accel_bias
Definition: ins_mekf_wind.cpp:99
baro_alt
float baro_alt
Definition: baro_bmp280_i2c.c:46
MEKF_WIND_qx
@ MEKF_WIND_qx
Definition: ins_mekf_wind.cpp:52
ins_mekf_wind_parameters::R_aos
float R_aos
sideslip angle measurement noise
Definition: ins_mekf_wind.h:57
MEKF_WIND_PROC_NOISE_SIZE
@ MEKF_WIND_PROC_NOISE_SIZE
Definition: ins_mekf_wind.cpp:73
InsMekfWindPrivate::mag_h
Vector3f mag_h
Definition: ins_mekf_wind.cpp:135
NedCoor_f
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_float.h:63
INS_MEKF_WIND_R_POS_Z
#define INS_MEKF_WIND_R_POS_Z
Definition: ins_mekf_wind.cpp:193
MEKF_WIND_rvx
@ MEKF_WIND_rvx
Definition: ins_mekf_wind.cpp:81
MekfWindState::pos
Vector3f pos
Definition: ins_mekf_wind.cpp:96
MEKF_WIND_qgq
@ MEKF_WIND_qgq
Definition: ins_mekf_wind.cpp:67
MekfWindMeasurements::mag
Vector3f mag
Definition: ins_mekf_wind.cpp:116
ins_mekf_wind_init
void ins_mekf_wind_init(void)
Init function.
Definition: ins_mekf_wind.cpp:308
ins_mekf_wind_update_mag
void ins_mekf_wind_update_mag(struct FloatVect3 *mag, bool attitude_only)
Definition: ins_mekf_wind.cpp:501
MEKF_WIND_wx
@ MEKF_WIND_wx
Definition: ins_mekf_wind.cpp:58
INS_MEKF_WIND_Q_RATES_BIAS
#define INS_MEKF_WIND_Q_RATES_BIAS
Definition: ins_mekf_wind.cpp:170
MEKF_WIND_qy
@ MEKF_WIND_qy
Definition: ins_mekf_wind.cpp:52
ins_mekf_wind_parameters::R_baro
float R_baro
baro measurement noise
Definition: ins_mekf_wind.h:54
H
static struct FloatVect3 H
Definition: mag_calib_ukf.c:127
MekfWindMNoiseVar
MekfWindMNoiseVar
Measurement noise elements and size.
Definition: ins_mekf_wind.cpp:80
mekf_wind_private
static struct InsMekfWindPrivate mekf_wind_private
Definition: ins_mekf_wind.cpp:220
ins_mekf_wind_parameters::Q_wind
float Q_wind
wind process noise
Definition: ins_mekf_wind.h:48
ins_mekf_wind_parameters::R_aoa
float R_aoa
angle of attack measurement noise
Definition: ins_mekf_wind.h:56
MEKF_WIND_qwz
@ MEKF_WIND_qwz
Definition: ins_mekf_wind.cpp:72
ins_mekf_wind_get_accel_ned
struct NedCoor_f ins_mekf_wind_get_accel_ned(void)
Definition: ins_mekf_wind.cpp:755
MEKF_WIND_qaby
@ MEKF_WIND_qaby
Definition: ins_mekf_wind.cpp:70
ins_mekf_wind_set_speed_ned
void ins_mekf_wind_set_speed_ned(struct NedCoor_f *s)
Definition: ins_mekf_wind.cpp:748
MEKF_WIND_pz
@ MEKF_WIND_pz
Definition: ins_mekf_wind.cpp:54
MekfWindInputs
filter command vector
Definition: ins_mekf_wind.cpp:106
s
static uint32_t s
Definition: light_scheduler.c:33
ins_mekf_wind_parameters::R_pos
float R_pos
pos measurement noise
Definition: ins_mekf_wind.h:50
INS_MEKF_WIND_R_POS
#define INS_MEKF_WIND_R_POS
Definition: ins_mekf_wind.cpp:190
INS_MEKF_WIND_R_AOS
#define INS_MEKF_WIND_R_AOS
Definition: ins_mekf_wind.cpp:208
ins_mekf_wind_get_baro_bias
float ins_mekf_wind_get_baro_bias(void)
Definition: ins_mekf_wind.cpp:841
MekfWindMeasurements::speed
Vector3f speed
Definition: ins_mekf_wind.cpp:114
INS_MEKF_WIND_R_SPEED_Z
#define INS_MEKF_WIND_R_SPEED_Z
Definition: ins_mekf_wind.cpp:187
MEKF_WIND_qaz
@ MEKF_WIND_qaz
Definition: ins_mekf_wind.cpp:68
ins_mekf_wind_update_incidence
void ins_mekf_wind_update_incidence(float aoa, float aos)
Definition: ins_mekf_wind.cpp:652
MEKF_WIND_bb
@ MEKF_WIND_bb
Definition: ins_mekf_wind.cpp:57
ins_mekf_wind_params
struct ins_mekf_wind_parameters ins_mekf_wind_params
Definition: ins_mekf_wind.cpp:217
MEKF_WIND_qgr
@ MEKF_WIND_qgr
Definition: ins_mekf_wind.cpp:67
ins_mekf_wind_set_quat
void ins_mekf_wind_set_quat(struct FloatQuat *quat)
Definition: ins_mekf_wind.cpp:776
INS_MEKF_WIND_R_AOA
#define INS_MEKF_WIND_R_AOA
Definition: ins_mekf_wind.cpp:205
MEKF_WIND_qabz
@ MEKF_WIND_qabz
Definition: ins_mekf_wind.cpp:70
MEKF_WIND_py
@ MEKF_WIND_py
Definition: ins_mekf_wind.cpp:54
MEKF_WIND_rbr
@ MEKF_WIND_rbr
Definition: ins_mekf_wind.cpp:55
INS_MEKF_WIND_P0_WIND
#define INS_MEKF_WIND_P0_WIND
Definition: ins_mekf_wind.cpp:159
InsMekfWindPrivate::inputs
struct MekfWindInputs inputs
Definition: ins_mekf_wind.cpp:127
ins_mekf_wind_reset
void ins_mekf_wind_reset(void)
Definition: ins_mekf_wind.cpp:343
INS_MEKF_WIND_P0_POS
#define INS_MEKF_WIND_P0_POS
Definition: ins_mekf_wind.cpp:147
MekfWindState::rates_bias
Vector3f rates_bias
Definition: ins_mekf_wind.cpp:98
FloatVect3
Definition: pprz_algebra_float.h:54
MEKF_WIND_MEAS_NOISE_SIZE
@ MEKF_WIND_MEAS_NOISE_SIZE
Definition: ins_mekf_wind.cpp:86
MEKF_WIND_qbb
@ MEKF_WIND_qbb
Definition: ins_mekf_wind.cpp:71
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
INS_MEKF_WIND_R_MAG
#define INS_MEKF_WIND_R_MAG
Definition: ins_mekf_wind.cpp:196
INS_MEKF_WIND_Q_ACCEL_BIAS
#define INS_MEKF_WIND_Q_ACCEL_BIAS
Definition: ins_mekf_wind.cpp:173
ins_mekf_wind_get_wind_ned
struct NedCoor_f ins_mekf_wind_get_wind_ned(void)
Definition: ins_mekf_wind.cpp:794
MekfWindState::quat
Quaternionf quat
Definition: ins_mekf_wind.cpp:94
MekfWindState::wind
Vector3f wind
Definition: ins_mekf_wind.cpp:101
MEKF_WIND_qax
@ MEKF_WIND_qax
Definition: ins_mekf_wind.cpp:68
MEKF_WIND_COV_SIZE
@ MEKF_WIND_COV_SIZE
Definition: ins_mekf_wind.cpp:59
quat_smul
static Quaternionf quat_smul(const Quaternionf &q1, float scal)
Definition: ins_mekf_wind.cpp:284
MEKF_WIND_qz
@ MEKF_WIND_qz
Definition: ins_mekf_wind.cpp:52
MEKF_WIND_aby
@ MEKF_WIND_aby
Definition: ins_mekf_wind.cpp:56
ins_mekf_wind_parameters::R_speed
float R_speed
speed measurement noise
Definition: ins_mekf_wind.h:49
MEKF_WIND_rbp
@ MEKF_WIND_rbp
Definition: ins_mekf_wind.cpp:55
INS_MEKF_WIND_P0_SPEED
#define INS_MEKF_WIND_P0_SPEED
Definition: ins_mekf_wind.cpp:144
INS_MEKF_WIND_R_BARO
#define INS_MEKF_WIND_R_BARO
Definition: ins_mekf_wind.cpp:199
A
#define A
Definition: pprz_geodetic_utm.h:44
MEKF_WIND_raoa
@ MEKF_WIND_raoa
Definition: ins_mekf_wind.cpp:85
ins_mekf_wind_parameters::Q_accel
float Q_accel
accel process noise
Definition: ins_mekf_wind.h:44
MEKF_WIND_vx
@ MEKF_WIND_vx
Definition: ins_mekf_wind.cpp:53
ins_mekf_wind_parameters::R_airspeed
float R_airspeed
airspeed measurement noise
Definition: ins_mekf_wind.h:55
INS_MEKF_WIND_Q_GYRO
#define INS_MEKF_WIND_Q_GYRO
Definition: ins_mekf_wind.cpp:164
ins_mekf_wind_parameters::Q_gyro
float Q_gyro
gyro process noise
Definition: ins_mekf_wind.h:43
MEKF_WIND_qay
@ MEKF_WIND_qay
Definition: ins_mekf_wind.cpp:68
MEKF_WIND_rpz
@ MEKF_WIND_rpz
Definition: ins_mekf_wind.cpp:82
MekfWindMeasurements
filter measurement vector
Definition: ins_mekf_wind.cpp:113
ins_mekf_wind_parameters
Definition: ins_mekf_wind.h:42
ins_mekf_wind_align
void ins_mekf_wind_align(struct FloatRates *gyro_bias, struct FloatQuat *quat)
Definition: ins_mekf_wind.cpp:487
MEKF_WIND_rb
@ MEKF_WIND_rb
Definition: ins_mekf_wind.cpp:84
MEKF_WIND_vy
@ MEKF_WIND_vy
Definition: ins_mekf_wind.cpp:53
INS_MEKF_WIND_Q_WIND
#define INS_MEKF_WIND_Q_WIND
Definition: ins_mekf_wind.cpp:179
INS_MEKF_WIND_Q_BARO_BIAS
#define INS_MEKF_WIND_Q_BARO_BIAS
Definition: ins_mekf_wind.cpp:176
MEKF_WIND_qwx
@ MEKF_WIND_qwx
Definition: ins_mekf_wind.cpp:72
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
MekfWindState
filter state vector
Definition: ins_mekf_wind.cpp:93
ins_mekf_wind_parameters::R_speed_z
float R_speed_z
vertical speed measurement noise
Definition: ins_mekf_wind.h:51
ins_mekf_wind_update_pos_speed
void ins_mekf_wind_update_pos_speed(struct FloatVect3 *pos, struct FloatVect3 *speed)
Definition: ins_mekf_wind.cpp:575
ins_mekf_wind_parameters::Q_rates_bias
float Q_rates_bias
rates bias process noise
Definition: ins_mekf_wind.h:45
INS_MEKF_WIND_R_SPEED
#define INS_MEKF_WIND_R_SPEED
Definition: ins_mekf_wind.cpp:184
MekfWindMeasurements::aoa
float aoa
Definition: ins_mekf_wind.cpp:119
INS_MEKF_WIND_P0_BARO_BIAS
#define INS_MEKF_WIND_P0_BARO_BIAS
Definition: ins_mekf_wind.cpp:156
MEKF_WIND_rpx
@ MEKF_WIND_rpx
Definition: ins_mekf_wind.cpp:82
INS_MEKF_WIND_Q_ACCEL
#define INS_MEKF_WIND_Q_ACCEL
Definition: ins_mekf_wind.cpp:167
ins_mekf_wind_get_rates_bias
struct FloatRates ins_mekf_wind_get_rates_bias(void)
Definition: ins_mekf_wind.cpp:831
InsMekfWindPrivate::state
struct MekfWindState state
Definition: ins_mekf_wind.cpp:126
INS_MEKF_WIND_P0_QUAT
#define INS_MEKF_WIND_P0_QUAT
Definition: ins_mekf_wind.cpp:141
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
ins_mekf_wind_propagate_ahrs
void ins_mekf_wind_propagate_ahrs(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
AHRS-only propagation + accel correction.
Definition: ins_mekf_wind.cpp:422
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
MEKF_WIND_qrbr
@ MEKF_WIND_qrbr
Definition: ins_mekf_wind.cpp:69
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
INS_MEKF_WIND_P0_ACCEL_BIAS
#define INS_MEKF_WIND_P0_ACCEL_BIAS
Definition: ins_mekf_wind.cpp:153
MekfWindMeasurements::airspeed
float airspeed
Definition: ins_mekf_wind.cpp:118
MEKF_WIND_ras
@ MEKF_WIND_ras
Definition: ins_mekf_wind.cpp:85
B
#define B
Definition: ahrs_float_invariant.c:101
ins_mekf_wind_parameters::R_mag
float R_mag
mag measurement noise
Definition: ins_mekf_wind.h:53
InsMekfWindPrivate::R
MEKFWMNoise R
Definition: ins_mekf_wind.cpp:132
MekfWindState::accel
Vector3f accel
Definition: ins_mekf_wind.cpp:97
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
MEKF_WIND_rmy
@ MEKF_WIND_rmy
Definition: ins_mekf_wind.cpp:83
MEKF_WIND_rmx
@ MEKF_WIND_rmx
Definition: ins_mekf_wind.cpp:83
ins_mekf_wind.h
ins_mekf_wind_get_quat
struct FloatQuat ins_mekf_wind_get_quat(void)
Definition: ins_mekf_wind.cpp:765
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
MEKFWMNoise
Matrix< float, MEKF_WIND_MEAS_NOISE_SIZE, MEKF_WIND_MEAS_NOISE_SIZE > MEKFWMNoise
Definition: ins_mekf_wind.cpp:89
MEKF_WIND_raos
@ MEKF_WIND_raos
Definition: ins_mekf_wind.cpp:85
MEKF_WIND_px
@ MEKF_WIND_px
Definition: ins_mekf_wind.cpp:54
MEKF_WIND_qrbp
@ MEKF_WIND_qrbp
Definition: ins_mekf_wind.cpp:69
InsMekfWindPrivate::Q
MEKFWPNoise Q
Definition: ins_mekf_wind.cpp:131
MekfWindState::speed
Vector3f speed
Definition: ins_mekf_wind.cpp:95
MEKF_WIND_rmz
@ MEKF_WIND_rmz
Definition: ins_mekf_wind.cpp:83
MekfWindPNoiseVar
MekfWindPNoiseVar
Process noise elements and size.
Definition: ins_mekf_wind.cpp:66
MEKF_WIND_rbq
@ MEKF_WIND_rbq
Definition: ins_mekf_wind.cpp:55
MekfWindMeasurements::baro_alt
float baro_alt
Definition: ins_mekf_wind.cpp:117
MEKF_WIND_qabx
@ MEKF_WIND_qabx
Definition: ins_mekf_wind.cpp:70
K
static float K[9]
Definition: undistort_image.c:41
gravity
static const Vector3f gravity(0.f, 0.f, 9.81f)
MEKF_WIND_qrbq
@ MEKF_WIND_qrbq
Definition: ins_mekf_wind.cpp:69
MEKF_WIND_abx
@ MEKF_WIND_abx
Definition: ins_mekf_wind.cpp:56
MEKF_WIND_rpy
@ MEKF_WIND_rpy
Definition: ins_mekf_wind.cpp:82
init_mekf_state
static void init_mekf_state(void)
Definition: ins_mekf_wind.cpp:229
MEKF_WIND_rvz
@ MEKF_WIND_rvz
Definition: ins_mekf_wind.cpp:81
MEKF_WIND_wy
@ MEKF_WIND_wy
Definition: ins_mekf_wind.cpp:58
InsMekfWindPrivate::measurements
struct MekfWindMeasurements measurements
Definition: ins_mekf_wind.cpp:128
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
INS_MEKF_WIND_DISABLE_WIND
#define INS_MEKF_WIND_DISABLE_WIND
Definition: ins_mekf_wind.cpp:213
ins_mekf_wind_get_body_rates
struct FloatRates ins_mekf_wind_get_body_rates(void)
Definition: ins_mekf_wind.cpp:784
MekfWindMeasurements::pos
Vector3f pos
Definition: ins_mekf_wind.cpp:115
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
ins_mekf_wind_set_pos_ned
void ins_mekf_wind_set_pos_ned(struct NedCoor_f *p)
Definition: ins_mekf_wind.cpp:731
MekfWindInputs::rates
Vector3f rates
Definition: ins_mekf_wind.cpp:107
MEKF_WIND_rvy
@ MEKF_WIND_rvy
Definition: ins_mekf_wind.cpp:81
MEKF_WIND_qwy
@ MEKF_WIND_qwy
Definition: ins_mekf_wind.cpp:72
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
MekfWindInputs::accel
Vector3f accel
Definition: ins_mekf_wind.cpp:108
ins_mekf_wind_propagate
void ins_mekf_wind_propagate(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
Full INS propagation.
Definition: ins_mekf_wind.cpp:350
mwp
#define mwp
Definition: ins_mekf_wind.cpp:222
ins_mekf_wind_get_speed_ned
struct NedCoor_f ins_mekf_wind_get_speed_ned(void)
Definition: ins_mekf_wind.cpp:738
ins_mekf_wind_set_mag_h
void ins_mekf_wind_set_mag_h(const struct FloatVect3 *mag_h)
Definition: ins_mekf_wind.cpp:335
MEKFWPNoise
Matrix< float, MEKF_WIND_PROC_NOISE_SIZE, MEKF_WIND_PROC_NOISE_SIZE > MEKFWPNoise
Definition: ins_mekf_wind.cpp:76
ins_mekf_wind_get_airspeed_norm
float ins_mekf_wind_get_airspeed_norm(void)
Definition: ins_mekf_wind.cpp:816
ins_mekf_wind_get_airspeed_body
struct NedCoor_f ins_mekf_wind_get_airspeed_body(void)
Definition: ins_mekf_wind.cpp:804
ins_mekf_wind_get_pos_ned
struct NedCoor_f ins_mekf_wind_get_pos_ned(void)
Getter/Setter functions.
Definition: ins_mekf_wind.cpp:721
MekfWindCovVar
MekfWindCovVar
Covariance matrix elements and size.
Definition: ins_mekf_wind.cpp:51
ins_mekf_wind_update_baro
void ins_mekf_wind_update_baro(float baro_alt)
Definition: ins_mekf_wind.cpp:541
state
struct State state
Definition: state.c:36
ins_mekf_wind_update_params
void ins_mekf_wind_update_params(void)
Definition: ins_mekf_wind.cpp:846
MekfWindMeasurements::aos
float aos
Definition: ins_mekf_wind.cpp:120
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
MekfWindState::baro_bias
float baro_bias
Definition: ins_mekf_wind.cpp:100
quat_add
static Quaternionf quat_add(const Quaternionf &q1, const Quaternionf &q2)
Definition: ins_mekf_wind.cpp:280
MEKFWCov
Matrix< float, MEKF_WIND_COV_SIZE, MEKF_WIND_COV_SIZE > MEKFWCov
Definition: ins_mekf_wind.cpp:62
p
static float p[2][2]
Definition: ins_alt_float.c:257
InsMekfWindPrivate
private filter structure
Definition: ins_mekf_wind.cpp:125
MEKF_WIND_qgp
@ MEKF_WIND_qgp
Definition: ins_mekf_wind.cpp:67
ins_mekf_wind_parameters::R_pos_z
float R_pos_z
vertical pos measurement noise
Definition: ins_mekf_wind.h:52
ins_mekf_wind_parameters::disable_wind
bool disable_wind
disable wind estimation
Definition: ins_mekf_wind.h:58
MEKF_WIND_abz
@ MEKF_WIND_abz
Definition: ins_mekf_wind.cpp:56
ins_mekf_wind_get_accel_bias
struct FloatVect3 ins_mekf_wind_get_accel_bias(void)
Definition: ins_mekf_wind.cpp:821
ins_mekf_wind_parameters::Q_accel_bias
float Q_accel_bias
accel bias process noise
Definition: ins_mekf_wind.h:46
INS_MEKF_WIND_P0_RATES_BIAS
#define INS_MEKF_WIND_P0_RATES_BIAS
Definition: ins_mekf_wind.cpp:150
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
INS_MEKF_WIND_R_AIRSPEED
#define INS_MEKF_WIND_R_AIRSPEED
Definition: ins_mekf_wind.cpp:202
ins_mekf_wind_update_airspeed
void ins_mekf_wind_update_airspeed(float airspeed)
Definition: ins_mekf_wind.cpp:614
MEKF_WIND_vz
@ MEKF_WIND_vz
Definition: ins_mekf_wind.cpp:53