Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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();
413  mwp.P(MEKF_WIND_wx,MEKF_WIND_wx) = INS_MEKF_WIND_P0_WIND;
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 
541 void ins_mekf_wind_update_baro(float baro_alt)
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 
#define INS_MEKF_WIND_R_SPEED_Z
void ins_mekf_wind_init(void)
Init function.
float R_mag
mag measurement noise
Definition: ins_mekf_wind.h:53
static uint32_t s
float y
in meters
void ins_mekf_wind_set_speed_ned(struct NedCoor_f *s)
static float K[9]
void ins_mekf_wind_set_quat(struct FloatQuat *quat)
#define INS_MEKF_WIND_R_POS
void ins_mekf_wind_update_baro(float baro_alt)
#define INS_MEKF_WIND_Q_ACCEL_BIAS
#define INS_MEKF_WIND_P0_BARO_BIAS
void ins_mekf_wind_update_params(void)
float r
in rad/s
#define INS_MEKF_WIND_P0_QUAT
private filter structure
float R_pos
pos measurement noise
Definition: ins_mekf_wind.h:50
float R_speed
speed measurement noise
Definition: ins_mekf_wind.h:49
static struct InsMekfWindPrivate mekf_wind_private
struct MekfWindInputs inputs
void ins_mekf_wind_align(struct FloatRates *gyro_bias, struct FloatQuat *quat)
float q
in rad/s
float p
in rad/s
struct NedCoor_f ins_mekf_wind_get_speed_ned(void)
filter state vector
float R_speed_z
vertical speed measurement noise
Definition: ins_mekf_wind.h:51
struct NedCoor_f ins_mekf_wind_get_wind_ned(void)
void ins_mekf_wind_reset(void)
#define B
float R_baro
baro measurement noise
Definition: ins_mekf_wind.h:54
Multiplicative Extended Kalman Filter in rotation matrix formulation.
float Q_wind
wind process noise
Definition: ins_mekf_wind.h:48
#define INS_MEKF_WIND_Q_GYRO
void ins_mekf_wind_set_mag_h(const struct FloatVect3 *mag_h)
Vector3f accel_bias
float z
in meters
#define INS_MEKF_WIND_R_AOS
Roation quaternion.
float ins_mekf_wind_get_airspeed_norm(void)
float ins_mekf_wind_get_baro_bias(void)
struct MekfWindState state
struct FloatQuat ins_mekf_wind_get_quat(void)
float Q_baro_bias
baro bias process noise
Definition: ins_mekf_wind.h:47
Matrix< float, MEKF_WIND_PROC_NOISE_SIZE, MEKF_WIND_PROC_NOISE_SIZE > MEKFWPNoise
void ins_mekf_wind_update_pos_speed(struct FloatVect3 *pos, struct FloatVect3 *speed)
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)...
#define INS_MEKF_WIND_Q_WIND
float Q_accel
accel process noise
Definition: ins_mekf_wind.h:44
#define mwp
filter command vector
vector in North East Down coordinates Units: meters
#define INS_MEKF_WIND_R_AIRSPEED
float R_airspeed
airspeed measurement noise
Definition: ins_mekf_wind.h:55
#define INS_MEKF_WIND_Q_BARO_BIAS
float Q_rates_bias
rates bias process noise
Definition: ins_mekf_wind.h:45
struct ins_mekf_wind_parameters ins_mekf_wind_params
#define INS_MEKF_WIND_R_SPEED
static const Vector3f gravity(0.f, 0.f, 9.81f)
#define INS_MEKF_WIND_R_AOA
Quaternionf quat
static void init_mekf_state(void)
#define INS_MEKF_WIND_Q_ACCEL
MekfWindCovVar
Covariance matrix elements and size.
struct FloatRates ins_mekf_wind_get_rates_bias(void)
float R_aoa
angle of attack measurement noise
Definition: ins_mekf_wind.h:56
void ins_mekf_wind_set_pos_ned(struct NedCoor_f *p)
static Quaternionf quat_add(const Quaternionf &q1, const Quaternionf &q2)
static Quaternionf quat_smul(const Quaternionf &q1, float scal)
#define INS_MEKF_WIND_R_BARO
void ins_mekf_wind_propagate(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
Full INS propagation.
struct NedCoor_f ins_mekf_wind_get_accel_ned(void)
struct MekfWindMeasurements measurements
#define INS_MEKF_WIND_DISABLE_WIND
float R_aos
sideslip angle measurement noise
Definition: ins_mekf_wind.h:57
filter measurement vector
struct FloatRates ins_mekf_wind_get_body_rates(void)
Matrix< float, MEKF_WIND_COV_SIZE, MEKF_WIND_COV_SIZE > MEKFWCov
MekfWindMNoiseVar
Measurement noise elements and size.
#define INS_MEKF_WIND_R_POS_Z
MekfWindPNoiseVar
Process noise elements and size.
float Q_accel_bias
accel bias process noise
Definition: ins_mekf_wind.h:46
bool disable_wind
disable wind estimation
Definition: ins_mekf_wind.h:58
void ins_mekf_wind_update_airspeed(float airspeed)
static struct FloatVect3 H
#define INS_MEKF_WIND_P0_RATES_BIAS
#define INS_MEKF_WIND_P0_ACCEL_BIAS
#define INS_MEKF_WIND_P0_POS
static float p[2][2]
#define A
#define INS_MEKF_WIND_P0_WIND
float Q_gyro
gyro process noise
Definition: ins_mekf_wind.h:43
struct NedCoor_f ins_mekf_wind_get_airspeed_body(void)
#define INS_MEKF_WIND_Q_RATES_BIAS
#define INS_MEKF_WIND_R_MAG
void ins_mekf_wind_propagate_ahrs(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
AHRS-only propagation + accel correction.
void ins_mekf_wind_update_mag(struct FloatVect3 *mag, bool attitude_only)
Vector3f rates_bias
struct FloatVect3 ins_mekf_wind_get_accel_bias(void)
void ins_mekf_wind_update_incidence(float aoa, float aos)
angular rates
float R_pos_z
vertical pos measurement noise
Definition: ins_mekf_wind.h:52
struct State state
Definition: state.c:36
float x
in meters
#define INS_MEKF_WIND_P0_SPEED
Matrix< float, MEKF_WIND_MEAS_NOISE_SIZE, MEKF_WIND_MEAS_NOISE_SIZE > MEKFWMNoise
struct NedCoor_f ins_mekf_wind_get_pos_ned(void)
Getter/Setter functions.