Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ekf_aw.cpp
Go to the documentation of this file.
1 #include "modules/meteo/ekf_aw.h"
2 #include <iostream>
3 #include <stdio.h>
4 #include "std.h"
5 #include <math.h>
6 
7 #include "mcu_periph/sys_time.h" // FOR DEBUG
8 
9 #include <matrix/math.hpp>
10 
11 // Covariance matrix elements and size
17 };
18 
19 typedef matrix::SquareMatrix<float, EKF_AW_COV_SIZE> EKF_Aw_Cov;
20 
21 // Process noise elements and size
22 enum ekfAwQVar {
28 };
29 
30 typedef matrix::SquareMatrix<float, EKF_AW_Q_SIZE> EKF_Aw_Q;
31 
32 // Measurement noise elements and size
33 enum ekfAwRVar {
38 };
39 
40 typedef matrix::SquareMatrix<float, EKF_AW_R_SIZE> EKF_Aw_R;
41 
42 // filter state vector
43 struct ekfAwState {
44  matrix::Vector3f V_body;
45  matrix::Vector3f wind;
46  matrix::Vector3f offset;
47 };
48 
49 // filter command vector
50 struct ekfAwInputs {
51  matrix::Vector3f accel;
52  matrix::Vector3f rates;
53  matrix::Vector3f euler;
54  float RPM_pusher;
55  matrix::Vector<float, 4> RPM_hover;
56  float skew;
58 };
59 
60 // filter measurement vector
62  matrix::Vector3f V_gnd;
63  matrix::Vector3f accel_filt;
64  float V_pitot;
65 };
66 
67 // forces vector
68 struct ekfAwForces {
69  matrix::Vector3f fuselage;
70  matrix::Vector3f wing;
71  matrix::Vector3f elevator;
72  matrix::Vector3f hover;
73  matrix::Vector3f pusher;
74 };
75 
76 // private filter structure
77 struct ekfAwPrivate {
78  struct ekfAwState state;
79  struct ekfAwInputs inputs;
82  struct ekfAwForces forces;
83 
87 
88  struct ekfHealth health;
89 };
90 
91 // Parameters Process Noise
92 #ifndef EKF_AW_Q_ACCEL
93 #define EKF_AW_Q_ACCEL 1.0E-4f
94 #endif
95 #ifndef EKF_AW_Q_GYRO
96 #define EKF_AW_Q_GYRO 1.0E-09f
97 #endif
98 #ifndef EKF_AW_Q_MU
99 #define EKF_AW_Q_MU 1.0E-6f
100 #endif
101 #ifndef EKF_AW_Q_OFFSET
102 #define EKF_AW_Q_OFFSET 1.E-8f
103 #endif
104 
105 // Parameters Initial Covariance Matrix
106 #ifndef EKF_AW_P0_V_BODY
107 #define EKF_AW_P0_V_BODY 1.E-2f
108 #endif
109 #ifndef EKF_AW_P0_MU
110 #define EKF_AW_P0_MU EKF_AW_Q_MU*1.E1f
111 #endif
112 #ifndef EKF_AW_P0_OFFSET
113 #define EKF_AW_P0_OFFSET EKF_AW_Q_OFFSET
114 #endif
115 
116 // Parameters Measurement Noise
117 #ifndef EKF_AW_R_V_GND
118 #define EKF_AW_R_V_GND 1.E-5f
119 #endif
120 #ifndef EKF_AW_R_ACCEL_FILT_X
121 #define EKF_AW_R_ACCEL_FILT_X 1.E-5f
122 #endif
123 #ifndef EKF_AW_R_ACCEL_FILT_Y
124 #define EKF_AW_R_ACCEL_FILT_Y 1.E-5f
125 #endif
126 #ifndef EKF_AW_R_ACCEL_FILT_Z
127 #define EKF_AW_R_ACCEL_FILT_Z 1.E-5f
128 #endif
129 #ifndef EKF_AW_R_V_PITOT
130 #define EKF_AW_R_V_PITOT 1.E-7f
131 #endif
132 
133 // Other options
134 #ifndef EKF_AW_WING_INSTALLED
135 #define EKF_AW_WING_INSTALLED false
136 #endif
137 #ifndef EKF_AW_USE_MODEL_BASED_X
138 #define EKF_AW_USE_MODEL_BASED_X false
139 #endif
140 #ifndef EKF_AW_USE_MODEL_BASED_Y
141 #define EKF_AW_USE_MODEL_BASED_Y false
142 #endif
143 #ifndef EKF_AW_USE_MODEL_BASED_Z
144 #define EKF_AW_USE_MODEL_BASED_Z false
145 #endif
146 #ifndef EKF_AW_USE_BETA
147 #define EKF_AW_USE_BETA false
148 #endif
149 #ifndef EKF_AW_PROPAGATE_OFFSET
150 #define EKF_AW_PROPAGATE_OFFSET false
151 #endif
152 #ifndef EKF_AW_USE_PITOT
153 #define EKF_AW_USE_PITOT false
154 #endif
155 
156 // Model Based Parameters
157 #ifndef EKF_AW_VEHICLE_MASS
158 #define EKF_AW_VEHICLE_MASS 6.5f
159 #endif
160 
161 // Fx
162 #ifndef EKF_AW_K1_FX_DRAG
163 #define EKF_AW_K1_FX_DRAG -3.0E-1f
164 #endif
165 #ifndef EKF_AW_K2_FX_DRAG
166 #define EKF_AW_K2_FX_DRAG -4.0E-2f
167 #endif
168 
169 #ifndef EKF_AW_K1_FX_FUSELAGE
170 #define EKF_AW_K1_FX_FUSELAGE 0.0f
171 #endif
172 #ifndef EKF_AW_K2_FX_FUSELAGE
173 #define EKF_AW_K2_FX_FUSELAGE -0.04f
174 #endif
175 #ifndef EKF_AW_K3_FX_FUSELAGE
176 #define EKF_AW_K3_FX_FUSELAGE 0.0f
177 #endif
178 #ifndef EKF_AW_K4_FX_FUSELAGE
179 #define EKF_AW_K4_FX_FUSELAGE 0.0f
180 #endif
181 
182 #ifndef EKF_AW_K1_FX_HOVER
183 #define EKF_AW_K1_FX_HOVER 0.0f
184 #endif
185 #ifndef EKF_AW_K2_FX_HOVER
186 #define EKF_AW_K2_FX_HOVER 0.0f
187 #endif
188 #ifndef EKF_AW_K3_FX_HOVER
189 #define EKF_AW_K3_FX_HOVER -0.3f
190 #endif
191 
192 #ifndef EKF_AW_K1_FX_WING
193 #define EKF_AW_K1_FX_WING -6.428638953316000e-03f
194 #endif
195 #ifndef EKF_AW_K2_FX_WING
196 #define EKF_AW_K2_FX_WING 1.671952644901720e-01f
197 #endif
198 #ifndef EKF_AW_K3_FX_WING
199 #define EKF_AW_K3_FX_WING 5.944103706458780e-01f
200 #endif
201 #ifndef EKF_AW_K4_FX_WING
202 #define EKF_AW_K4_FX_WING 3.983889380919000e-03f
203 #endif
204 #ifndef EKF_AW_K5_FX_WING
205 #define EKF_AW_K5_FX_WING 3.532085496834000e-03f
206 #endif
207 
208 #ifndef EKF_AW_K1_FX_PUSH
209 #define EKF_AW_K1_FX_PUSH 3.96222948E-07f
210 #endif
211 #ifndef EKF_AW_K2_FX_PUSH
212 #define EKF_AW_K2_FX_PUSH -5.2930351318E-05f
213 #endif
214 #ifndef EKF_AW_K3_FX_PUSH
215 #define EKF_AW_K3_FX_PUSH -2.68843366027904E-01f
216 #endif
217 
218 #ifndef EKF_AW_K1_FX_ELEV
219 #define EKF_AW_K1_FX_ELEV 0.0f
220 #endif
221 #ifndef EKF_AW_K2_FX_ELEV
222 #define EKF_AW_K2_FX_ELEV 0.0f
223 #endif
224 #ifndef EKF_AW_K3_FX_ELEV
225 #define EKF_AW_K3_FX_ELEV 0.0f
226 #endif
227 
228 // Fy
229 #ifndef EKF_AW_K_FY_BETA
230 #define EKF_AW_K_FY_BETA -2.19E-1f
231 #endif
232 #ifndef EKF_AW_K_FY_V
233 #define EKF_AW_K_FY_V -3.2E-1f
234 #endif
235 #ifndef EKF_AW_K1_FY_WING
236 #define EKF_AW_K1_FY_WING 0.0f
237 #endif
238 #ifndef EKF_AW_K2_FY_WING
239 #define EKF_AW_K2_FY_WING 0.0f
240 #endif
241 #ifndef EKF_AW_K3_FY_WING
242 #define EKF_AW_K3_FY_WING 0.0f
243 #endif
244 #ifndef EKF_AW_K4_FY_WING
245 #define EKF_AW_K4_FY_WING 0.0f
246 #endif
247 #ifndef EKF_AW_K5_FY_WING
248 #define EKF_AW_K5_FY_WING 0.0f
249 #endif
250 
251 // Fz
252 #ifndef EKF_AW_K1_FZ_FUSELAGE
253 #define EKF_AW_K1_FZ_FUSELAGE 0.0f
254 #endif
255 #ifndef EKF_AW_K2_FZ_FUSELAGE
256 #define EKF_AW_K2_FZ_FUSELAGE 0.0f
257 #endif
258 #ifndef EKF_AW_K3_FZ_FUSELAGE
259 #define EKF_AW_K3_FZ_FUSELAGE 0.0f
260 #endif
261 #ifndef EKF_AW_K4_FZ_FUSELAGE
262 #define EKF_AW_K4_FZ_FUSELAGE 0.0f
263 #endif
264 
265 #ifndef EKF_AW_K1_FZ_WING
266 #define EKF_AW_K1_FZ_WING -1.000778727574050e-01f
267 #endif
268 #ifndef EKF_AW_K2_FZ_WING
269 #define EKF_AW_K2_FZ_WING -8.696479964371250e-01f
270 #endif
271 #ifndef EKF_AW_K3_FZ_WING
272 #define EKF_AW_K3_FZ_WING 1.457831456377660e-01f
273 #endif
274 #ifndef EKF_AW_K4_FZ_WING
275 #define EKF_AW_K4_FZ_WING 2.185394878246410e-01f
276 #endif
277 
278 #ifndef EKF_AW_K1_FZ_HOVER
279 #define EKF_AW_K1_FZ_HOVER -8.738705811080210e-07f
280 #endif
281 #ifndef EKF_AW_K2_FZ_HOVER
282 #define EKF_AW_K2_FZ_HOVER -9.517409386179890e-07f
283 #endif
284 #ifndef EKF_AW_K3_FZ_HOVER
285 #define EKF_AW_K3_FZ_HOVER -8.946217883362630e-07f
286 #endif
287 #ifndef EKF_AW_K4_FZ_HOVER
288 #define EKF_AW_K4_FZ_HOVER -8.520556416144729e-07f
289 #endif
290 #ifndef EKF_AW_K5_FZ_HOVER
291 #define EKF_AW_K5_FZ_HOVER 0.0f
292 #endif
293 
294 #ifndef EKF_AW_K1_FZ_ELEV
295 #define EKF_AW_K1_FZ_ELEV 0.0f
296 #endif
297 #ifndef EKF_AW_K2_FZ_ELEV
298 #define EKF_AW_K2_FZ_ELEV 0.0f
299 #endif
300 
301 // Covariance Schedule
302 #ifndef EKF_AW_AZ_SCHED_GAIN
303 #define EKF_AW_AZ_SCHED_GAIN 0
304 #endif
305 #ifndef EKF_AW_AZ_SCHED_START_DEG
306 #define EKF_AW_AZ_SCHED_START_DEG 60
307 #endif
308 #ifndef EKF_AW_AZ_SCHED_END_DEG
309 #define EKF_AW_AZ_SCHED_END_DEG 70
310 #endif
311 #ifndef EKF_AW_AX_SCHED_GAIN
312 #define EKF_AW_AX_SCHED_GAIN 0
313 #endif
314 #ifndef EKF_AW_AX_SCHED_START_DEG
315 #define EKF_AW_AX_SCHED_START_DEG 40
316 #endif
317 #ifndef EKF_AW_AX_SCHED_END_DEG
318 #define EKF_AW_AX_SCHED_END_DEG 60
319 #endif
320 
321 // Quick Convergence
322 #ifndef EKF_AW_AZ_QUICK_CONV_MU_GAIN
323 #define EKF_AW_AZ_QUICK_CONV_MU_GAIN -2
324 #endif
325 #ifndef EKF_AW_AZ_QUICK_CONV_ACCEL_GAIN
326 #define EKF_AW_AZ_QUICK_CONV_ACCEL_GAIN 0
327 #endif
328 
329 // Other
330 #ifndef EKF_AW_ELEV_MAX_ANGLE
331 #define EKF_AW_ELEV_MAX_ANGLE 37.0f
332 #endif
333 #ifndef EKF_AW_ELEV_MIN_ANGLE
334 #define EKF_AW_ELEV_MIN_ANGLE -10.0f
335 #endif
336 #ifndef EKF_AW_AOA_MAX_ANGLE
337 #define EKF_AW_AOA_MAX_ANGLE 15.0f
338 #endif
339 #ifndef EKF_AW_AOA_MIN_ANGLE
340 #define EKF_AW_AOA_MIN_ANGLE -15.0f
341 #endif
342 
343 // Measured skew value to real skew
344 #ifndef EKF_AW_SKEW_POLY_0
345 #define EKF_AW_SKEW_POLY_0 0.0f
346 #endif
347 #ifndef EKF_AW_SKEW_POLY_1
348 #define EKF_AW_SKEW_POLY_1 1.0f
349 #endif
350 #ifndef EKF_AW_SKEW_POLY_2
351 #define EKF_AW_SKEW_POLY_2 0.0f
352 #endif
353 
354 // FOR DEBUG
355 #ifndef EKF_AW_DEBUG
356 #define EKF_AW_DEBUG false
357 #endif
358 
359 /*
360 // TIMING INFO (for future optimization)
361 Part of code | Time to run (micro sec)
362 ------------------------------------------------------
363 Var declaration | 2
364 Trig fn | 3
365 Aoa, Beta | 5
366 F+L declaration | 2
367 P = F*P*F^T+L*Q*L^T | 3
368 Accel calc | 1
369 G | 2
370 S calculation G*P*G^T+R | 28
371 Kalman gain calc P*G^T*S^-1 | 48
372 State update | 7
373 Cov Update P=(I-K*G)*P | 36
374 */
375 
376 // Parameters
378 
379 // Internal structure
380 static struct ekfAwPrivate ekf_aw_private;
381 // Short name
382 #define eawp ekf_aw_private
383 
384 // Earth Gravity
385 static const matrix::Vector3f gravity(0.f, 0.f, 9.81f);
386 
387 // Constants
388 float deg2rad = M_PI / 180.0;
389 float rad2deg = 180.0 / M_PI;
390 
391 // Forces functions
392 float fx_fuselage(float *skew, float *aoa, float *u);
393 float fx_elevator(float *elevator_angle, float *V_a);
394 float fx_wing(float *skew, float *aoa, float *u);
395 float fy_wing(float *skew, float *aoa, float *u);
396 float fx_fy_hover(float *RPM_hover_mean, float *V);
397 float fx_pusher(float *RPM_pusher, float *u);
398 float fz_fuselage(float *skew, float *aoa, float *V_a);
399 float fz_elevator(float *elevator_angle, float *V_a);
400 float fz_wing(float *skew, float *aoa, float *V_a);
401 float fz_hover(matrix::Vector<float, 4> RPM_hover, float *V_a);
402 
403 /* init state and measurements */
404 static void init_ekf_aw_state(void)
405 {
406  // Init State
407  eawp.state.V_body.setZero();
408  eawp.state.wind.setZero();
409  eawp.state.offset.setZero();
410 
411  // Init Measures
412  eawp.measurements.V_gnd.setZero();
413  eawp.measurements.accel_filt.setZero();
414  eawp.measurements.V_pitot = 0.f;
415 
416  // Init Input
417  eawp.inputs.accel.setZero();
418  eawp.inputs.rates.setZero();
419  eawp.inputs.euler.setZero();
420  eawp.inputs.RPM_pusher = 0.f;
421  eawp.inputs.RPM_hover.setZero();
422  eawp.inputs.skew = 0.f;
423  eawp.inputs.elevator_angle = 0.f;
424 
425  // Init Innovation
426  eawp.innovations.V_gnd.setZero();
427  eawp.innovations.accel_filt.setZero();
428  eawp.innovations.V_pitot = 0.f;
429 
430  // Init Forces
431  eawp.forces.fuselage.setZero();
432  eawp.forces.wing.setZero();
433  eawp.forces.elevator.setZero();
434  eawp.forces.hover.setZero();
435  eawp.forces.pusher.setZero();
436 
437  // Init State Covariance
438  eawp.P.setZero();
448 
449  // Init Process and Measurements Noise Matrix
451 
452  // Init Filter Health
453  eawp.health.healthy = true;
454 }
455 
456 // Init function
457 void ekf_aw_init(void)
458 {
459  // Process noise
464 
465  // Measurement noise
467 
471 
473 
474  // Other options
476 
480 
482 
483  // Model based parameters
485  // X Axis
488 
496 
502 
506 
510 
511  // Y Axis
514 
520 
521  // Z Axis
526 
531 
537 
540 
541  // Init state and measurements
543 
544  // Init crashes number
545  eawp.health.crashes_n = 0;
546 
547  // Init quick convergence
549 
550 }
551 
553 {
554  // Update Process Noise Q Matrix
558 
562 
566 
570 
571  // Update Measurement Noise R Matrix
575 
579 
581 }
582 
583 // Reset function
584 void ekf_aw_reset(void)
585 {
586  // Only reset state, measurement and innovation
588 }
589 
590 // Full propagation
591 void ekf_aw_propagate(struct FloatVect3 *acc, struct FloatRates *gyro, struct FloatEulers *euler, float *pusher_RPM,
592  float *hover_RPM_array, float *skew, float *elevator_angle, FloatVect3 *V_gnd, FloatVect3 *acc_filt, float *V_pitot,
593  float dt)
594 {
595  /*
596  x = [u v w mu_x mu_y mu_z k_x k_y k_z];
597  u = [a_x a_y a_z p q r phi theta psi RPM_pusher RPM_hover skew elevator_angle];
598  z = [V_x V_y V_z a_x a_y a_z];
599  */
600 
601  // FOR DEBUG
602  uint32_t tic = get_sys_time_usec(); uint32_t duration_1; uint32_t duration_2; uint32_t duration_3; uint32_t duration_4;
603  uint32_t duration_5; uint32_t duration_6; uint32_t duration_7; uint32_t duration_8; uint32_t duration_9;
604  uint32_t duration_10; uint32_t duration_11;
605 
606  // Exit filter if the filter crashed for more than 5s
607  if (eawp.health.crashes_n > floor(5.0f / dt)) {
608  eawp.health.healthy = false;
609  return;
610  }
611 
613  // Preparing inputs //
615 
616  // Inputs
617  eawp.inputs.accel(0) = acc->x; eawp.inputs.accel(1) = acc->y; eawp.inputs.accel(2) = acc->z;
618  eawp.inputs.rates(0) = gyro->p; eawp.inputs.rates(1) = gyro->q; eawp.inputs.rates(2) = gyro->r;
619  eawp.inputs.euler(0) = euler->phi; eawp.inputs.euler(1) = euler->theta; eawp.inputs.euler(2) = euler->psi;
620 
621  eawp.inputs.RPM_pusher = *pusher_RPM;
622 
623  eawp.inputs.RPM_hover(0) = hover_RPM_array[0]; eawp.inputs.RPM_hover(1) = hover_RPM_array[1];
624  eawp.inputs.RPM_hover(2) = hover_RPM_array[2]; eawp.inputs.RPM_hover(3) = hover_RPM_array[3];
625  //std::cout << "Hover prop:\n" << eawp.inputs.RPM_hover << std::endl;
626 
627  eawp.inputs.skew = *skew;
628  // Polyval from measured skew to real skew // TO DO: complementary filter between SP (low pass) and measured skew (high pass)
629  eawp.inputs.skew = EKF_AW_SKEW_POLY_0 + EKF_AW_SKEW_POLY_1 * eawp.inputs.skew + EKF_AW_SKEW_POLY_2 * eawp.inputs.skew *
630  eawp.inputs.skew;
631 
632  Bound(eawp.inputs.skew, 0.0f, deg2rad * 90.0f); // Saturate 0-90 deg
633 
634  eawp.inputs.elevator_angle = *elevator_angle;
635  eawp.inputs.elevator_angle = eawp.inputs.elevator_angle < deg2rad * EKF_AW_ELEV_MIN_ANGLE ? deg2rad *
637  eawp.inputs.elevator_angle; // Saturate
638 
639  // Measurements
640  eawp.measurements.V_gnd(0) = V_gnd->x; eawp.measurements.V_gnd(1) = V_gnd->y; eawp.measurements.V_gnd(2) = V_gnd->z;
641  eawp.measurements.accel_filt(0) = acc_filt->x; eawp.measurements.accel_filt(1) = acc_filt->y;
642  eawp.measurements.accel_filt(2) = acc_filt->z;
643  eawp.measurements.V_pitot = *V_pitot;
644 
645  // Variables used in matrices and precomputed values
646  float V_a = eawp.state.V_body.norm(); //airspeed
647  float u = eawp.state.V_body(0);
648  float v = eawp.state.V_body(1);
649  float w = eawp.state.V_body(2);
650  float sign_u = u < 0.0f ? -1.0f : u > 0.0f ? 1.0f : 0.0f;
651  float sign_v = v < 0.0f ? -1.0f : v > 0.0f ? 1.0f : 0.0f;
652 
653  float p = eawp.inputs.rates(0);
654  float q = eawp.inputs.rates(1);
655  float r = eawp.inputs.rates(2);
656 
657  float phi = eawp.inputs.euler(0);
658  float theta = eawp.inputs.euler(1);
659  float psi = eawp.inputs.euler(2);
660 
661  // FOR DEBUG
662  if (EKF_AW_DEBUG) {
663  duration_1 = get_sys_time_usec() - tic;
664  }
665 
666  float cos_phi = cosf(phi);
667  float sin_phi = sinf(phi);
668  float cos_theta = cosf(theta);
669  float sin_theta = sinf(theta);
670  float cos_psi = cosf(psi);
671  float sin_psi = sinf(psi);
672 
673  float cos_skew = cosf(eawp.inputs.skew);
674  float sin_skew = sinf(eawp.inputs.skew);
675 
676  // FOR DEBUG
677 #if EKF_AW_DEBUG
678  duration_2 = get_sys_time_usec() - tic;
679 #endif
680 
681  // DCM from Euler Angles
682  matrix::Matrix3f dcm;
683  dcm(0, 0) = cos_theta * cos_psi;
684  dcm(0, 1) = -cos_phi * sin_psi + sin_phi * sin_theta * cos_psi;
685  dcm(0, 2) = sin_phi * sin_psi + cos_phi * sin_theta * cos_psi;
686 
687  dcm(1, 0) = cos_theta * sin_psi;
688  dcm(1, 1) = cos_phi * cos_psi + sin_phi * sin_theta * sin_psi;
689  dcm(1, 2) = -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi;
690 
691  dcm(2, 0) = -sin_theta;
692  dcm(2, 1) = sin_phi * cos_theta;
693  dcm(2, 2) = cos_phi * cos_theta;
694 
695  // Calculate Angle of Attack
696  float aoa = atan2f(w, fabsf(u) < 1.E-5 ? 1E-5 : u); // Protected alpha calculation
697  float sat_aoa;
698  Bound(sat_aoa, deg2rad * EKF_AW_AOA_MIN_ANGLE, deg2rad * EKF_AW_AOA_MAX_ANGLE); // Saturate alpha
699  float tan_aoa = tanf(aoa);
700 
701  // Derivative of Angle of Attack
702  float diff_alpha_u = 0.0f;
703  float diff_alpha_w = 0.0f;
704  if ((fabsf(u) > 1E-5) & (fabsf(w) > 1E-5)) {
705  diff_alpha_u = -w / (u * u + w * w);
706  diff_alpha_w = u / (u * u + w * w);
707  }
708 
709  // Calculate Sideslip
710  float beta = 0.0f;
711  if (V_a > 1E-5) {
712  beta = asinf(v / V_a < -1.0f ? -1.0f : v / V_a > 1.0f ? 1.0f : v / V_a); // Ratio is kept between -1 and 1
713  }
714 
715  float hover_RPM_mean = (eawp.inputs.RPM_hover(0) + eawp.inputs.RPM_hover(1) + eawp.inputs.RPM_hover(
716  2) + eawp.inputs.RPM_hover(3)) / 4.0f;
717 
718  // Verify vehicle mass is not 0
720 
721  // FOR DEBUG
722 #if EKF_AW_DEBUG
723  duration_3 = get_sys_time_usec() - tic;
724 #endif
725 
727  // Special Conditions //
729 
730  // A_z covariance Scheduling
731  /*
732  Cov
733 
734  10^Gain │ ────────
735  │ \
736  │ \
737  │ \
738  │ \
739  1 │ ────────
740 
741  └─────────┬───┬──────► Skew Angle
742  │ │
743  Start End
744 
745  */
746  float exp_az = 0.0f;
747  if (eawp.inputs.skew < deg2rad * EKF_AW_AZ_SCHED_START_DEG) {
748  exp_az = EKF_AW_AZ_SCHED_GAIN;
749  } else if (eawp.inputs.skew > deg2rad * EKF_AW_AZ_SCHED_END_DEG) {
750  exp_az = 0.0f;
751  } else {
752  // Make sure Start and End points are not the same
756  }
757  }
758  Bound(exp_az, 0, EKF_AW_AZ_SCHED_GAIN); // Saturate
760 
761  // A_x covariance Scheduling
762  /*
763  Cov
764 
765  10^Gain │ /────────
766  │ /
767  │ /
768  │ /
769  │ /
770  1 │ ────────/
771 
772  └─────────┬───┬──────► Skew Angle
773  │ │
774  Start End
775 
776  */
777  float exp_ax = 0.0f;
778  if (eawp.inputs.skew < deg2rad * EKF_AW_AX_SCHED_START_DEG) {
779  exp_ax = 0.0f;
780  } else if (eawp.inputs.skew > deg2rad * EKF_AW_AX_SCHED_END_DEG) {
781  exp_ax = EKF_AW_AX_SCHED_GAIN;
782  } else {
783  // Make sure Start and End points are not the same
785  exp_ax = (eawp.inputs.skew - deg2rad * EKF_AW_AX_SCHED_START_DEG) * ((EKF_AW_AX_SCHED_GAIN) / (deg2rad *
787  }
788  }
789  Bound(exp_ax, 0, EKF_AW_AX_SCHED_GAIN); // Saturate
791 
792  // Quick Convergence Mode
793  // Increases wind covariance and decreases accel covariance
797 
804  } else {
807 
811  }
812 
814  // Propagate States //
816 
817  // Propagate state by Euler Integration
818  matrix::Vector3f state_dev = -eawp.inputs.rates.cross(eawp.state.V_body) + dcm.transpose() * gravity +
819  eawp.inputs.accel; // Verified and compared to Matlab output
820 
821  // Check if state derivative is a NAN and only propagate if its not
822  bool anyNan = false;
823  for (int i = 0; i < 3; i++) {
824  if (std::isnan(state_dev(i))) {
825  anyNan = true;
826  break;
827  }
828  }
829 
830  if (anyNan) {
831  eawp.health.healthy = false;
832  eawp.health.crashes_n += 1;
833  } else {
834  eawp.state.V_body += state_dev * dt;
835  }
836 
838  // Propagate Covariance //
840 
841  // FOR DEBUG
842 #if EKF_AW_DEBUG
843  duration_4 = get_sys_time_usec() - tic;
844 #endif
845 
846  // This is an optimized version using code generation from Matlab, as L and F are sparse matrix
847  // Goes from approx 4000 operation to 120 operations
848  eawp.P.setZero();
849  eawp.P(0, 0) = eawp.Q(0, 0) + q * (q * eawp.P(2, 2) - r * eawp.P(1, 2)) - r * (q * eawp.P(2, 1) - r * eawp.P(1,
850  1)) + v * v * eawp.Q(5, 5) + w * w * eawp.Q(4, 4);
851  eawp.P(0, 1) = r * (q * eawp.P(2, 0) - r * eawp.P(1, 0)) - p * (q * eawp.P(2, 2) - r * eawp.P(1, 2)) - u * v * eawp.Q(5,
852  5);
853  eawp.P(0, 2) = p * (q * eawp.P(2, 1) - r * eawp.P(1, 1)) - q * (q * eawp.P(2, 0) - r * eawp.P(1, 0)) - u * w * eawp.Q(4,
854  4);
855  eawp.P(1, 0) = r * (p * eawp.P(2, 1) - r * eawp.P(0, 1)) - q * (p * eawp.P(2, 2) - r * eawp.P(0, 2)) - u * v * eawp.Q(5,
856  5);
857  eawp.P(1, 1) = eawp.Q(1, 1) + p * (p * eawp.P(2, 2) - r * eawp.P(0, 2)) - r * (p * eawp.P(2, 0) - r * eawp.P(0,
858  0)) + u * u * eawp.Q(5, 5) + w * w * eawp.Q(3, 3);
859  eawp.P(1, 2) = q * (p * eawp.P(2, 0) - r * eawp.P(0, 0)) - p * (p * eawp.P(2, 1) - r * eawp.P(0, 1)) - v * w * eawp.Q(3,
860  3);
861  eawp.P(2, 0) = q * (p * eawp.P(1, 2) - q * eawp.P(0, 2)) - r * (p * eawp.P(1, 1) - q * eawp.P(0, 1)) - u * w * eawp.Q(4,
862  4);
863  eawp.P(2, 1) = r * (p * eawp.P(1, 0) - q * eawp.P(0, 0)) - p * (p * eawp.P(1, 2) - q * eawp.P(0, 2)) - v * w * eawp.Q(3,
864  3);
865  eawp.P(2, 2) = eawp.Q(2, 2) + p * (p * eawp.P(1, 1) - q * eawp.P(0, 1)) - q * (p * eawp.P(1, 0) - q * eawp.P(0,
866  0)) + u * u * eawp.Q(4, 4) + v * v * eawp.Q(3, 3);
867  eawp.P(3, 3) = eawp.Q(6, 6);
868  eawp.P(4, 4) = eawp.Q(7, 7);
869  eawp.P(5, 5) = eawp.Q(8, 8);
870  eawp.P(6, 6) = eawp.Q(9, 9);
871  eawp.P(7, 7) = eawp.Q(10, 10);
872  eawp.P(8, 8) = eawp.Q(11, 11);
873 
874  // Other way of calculating covariance, but involves more operations:
875  /*
876  // F Matrix
877  //F = [ dx_1/dx_1 dx_1/dx_2 ... dx_1/dx_n ;
878  // dx_2/dx_1 dx_2/dx_2 ....dx_2/dx_n ;
879  // ... ;
880  // dx_n/dx_1 dx_n/dx_2 ....dx_n/dx_n ];
881  //
882  //Validated and compared to Matlab output for F
883  //Generated with Matlab script
884 
885  EKF_Aw_Cov F;
886  F.setZero();
887  F(0,1) = r;
888  F(0,2) = -q;
889  F(1,0) = -r;
890  F(1,2) = p;
891  F(2,0) = q;
892  F(2,1) = -p;
893 
894  // L Matrix
895  //
896  //L = [ dx_1/dw_1 dx_1/dw_2 ... dx_1/dw_m ;
897  // dx_2/dw_1 dx_2/dw_2 ....dx_2/dw_m ;
898  // ... ;
899  // dx_n/dw_1 dx_n/dw_2 ....dx_n/dw_m ];
900  //
901  //Validated and compared to Matlab output for L
902  //Generated with Matlab script
903 
904  matrix::Matrix<float, EKF_AW_COV_SIZE, EKF_AW_Q_SIZE> L;
905  L.setZero();
906  L(0,0) =1;
907  L(0,4) =-w;
908  L(0,5) =v;
909  L(1,1) =1;
910  L(1,3) =w;
911  L(1,5) =-u;
912  L(2,2) =1;
913  L(2,3) =-v;
914  L(2,4) =u;
915  L(3,6) =1;
916  L(4,7) =1;
917  L(5,8) =1;
918  L(6,9) =1;
919  L(7,10) =1;
920  L(8,11) =1;
921 
922  eawp.P = F * eawp.P * F.transpose() + L * eawp.Q * L.transpose();
923  */
924 
925  // FOR DEBUG
926 #if EKF_AW_DEBUG
927  duration_5 = get_sys_time_usec() - tic;
928 #endif
930  // Measurement estimation from state //
932 
933  // Calculate acceleration estimation from filter states
934  float a_x;
935  float a_y;
936  float a_z;
937 
938  // A_x
939  if (ekf_aw_params.use_model[0]) {
940  // Calculate forces in x axis
941  eawp.forces.fuselage(0) = fx_fuselage(&eawp.inputs.skew, &sat_aoa, &u);
942  eawp.forces.wing(0) = EKF_AW_WING_INSTALLED ? fx_wing(&eawp.inputs.skew, &sat_aoa, &u) : 0;
943  eawp.forces.elevator(0) = fx_elevator(&eawp.inputs.elevator_angle, &V_a);
944  eawp.forces.hover(0) = fx_fy_hover(&hover_RPM_mean, &u);
945  eawp.forces.pusher(0) = fx_pusher(&eawp.inputs.RPM_pusher, &u);
946 
947  // Acceleration is sum of forces
948  a_x = (eawp.forces.fuselage(0) +
949  eawp.forces.wing(0) +
950  eawp.forces.elevator(0) +
951  eawp.forces.hover(0) +
952  eawp.forces.pusher(0) +
953  eawp.state.offset(0) * u * u * sign_u) / ekf_aw_params.vehicle_mass;
954  } else {
955  a_x = (ekf_aw_params.k_fx_drag[0] * u +
956  ekf_aw_params.k_fx_drag[1] * u * u * sign_u +
957  eawp.state.offset(0) * u * u * sign_u) / ekf_aw_params.vehicle_mass;
958 
959  // Pusher prop
960  eawp.forces.pusher(0) = fx_pusher(&eawp.inputs.RPM_pusher, &u);
961  a_x += eawp.forces.pusher(0) / ekf_aw_params.vehicle_mass;
962  }
963 
964  // A_y
965 #if EKF_AW_USE_BETA
966  a_y = beta * ekf_aw_params.k_fy_beta * (V_a * V_a) / ekf_aw_params.vehicle_mass + eawp.state.offset(
967  1); // No need to bound beta equation as beta is generated by asin, which is bounded between -pi/2 and pi/2
968 #else
969  a_y = sign_v * v * v * ekf_aw_params.k_fy_v / ekf_aw_params.vehicle_mass + eawp.state.offset(1);
970 #endif
971  if (ekf_aw_params.use_model[1]) {
972  // Wing sideforce
973  eawp.forces.wing(1) = EKF_AW_WING_INSTALLED ? fy_wing(&eawp.inputs.skew, &sat_aoa, &u) : 0;
974  a_y += eawp.forces.wing(1) / ekf_aw_params.vehicle_mass;
975  }
976 
977  // A_z
978  if (ekf_aw_params.use_model[2]) {
979  // Calculate forces in x axis
980  eawp.forces.fuselage(2) = fz_fuselage(&eawp.inputs.skew, &sat_aoa, &V_a);
981  eawp.forces.elevator(2) = fz_elevator(&eawp.inputs.elevator_angle, &V_a);
982  eawp.forces.wing(2) = EKF_AW_WING_INSTALLED ? fz_wing(&eawp.inputs.skew, &sat_aoa, &V_a) : 0;
983  eawp.forces.hover(2) = fz_hover(eawp.inputs.RPM_hover, &V_a);
984 
985  // Acceleration is sum of forces
986  a_z = (eawp.forces.fuselage(2) +
987  eawp.forces.elevator(2) +
988  eawp.forces.wing(2) +
989  eawp.forces.hover(2)) / ekf_aw_params.vehicle_mass + eawp.state.offset(1);
990  } else {
991  a_z = eawp.measurements.accel_filt(2);
992  }
993 
994  matrix::Vector3f accel_est = {a_x, a_y, a_z};
995 
996  // Innovation
997  eawp.innovations.V_gnd = eawp.measurements.V_gnd - (dcm * eawp.state.V_body +
998  eawp.state.wind); // Ground speed in NED Frame
999  eawp.innovations.accel_filt = eawp.measurements.accel_filt - accel_est;
1000  eawp.innovations.V_pitot = eawp.measurements.V_pitot - eawp.state.V_body(0);
1001 
1002  //std::cout << "State wind:\n" << eawp.state.wind << std::endl;
1003  //std::cout << "V_body:\n" << eawp.state.V_body << std::endl;
1004  //std::cout << "Q :\n" << eawp.Q(EKF_AW_Q_k_x_index,EKF_AW_Q_k_x_index) << std::endl;
1005  //std::cout << "V_body_gnd:\n" << quat.toRotationMatrix() * eawp.state.V_body << std::endl;
1006  //std::cout << "V_gnd:\n" << eawp.measurements.V_gnd << std::endl;
1007  //std::cout << "Innov V_gnd:\n" << eawp.innovations.V_gnd << std::endl;
1008  //std::cout << "Innov accel_filt:\n" << eawp.innovations.accel_filt << std::endl;
1009  //std::cout << "Euler:\n" << eawp.inputs.euler << std::endl;
1010 
1011  // FOR DEBUG
1012 #if EKF_AW_DEBUG
1013  duration_6 = get_sys_time_usec() - tic;
1014 #endif
1015 
1017  // State Update //
1019 
1020  // G Matrix Calculation
1021  /*
1022  G = [ dg_1/dx_1 dg_1/dx_2 ... dg_1/dx_m ;
1023  dg_2/dx_1 dg_2/dx_2 ....dg_2/dx_m ;
1024  ... ;
1025  dg_n/dx_1 dg_n/dx_2 ....dg_n/dx_m ];
1026 
1027  Generated with Matlab script
1028  */
1029  matrix::Matrix<float, EKF_AW_R_SIZE, EKF_AW_COV_SIZE> G;
1030  G.setZero();
1031  // V_gnd related lines (verified and compared to Matlab output)
1032  //Generated with Matlab script
1033  G(0, 0) = cos_psi * cos_theta;
1034  G(0, 1) = cos_psi * sin_phi * sin_theta - cos_phi * sin_psi;
1035  G(0, 2) = sin_phi * sin_psi + cos_phi * cos_psi * sin_theta;
1036  G(0, 3) = 1;
1037  G(1, 0) = cos_theta * sin_psi;
1038  G(1, 1) = cos_phi * cos_psi + sin_phi * sin_psi * sin_theta;
1039  G(1, 2) = cos_phi * sin_psi * sin_theta - cos_psi * sin_phi;
1040  G(1, 4) = 1;
1041  G(2, 0) = -sin_theta;
1042  G(2, 1) = cos_theta * sin_phi;
1043  G(2, 2) = cos_phi * cos_theta;
1044  G(2, 5) = 1;
1045 
1046  // A_x_filt related lines
1047  if (ekf_aw_params.use_model[0]) {
1048  // Validated and compared to Matlab output for G
1049  //Generated with Matlab script
1050 
1051  // Pusher contribution
1052  G(3, 0) += (ekf_aw_params.k_fx_push[2] + eawp.inputs.RPM_pusher * ekf_aw_params.k_fx_push[1]) /
1054 
1055  // Fuselage contribution
1056  float temp_1 = ekf_aw_params.k_fx_fuselage[1] + ekf_aw_params.k_fx_fuselage[3] * aoa * aoa +
1057  ekf_aw_params.k_fx_fuselage[0] * cos_skew + ekf_aw_params.k_fx_fuselage[2] * aoa;
1058  G(3, 0) += (2 * u * (temp_1) + (ekf_aw_params.k_fx_fuselage[2] * diff_alpha_u + 2 * ekf_aw_params.k_fx_fuselage[3] * aoa
1059  * diff_alpha_u) * V_a * V_a) / ekf_aw_params.vehicle_mass;
1060  G(3, 1) += (2 * v * (temp_1) / ekf_aw_params.vehicle_mass);
1061  G(3, 2) += (2 * w * (temp_1) + (ekf_aw_params.k_fx_fuselage[2] * diff_alpha_w + 2 * ekf_aw_params.k_fx_fuselage[3] * aoa
1062  * diff_alpha_w) * V_a * V_a) / ekf_aw_params.vehicle_mass;
1063 
1064  // Hover prop contribution
1065  G(3, 0) += (2 * ekf_aw_params.k_fx_hover[0] * sign_u * u + (0.5 * ekf_aw_params.k_fx_hover[1] * sign_u * hover_RPM_mean
1066  * hover_RPM_mean) / sqrtf(fabsf(fabsf(u) < 1E-5 ? 1E-5 : u)) + ekf_aw_params.k_fx_hover[2] * sign_u) /
1067  ekf_aw_params.vehicle_mass; // TO DO: Verify the abs()
1068 
1070  // Wing contribution
1071  float temp_2 = ekf_aw_params.k_fx_wing[0] + ekf_aw_params.k_fx_wing[4] * eawp.inputs.skew +
1072  (sin_skew * sin_skew + ekf_aw_params.k_fx_wing[3]) * (ekf_aw_params.k_fx_wing[2] * aoa * aoa +
1073  ekf_aw_params.k_fx_wing[1] * aoa);
1074  float temp_3 = sin_skew * sin_skew + ekf_aw_params.k_fx_wing[3];
1075  G(3, 0) += (2 * u * temp_2 + temp_3 * (ekf_aw_params.k_fx_wing[1] * diff_alpha_u + 2 * ekf_aw_params.k_fx_wing[2] * aoa
1076  * diff_alpha_u) * V_a * V_a) / ekf_aw_params.vehicle_mass;
1077  G(3, 1) += (2 * v * temp_2) / ekf_aw_params.vehicle_mass;
1078  G(3, 2) += (2 * w * temp_2 + temp_3 * (ekf_aw_params.k_fx_wing[1] * diff_alpha_w + 2 * ekf_aw_params.k_fx_wing[2] * aoa
1079  * diff_alpha_w) * V_a * V_a) / ekf_aw_params.vehicle_mass;
1080 #endif
1081  // Elevator contribution
1082  float temp_4 = ekf_aw_params.k_fx_elev[2] * eawp.inputs.elevator_angle * eawp.inputs.elevator_angle +
1083  ekf_aw_params.k_fx_elev[1] * eawp.inputs.elevator_angle + ekf_aw_params.k_fx_elev[0];
1084  G(3, 0) += (2 * u * temp_4) / ekf_aw_params.vehicle_mass;
1085  G(3, 1) += (2 * v * temp_4) / ekf_aw_params.vehicle_mass;
1086  G(3, 2) += (2 * w * temp_4) / ekf_aw_params.vehicle_mass;
1087 
1088  } else {
1089  G(3, 0) = (ekf_aw_params.k_fx_drag[0] + 2 * ekf_aw_params.k_fx_drag[1] * u * sign_u) /
1090  ekf_aw_params.vehicle_mass; // Simplified version (using u instead of V_a)
1091 
1092  // Pusher contribution
1093  G(3, 0) += (ekf_aw_params.k_fx_push[2] + eawp.inputs.RPM_pusher * ekf_aw_params.k_fx_push[1]) /
1095  }
1096  // Offset contribution
1097  G(3, 0) += (2 * eawp.state.offset(0) * sign_u * u) / ekf_aw_params.vehicle_mass;
1098  G(3, 6) = (u * u * sign_u) / ekf_aw_params.vehicle_mass;
1099 
1100  // A_y_filt related lines
1101 #if EKF_AW_USE_BETA
1102  float protected_V_a = fabsf(V_a) < 1E-5 ? 1E-5 : V_a;
1103  float cos_beta = fabsf(cosf(beta)) < 1E-5 ? 1E-5 : cosf(beta);
1104  G(4, 0) = 2 * ekf_aw_params.k_fy_beta * u * beta - (ekf_aw_params.k_fy_beta * u * v) / (cos_beta * protected_V_a);
1105  G(4, 1) = 2 * ekf_aw_params.k_fy_beta * v * beta + (ekf_aw_params.k_fy_beta * (V_a - v * sinf(beta))) / cos_beta;
1106  G(4, 2) = 2 * ekf_aw_params.k_fy_beta * w * beta - (ekf_aw_params.k_fy_beta * v * w) / (cos_beta * protected_V_a);
1107 #else
1108  G(4, 1) = 2 * ekf_aw_params.k_fy_v * v * sign_v;
1109 #endif
1110 
1111 #if EKF_AW_WING_INSTALLED
1112  G(4, 0) += (ekf_aw_params.k_fy_wing[0] * u * sinf(2 * eawp.inputs.skew)) / ekf_aw_params.vehicle_mass;
1113 #endif
1114 
1115  G(4, 7) = 1;
1116 
1117  // A_z_filt related lines
1118  if (ekf_aw_params.use_model[2]) {
1119  // Validated and compared to Matlab output for G
1120  //Generated with Matlab script
1121 
1122  // Fuselage contribution
1123  float temp_5 = ekf_aw_params.k_fz_fuselage[1] + ekf_aw_params.k_fz_fuselage[3] * aoa * aoa +
1124  ekf_aw_params.k_fz_fuselage[0] * cos_skew + ekf_aw_params.k_fz_fuselage[2] * aoa;
1125  G(5, 0) += (2 * u * temp_5 + (ekf_aw_params.k_fz_fuselage[2] * diff_alpha_u + 2 * ekf_aw_params.k_fz_fuselage[3] * aoa *
1126  diff_alpha_u) * V_a * V_a) / ekf_aw_params.vehicle_mass;
1127  G(5, 1) += (2 * v * temp_5) / ekf_aw_params.vehicle_mass;
1128  G(5, 2) += (2 * w * temp_5 + (ekf_aw_params.k_fz_fuselage[2] * diff_alpha_w + 2 * ekf_aw_params.k_fz_fuselage[3] * aoa *
1129  diff_alpha_w) * V_a * V_a) / ekf_aw_params.vehicle_mass;
1130 
1131 #if EKF_AW_WING_INSTALLED
1132  // Wing contribution
1133  G(5, 0) += (2 * u * (sin_skew * sin_skew + ekf_aw_params.k_fz_wing[3]) * (ekf_aw_params.k_fz_wing[0] +
1134  ekf_aw_params.k_fz_wing[2] * aoa * aoa + ekf_aw_params.k_fz_wing[1] * aoa) - (sin_skew * sin_skew +
1135  ekf_aw_params.k_fz_wing[3]) * (-ekf_aw_params.k_fz_wing[1] * diff_alpha_u + -2 * ekf_aw_params.k_fz_wing[2] * aoa *
1136  diff_alpha_u) * V_a * V_a) / ekf_aw_params.vehicle_mass;
1137  G(5, 1) += (2 * v * (sin_skew * sin_skew + ekf_aw_params.k_fz_wing[3]) * (ekf_aw_params.k_fz_wing[0] +
1139  G(5, 2) += ((sin_skew * sin_skew + ekf_aw_params.k_fz_wing[3]) * (ekf_aw_params.k_fz_wing[1] * diff_alpha_w + 2 *
1140  ekf_aw_params.k_fz_wing[2] * aoa * diff_alpha_w) * V_a * V_a + 2 * w * (sin_skew * sin_skew +
1143 #endif
1144 
1145  // Elevator contribution
1146  float temp_6 = ekf_aw_params.k_fz_elev[1] * eawp.inputs.elevator_angle + ekf_aw_params.k_fz_elev[0];
1147  G(5, 0) += (2 * u * temp_6) / ekf_aw_params.vehicle_mass;
1148  G(5, 1) += (2 * v * temp_6) / ekf_aw_params.vehicle_mass;
1149  G(5, 2) += (2 * w * temp_6) / ekf_aw_params.vehicle_mass;
1150 
1151  // Hover prop contribution
1152  G(5, 0) += (ekf_aw_params.k_fz_hover[4] * sign_u) / ekf_aw_params.vehicle_mass;
1153  }
1154  // Offset contribution
1155  G(5, 8) = 1;
1156 
1157  // V_pitot related lines
1158  G(6, 0) = 1;
1159 
1160  // FOR DEBUG
1161 #if EKF_AW_DEBUG
1162  duration_7 = get_sys_time_usec() - tic;
1163 #endif
1164 
1165  // Innovation S Matrix Calculation
1166  matrix::SquareMatrix<float, EKF_AW_R_SIZE> S = G * eawp.P * G.transpose() + eawp.R; // M = identity
1167 
1168  // FOR DEBUG
1169 #if EKF_AW_DEBUG
1170  duration_8 = get_sys_time_usec() - tic;
1171 #endif
1172 
1173  // Kalman Gain Calculation
1174  matrix::Matrix<float, EKF_AW_COV_SIZE, EKF_AW_R_SIZE> K = eawp.P * G.transpose() * S.I();
1175 
1176  // FOR DEBUG
1177 #if EKF_AW_DEBUG
1178  duration_9 = get_sys_time_usec() - tic;
1179 #endif
1180 
1181  // Check if Kalman Gain contains any nan. Only update state if it is not NAN
1182  anyNan = false;
1183  for (int i = 0; i < EKF_AW_COV_SIZE; i++) {
1184  for (int j = 0; j < EKF_AW_R_SIZE; j++) {
1185  if (std::isnan(K(i, j))) {
1186  anyNan = true;
1187  break;
1188  }
1189  }
1190  }
1191 
1192  if (anyNan) {
1193  eawp.health.healthy = false;
1194  eawp.health.crashes_n += 1;
1195  } else {
1196  eawp.health.healthy = true;
1197 
1198  // Temp variable to slice K
1199  matrix::Matrix<float, 3, 3> K_slice_3;
1200  matrix::Matrix<float, 3, 1> K_slice_1;
1201 
1202  // State update using V_gnd
1203  K_slice_3 = K.slice<3, 3>(0, 0); eawp.state.V_body += K_slice_3 * eawp.innovations.V_gnd;
1204  K_slice_3 = K.slice<3, 3>(3, 0); eawp.state.wind += K_slice_3 * eawp.innovations.V_gnd;
1206  K_slice_3 = K.slice<3, 3>(6, 0); eawp.state.offset += K_slice_3 * eawp.innovations.V_gnd;
1207  }
1208 
1209  // State update using a_filt
1210  K_slice_3 = K.slice<3, 3>(0, 3); eawp.state.V_body += K_slice_3 * eawp.innovations.accel_filt;
1211  K_slice_3 = K.slice<3, 3>(3, 3); eawp.state.wind += K_slice_3 * eawp.innovations.accel_filt;
1213  K_slice_3 = K.slice<3, 3>(6, 3); eawp.state.offset += K_slice_3 * eawp.innovations.accel_filt;
1214  }
1215 
1216  // State update using V_pitot (if available)
1217  if (ekf_aw_params.use_pitot) {
1218  K_slice_1 = K.slice<3, 1>(0, 6); eawp.state.V_body += K_slice_1 * eawp.innovations.V_pitot;
1219  K_slice_1 = K.slice<3, 1>(3, 6); eawp.state.wind += K_slice_1 * eawp.innovations.V_pitot;
1221  K_slice_1 = K.slice<3, 1>(6, 6); eawp.state.offset += K_slice_1 * eawp.innovations.V_pitot;
1222  }
1223  }
1224  // FOR DEBUG
1225 #if EKF_AW_DEBUG
1226  duration_10 = get_sys_time_usec() - tic;
1227 #endif
1228 
1229  // Covariance update
1230  matrix::SquareMatrix<float, EKF_AW_COV_SIZE> eye;
1231  eye.setIdentity();
1232  eawp.P = (eye - K * G) * eawp.P;
1233  }
1234 
1235  // FOR DEBUG
1236 #if EKF_AW_DEBUG
1237  duration_11 = get_sys_time_usec() - tic;
1238 
1239  eawp.forces.fuselage(0) = duration_1 * 1.0f;
1240  eawp.forces.fuselage(1) = duration_2 * 1.0f;
1241  eawp.forces.fuselage(2) = duration_3 * 1.0f;
1242 
1243  eawp.forces.wing(0) = duration_4 * 1.0f;
1244  eawp.forces.wing(1) = duration_5 * 1.0f;
1245  eawp.forces.wing(2) = duration_6 * 1.0f;
1246 
1247  eawp.forces.elevator(0) = duration_7 * 1.0f;
1248  eawp.forces.elevator(1) = duration_8 * 1.0f;
1249  eawp.forces.elevator(2) = duration_9 * 1.0f;
1250 
1251  eawp.forces.hover(0) = duration_10 * 1.0f;
1252  eawp.forces.hover(1) = duration_11 * 1.0f;
1253 #endif
1254 
1255  //std::cout << "subs:\n" << EKF_Aw_Cov::Identity() - K * G << std::endl;
1256  //std::cout << "Cov matrix:\n" << eawp.P << std::endl;
1257  //std::cout << "S inverse:\n" << S.inverse() << std::endl;
1258  //std::cout << "K V_body V_gnd:\n" << K.block<3,3>(0,0) * eawp.innovations.V_gnd << std::endl;
1259  //std::cout << "K V_body Accel_filt:\n" << K.block<3,3>(0,3) * eawp.innovations.accel_filt << std::endl;
1260  //std::cout << "K:\n" << K << std::endl;
1261 
1262 }
1263 
1264 // Getter Functions
1265 struct NedCoor_f ekf_aw_get_speed_body(void)
1266 {
1267  const struct NedCoor_f s = {
1268  .x = eawp.state.V_body(0),
1269  .y = eawp.state.V_body(1),
1270  .z = eawp.state.V_body(2)
1271  };
1272  return s;
1273 }
1274 
1275 struct NedCoor_f ekf_aw_get_wind_ned(void)
1276 {
1277  const struct NedCoor_f w = {
1278  .x = eawp.state.wind(0),
1279  .y = eawp.state.wind(1),
1280  .z = eawp.state.wind(2)
1281  };
1282  return w;
1283 }
1284 
1285 struct NedCoor_f ekf_aw_get_offset(void) // TO DO: use right type instead of NEDCOORD_f
1286 {
1287  const struct NedCoor_f w = {
1288  .x = eawp.state.offset(0),
1289  .y = eawp.state.offset(1),
1290  .z = eawp.state.offset(2)
1291  };
1292  return w;
1293 }
1294 
1295 struct ekfHealth ekf_aw_get_health(void)
1296 {
1297  const struct ekfHealth w = {
1298  .healthy = eawp.health.healthy,
1299  .crashes_n = eawp.health.crashes_n
1300  };
1301  return w;
1302 }
1303 
1305 {
1306  const struct FloatVect3 w = {
1307  .x = eawp.innovations.V_gnd(0),
1308  .y = eawp.innovations.V_gnd(1),
1309  .z = eawp.innovations.V_gnd(2)
1310  };
1311  return w;
1312 }
1314 {
1315  const struct FloatVect3 w = {
1316  .x = eawp.innovations.accel_filt(0),
1317  .y = eawp.innovations.accel_filt(1),
1318  .z = eawp.innovations.accel_filt(2)
1319  };
1320  return w;
1321 }
1322 
1324 {
1325  const float w = eawp.innovations.V_pitot;
1326  return w;
1327 }
1328 
1329 void ekf_aw_get_meas_cov(float meas_cov[7])
1330 {
1331  float diagonal[EKF_AW_R_SIZE];
1332  for (int8_t i = 0; i < EKF_AW_R_SIZE; i++) {
1333  // Protect log10 against 0 and negative values
1334  if (eawp.R(i, i) <= 0.0f) {
1335  diagonal[i] = eawp.R(i, i);
1336  } else {
1337  diagonal[i] = log10(eawp.R(i, i));
1338  }
1339  }
1340 
1341  memcpy(meas_cov, diagonal, 7 * sizeof(float));
1342 }
1343 
1344 void ekf_aw_get_state_cov(float state_cov[9])
1345 {
1346  float diagonal[EKF_AW_COV_SIZE];
1347  for (int8_t i = 0; i < EKF_AW_COV_SIZE; i++) {
1348  // Protect log10 against 0 and negative values
1349  if (eawp.P(i, i) <= 0.0f) {
1350  diagonal[i] = eawp.P(i, i);
1351  } else {
1352  diagonal[i] = log10(eawp.P(i, i));
1353  }
1354  }
1355 
1356  memcpy(state_cov, diagonal, 9 * sizeof(float));
1357 }
1358 
1359 void ekf_aw_get_process_cov(float process_cov[12])
1360 {
1361  float diagonal[EKF_AW_Q_SIZE];
1362  for (int8_t i = 0; i < EKF_AW_Q_SIZE; i++) {
1363  // Protect log10 against 0 and negative values
1364  if (eawp.Q(i, i) <= 0.0f) {
1365  diagonal[i] = eawp.Q(i, i);
1366  } else {
1367  diagonal[i] = log10(eawp.Q(i, i));
1368  }
1369  }
1370 
1371  memcpy(process_cov, diagonal, 9 * sizeof(float));
1372 }
1373 
1374 void ekf_aw_get_fuselage_force(float force[3])
1375 {
1376  float temp[3];
1377  for (int8_t i = 0; i < 3; i++) {
1378  temp[i] = eawp.forces.fuselage(i);
1379  }
1380  memcpy(force, temp, 3 * sizeof(float));
1381 }
1382 
1383 void ekf_aw_get_wing_force(float force[3])
1384 {
1385  float temp[3];
1386  for (int8_t i = 0; i < 3; i++) {
1387  temp[i] = eawp.forces.wing(i);
1388  }
1389  memcpy(force, temp, 3 * sizeof(float));
1390 }
1391 
1392 void ekf_aw_get_elevator_force(float force[3])
1393 {
1394  float temp[3];
1395  for (int8_t i = 0; i < 3; i++) {
1396  temp[i] = eawp.forces.elevator(i);
1397  }
1398  memcpy(force, temp, 3 * sizeof(float));
1399 }
1400 
1401 void ekf_aw_get_hover_force(float force[3])
1402 {
1403  float temp[3];
1404  for (int8_t i = 0; i < 3; i++) {
1405  temp[i] = eawp.forces.hover(i);
1406  }
1407  memcpy(force, temp, 3 * sizeof(float));
1408 }
1409 
1410 void ekf_aw_get_pusher_force(float force[3])
1411 {
1412  float temp[3];
1413  for (int8_t i = 0; i < 3; i++) {
1414  temp[i] = eawp.forces.pusher(i);
1415  }
1416  memcpy(force, temp, 3 * sizeof(float));
1417 }
1418 
1420 {
1421  return &ekf_aw_params;
1422 }
1423 
1424 
1425 // Setter Functions
1427 {
1428  eawp.state.V_body(0) = s->x;
1429  eawp.state.V_body(1) = s->y;
1430  eawp.state.V_body(2) = s->z;
1431 }
1432 
1434 {
1435  eawp.state.wind(0) = s->x;
1436  eawp.state.wind(1) = s->y;
1437  eawp.state.wind(2) = s->z;
1438 }
1439 
1441 {
1442  eawp.state.offset(0) = s->x;
1443  eawp.state.offset(1) = s->y;
1444  eawp.state.offset(2) = s->z;
1445 }
1446 
1448 {
1449  eawp.health.healthy = true;
1450  eawp.health.crashes_n = 0;
1451 }
1452 
1453 // Forces functions
1454 
1455 // Fx Forces functions
1456 float fx_fuselage(float *skew, float *aoa, float *u)
1457 {
1458 
1459  float sign = *u < 0.0f ? -1.0f : *u > 0.0f ? 1.0f : 0.0f;
1460  // Fx = (k1*cos(skew)+k2+k3*alpha+k4*alpha^2)*V^2
1461  float Fx = (ekf_aw_params.k_fx_fuselage[0] * cosf(*skew) +
1463  ekf_aw_params.k_fx_fuselage[2] * *aoa +
1464  ekf_aw_params.k_fx_fuselage[3] * *aoa * *aoa) * *u * *u * sign;
1465 
1466  return Fx;
1467 };
1468 
1469 float fx_elevator(float *elevator_angle, float *V_a)
1470 {
1471 
1472  // Fx = (k1+k2*alpha+k3*alpha^2)*V^2
1473  float Fx = (ekf_aw_params.k_fx_elev[0] +
1474  ekf_aw_params.k_fx_elev[1] * *elevator_angle +
1475  ekf_aw_params.k_fx_elev[2] * *elevator_angle * *elevator_angle) * *V_a * *V_a;
1476 
1477  return Fx;
1478 };
1479 
1480 float fx_wing(float *skew, float *aoa, float *u)
1481 {
1482 
1483  float sign = *u < 0.0f ? -1.0f : *u > 0.0f ? 1.0f : 0.0f;
1484 
1485  float Fx = 0.0f;
1486 
1487  // Verify aircraft is flying forwards
1488  if (*u > 0.0f) {
1489  // Fx2 = (k1*(1+k5*skew)+(k2*alpha+k3*alpha^2))*(sin(skew)^2+k4)*V^2 //TO DO: UPDATE WITH SIMPLER MODEL
1490  Fx = sign * (*u * *u) * (ekf_aw_params.k_fx_wing[0] +
1491  ekf_aw_params.k_fx_wing[4] * sinf(*skew) +
1492  (ekf_aw_params.k_fx_wing[1] * *aoa +
1493  ekf_aw_params.k_fx_wing[2] * (*aoa * *aoa)) * (sinf(*skew) * sinf(*skew) + ekf_aw_params.k_fx_wing[3]));
1494  } else {
1495  Fx = sign * (*u * *u) * (ekf_aw_params.k_fx_wing[0]);
1496  }
1497 
1498 
1499  return Fx;
1500 };
1501 
1502 float fx_fy_hover(float *RPM_hover_mean, float *V)
1503 {
1504 
1505  float sign = *V < 0.0f ? -1.0f : *V > 0.0f ? 1.0f : 0.0f;
1506 
1507  // Fx = K1*V^2+K2*RPM^2*sqrt(V) + K3*V
1508  float Fx = ekf_aw_params.k_fx_hover[0] * (*V * *V) * sign +
1509  ekf_aw_params.k_fx_hover[1] * sqrtf(fabsf(*V)) * sign * (*RPM_hover_mean * *RPM_hover_mean);
1510 
1511  Fx += ekf_aw_params.k_fx_hover[2] * *V * sign;
1512 
1513  return Fx;
1514 };
1515 
1516 float fx_pusher(float *RPM_pusher, float *u)
1517 {
1518  float Fx = 0.0f;
1519 
1520  // Only generate a force if the pusher is turning
1521  if (*RPM_pusher > 500) {
1522  // Take care of case where drone is flying backwards with pusher (quite rare)
1523  if (*u > 0.0f) {
1524  // Fx_pusher = k1*RPM^2+k2*RPM*V+k3*V
1525  Fx = ekf_aw_params.k_fx_push[0] * (*RPM_pusher * *RPM_pusher) +
1526  ekf_aw_params.k_fx_push[1] * *RPM_pusher * *u +
1527  ekf_aw_params.k_fx_push[2] * *u;
1528 
1529  } else {
1530  Fx = ekf_aw_params.k_fx_push[0] * (*RPM_pusher * *RPM_pusher);
1531  };
1532  }
1533  return Fx;
1534 }
1535 
1536 // Fy Forces functions
1537 float fy_wing(float *skew, float *aoa __attribute__((unused)), float *u)
1538 {
1539 
1540  float sign = *u < 0.0f ? -1.0f : *u > 0.0f ? 1.0f : 0.0f;
1541 
1542  // Fy = K*cos(skew)*sin(skew)*u^2
1543  //TO DO: UPDATE MODEL
1544  float Fy = (ekf_aw_params.k_fy_wing[0] * cosf(*skew) * sinf(*skew)) * *u * *u * sign;
1545 
1546  return Fy;
1547 };
1548 
1549 // Fz Forces functions
1550 float fz_fuselage(float *skew, float *aoa, float *V_a)
1551 {
1552 
1553  // Fz = (k1*cos(skew)+k2+k3*alpha+k4*alpha^2)*V^2
1554  float Fz = (ekf_aw_params.k_fz_fuselage[0] * cosf(*skew) +
1556  ekf_aw_params.k_fz_fuselage[2] * *aoa +
1557  ekf_aw_params.k_fz_fuselage[3] * *aoa * *aoa) * *V_a * *V_a;
1558 
1559  return Fz;
1560 };
1561 
1562 float fz_elevator(float *elevator_angle, float *V_a)
1563 {
1564 
1565  // Fz = (k1+k2*alpha+k3*alpha^2)*V^2
1566  float Fz = (ekf_aw_params.k_fz_elev[0] +
1567  ekf_aw_params.k_fz_elev[1] * *elevator_angle) * *V_a * *V_a;
1568 
1569  return Fz;
1570 };
1571 
1572 float fz_wing(float *skew, float *aoa, float *u)
1573 {
1574 
1575  float sign = *u < 0.0f ? -1.0f : *u > 0.0f ? 1.0f : 0.0f;
1576 
1577  //Fz1 = ((k1+k2*alpha+k3*alpha^2)*(sin(skew)^2+k4)*V^2 //TO DO: UPDATE WITH SIMPLER MODEL
1578  float Fz = 0.0f;
1579  if (*u > 0.0f) {
1580  float Fz = ((ekf_aw_params.k_fz_wing[0] +
1581  ekf_aw_params.k_fz_wing[1] * *aoa +
1582  ekf_aw_params.k_fz_wing[2] * (*aoa * *aoa)) * (sinf(*skew) * sinf(*skew) + ekf_aw_params.k_fz_wing[3])) *
1583  (*u * *u) * sign;
1584  } else {
1585  Fz = 0.0f;
1586  }
1587 
1588 
1589  return Fz;
1590 };
1591 
1592 float fz_hover(matrix::Vector<float, 4> RPM_hover, float *V_a)
1593 {
1594 
1595  // Fz = RPM_1^2*K1 + RPM_2^2*K2 + RPM_3^2*K3 + RPM_4^2*K4 + K5*V
1596  float Fz = RPM_hover(0) * RPM_hover(0) * ekf_aw_params.k_fz_hover[0] +
1597  RPM_hover(1) * RPM_hover(1) * ekf_aw_params.k_fz_hover[1] +
1598  RPM_hover(2) * RPM_hover(2) * ekf_aw_params.k_fz_hover[2] +
1599  RPM_hover(3) * RPM_hover(3) * ekf_aw_params.k_fz_hover[3] ;
1600 
1601  Fz += ekf_aw_params.k_fz_hover[4] * *V_a;
1602 
1603  return Fz;
1604 
1605 };
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
float fz_fuselage(float *skew, float *aoa, float *V_a)
Definition: ekf_aw.cpp:1550
#define eawp
Definition: ekf_aw.cpp:382
#define EKF_AW_K_FY_BETA
Definition: ekf_aw.cpp:230
struct ekfAwState state
Definition: ekf_aw.cpp:78
#define EKF_AW_K2_FX_HOVER
Definition: ekf_aw.cpp:186
void ekf_aw_get_hover_force(float force[3])
Definition: ekf_aw.cpp:1401
#define EKF_AW_USE_MODEL_BASED_X
Definition: ekf_aw.cpp:138
#define EKF_AW_K2_FZ_HOVER
Definition: ekf_aw.cpp:282
#define EKF_AW_Q_OFFSET
Definition: ekf_aw.cpp:102
matrix::Vector3f accel
Definition: ekf_aw.cpp:51
void ekf_aw_update_params(void)
Definition: ekf_aw.cpp:552
struct ekfAwInputs inputs
Definition: ekf_aw.cpp:79
#define EKF_AW_K4_FZ_FUSELAGE
Definition: ekf_aw.cpp:262
float ekf_aw_get_innov_V_pitot(void)
Definition: ekf_aw.cpp:1323
struct FloatVect3 ekf_aw_get_innov_accel_filt(void)
Definition: ekf_aw.cpp:1313
matrix::Vector3f accel_filt
Definition: ekf_aw.cpp:63
void ekf_aw_get_meas_cov(float meas_cov[7])
Definition: ekf_aw.cpp:1329
ekfAwQVar
Definition: ekf_aw.cpp:22
@ EKF_AW_Q_mu_z_index
Definition: ekf_aw.cpp:25
@ EKF_AW_Q_mu_y_index
Definition: ekf_aw.cpp:25
@ EKF_AW_Q_mu_x_index
Definition: ekf_aw.cpp:25
@ EKF_AW_Q_accel_z_index
Definition: ekf_aw.cpp:23
@ EKF_AW_Q_gyro_x_index
Definition: ekf_aw.cpp:24
@ EKF_AW_Q_k_z_index
Definition: ekf_aw.cpp:26
@ EKF_AW_Q_accel_y_index
Definition: ekf_aw.cpp:23
@ EKF_AW_Q_accel_x_index
Definition: ekf_aw.cpp:23
@ EKF_AW_Q_gyro_y_index
Definition: ekf_aw.cpp:24
@ EKF_AW_Q_gyro_z_index
Definition: ekf_aw.cpp:24
@ EKF_AW_Q_k_x_index
Definition: ekf_aw.cpp:26
@ EKF_AW_Q_k_y_index
Definition: ekf_aw.cpp:26
@ EKF_AW_Q_SIZE
Definition: ekf_aw.cpp:27
float fz_wing(float *skew, float *aoa, float *V_a)
Definition: ekf_aw.cpp:1572
#define EKF_AW_P0_V_BODY
Definition: ekf_aw.cpp:107
void ekf_aw_set_speed_body(struct NedCoor_f *s)
Definition: ekf_aw.cpp:1426
void ekf_aw_get_state_cov(float state_cov[9])
Definition: ekf_aw.cpp:1344
#define EKF_AW_K4_FX_WING
Definition: ekf_aw.cpp:202
#define EKF_AW_K4_FX_FUSELAGE
Definition: ekf_aw.cpp:179
matrix::SquareMatrix< float, EKF_AW_R_SIZE > EKF_Aw_R
Definition: ekf_aw.cpp:40
#define EKF_AW_R_V_GND
Definition: ekf_aw.cpp:118
#define EKF_AW_K4_FZ_WING
Definition: ekf_aw.cpp:275
#define EKF_AW_K3_FX_ELEV
Definition: ekf_aw.cpp:225
#define EKF_AW_Q_MU
Definition: ekf_aw.cpp:99
#define EKF_AW_ELEV_MAX_ANGLE
Definition: ekf_aw.cpp:331
struct NedCoor_f ekf_aw_get_speed_body(void)
Definition: ekf_aw.cpp:1265
float skew
Definition: ekf_aw.cpp:56
#define EKF_AW_K5_FZ_HOVER
Definition: ekf_aw.cpp:291
#define EKF_AW_K1_FX_DRAG
Definition: ekf_aw.cpp:163
#define EKF_AW_P0_MU
Definition: ekf_aw.cpp:110
matrix::SquareMatrix< float, EKF_AW_Q_SIZE > EKF_Aw_Q
Definition: ekf_aw.cpp:30
struct ekfAwForces forces
Definition: ekf_aw.cpp:82
#define EKF_AW_K1_FZ_HOVER
Definition: ekf_aw.cpp:279
#define EKF_AW_K4_FZ_HOVER
Definition: ekf_aw.cpp:288
float fx_wing(float *skew, float *aoa, float *u)
Definition: ekf_aw.cpp:1480
#define EKF_AW_R_ACCEL_FILT_Y
Definition: ekf_aw.cpp:124
float fy_wing(float *skew, float *aoa, float *u)
Definition: ekf_aw.cpp:1537
#define EKF_AW_AZ_QUICK_CONV_MU_GAIN
Definition: ekf_aw.cpp:323
#define EKF_AW_USE_MODEL_BASED_Z
Definition: ekf_aw.cpp:144
#define EKF_AW_K1_FX_FUSELAGE
Definition: ekf_aw.cpp:170
struct ekfHealth ekf_aw_get_health(void)
Definition: ekf_aw.cpp:1295
matrix::Vector3f elevator
Definition: ekf_aw.cpp:71
#define EKF_AW_R_ACCEL_FILT_Z
Definition: ekf_aw.cpp:127
#define EKF_AW_ELEV_MIN_ANGLE
Definition: ekf_aw.cpp:334
#define EKF_AW_AX_SCHED_GAIN
Definition: ekf_aw.cpp:312
#define EKF_AW_AZ_SCHED_END_DEG
Definition: ekf_aw.cpp:309
void ekf_aw_get_fuselage_force(float force[3])
Definition: ekf_aw.cpp:1374
#define EKF_AW_R_V_PITOT
Definition: ekf_aw.cpp:130
#define EKF_AW_USE_PITOT
Definition: ekf_aw.cpp:153
#define EKF_AW_USE_BETA
Definition: ekf_aw.cpp:147
void ekf_aw_propagate(struct FloatVect3 *acc, struct FloatRates *gyro, struct FloatEulers *euler, float *pusher_RPM, float *hover_RPM_array, float *skew, float *elevator_angle, FloatVect3 *V_gnd, FloatVect3 *acc_filt, float *V_pitot, float dt)
Definition: ekf_aw.cpp:591
#define EKF_AW_Q_GYRO
Definition: ekf_aw.cpp:96
float elevator_angle
Definition: ekf_aw.cpp:57
struct ekfAwMeasurements innovations
Definition: ekf_aw.cpp:81
matrix::Vector3f hover
Definition: ekf_aw.cpp:72
ekfAwRVar
Definition: ekf_aw.cpp:33
@ EKF_AW_R_SIZE
Definition: ekf_aw.cpp:37
@ EKF_AW_R_V_gnd_x_index
Definition: ekf_aw.cpp:34
@ EKF_AW_R_a_z_filt_index
Definition: ekf_aw.cpp:35
@ EKF_AW_R_a_y_filt_index
Definition: ekf_aw.cpp:35
@ EKF_AW_R_a_x_filt_index
Definition: ekf_aw.cpp:35
@ EKF_AW_R_V_gnd_z_index
Definition: ekf_aw.cpp:34
@ EKF_AW_R_V_gnd_y_index
Definition: ekf_aw.cpp:34
@ EKF_AW_R_V_pitot_index
Definition: ekf_aw.cpp:36
#define EKF_AW_K2_FZ_ELEV
Definition: ekf_aw.cpp:298
#define EKF_AW_K_FY_V
Definition: ekf_aw.cpp:233
#define EKF_AW_K5_FX_WING
Definition: ekf_aw.cpp:205
#define EKF_AW_AX_SCHED_START_DEG
Definition: ekf_aw.cpp:315
EKF_Aw_R R
Definition: ekf_aw.cpp:86
matrix::Vector3f offset
Definition: ekf_aw.cpp:46
#define EKF_AW_K3_FZ_HOVER
Definition: ekf_aw.cpp:285
EKF_Aw_Cov P
Definition: ekf_aw.cpp:84
float deg2rad
Definition: ekf_aw.cpp:388
ekfAwCovVar
Definition: ekf_aw.cpp:12
@ EKF_AW_k_y_index
Definition: ekf_aw.cpp:15
@ EKF_AW_u_index
Definition: ekf_aw.cpp:13
@ EKF_AW_w_index
Definition: ekf_aw.cpp:13
@ EKF_AW_v_index
Definition: ekf_aw.cpp:13
@ EKF_AW_k_z_index
Definition: ekf_aw.cpp:15
@ EKF_AW_mu_y_index
Definition: ekf_aw.cpp:14
@ EKF_AW_k_x_index
Definition: ekf_aw.cpp:15
@ EKF_AW_mu_x_index
Definition: ekf_aw.cpp:14
@ EKF_AW_mu_z_index
Definition: ekf_aw.cpp:14
@ EKF_AW_COV_SIZE
Definition: ekf_aw.cpp:16
void ekf_aw_reset_health(void)
Definition: ekf_aw.cpp:1447
void ekf_aw_set_offset(struct NedCoor_f *s)
Definition: ekf_aw.cpp:1440
matrix::Vector3f V_body
Definition: ekf_aw.cpp:44
#define EKF_AW_K2_FZ_FUSELAGE
Definition: ekf_aw.cpp:256
struct ekfAwParameters * ekf_aw_get_param_handle(void)
Definition: ekf_aw.cpp:1419
#define EKF_AW_P0_OFFSET
Definition: ekf_aw.cpp:113
#define EKF_AW_AX_SCHED_END_DEG
Definition: ekf_aw.cpp:318
#define EKF_AW_K3_FX_HOVER
Definition: ekf_aw.cpp:189
#define EKF_AW_K5_FY_WING
Definition: ekf_aw.cpp:248
#define EKF_AW_AOA_MAX_ANGLE
Definition: ekf_aw.cpp:337
float fx_pusher(float *RPM_pusher, float *u)
Definition: ekf_aw.cpp:1516
#define EKF_AW_K1_FZ_WING
Definition: ekf_aw.cpp:266
#define EKF_AW_USE_MODEL_BASED_Y
Definition: ekf_aw.cpp:141
#define EKF_AW_K1_FZ_ELEV
Definition: ekf_aw.cpp:295
float fx_elevator(float *elevator_angle, float *V_a)
Definition: ekf_aw.cpp:1469
#define EKF_AW_K1_FX_HOVER
Definition: ekf_aw.cpp:183
#define EKF_AW_K3_FX_FUSELAGE
Definition: ekf_aw.cpp:176
#define EKF_AW_K2_FX_WING
Definition: ekf_aw.cpp:196
matrix::Vector3f pusher
Definition: ekf_aw.cpp:73
struct ekfHealth health
Definition: ekf_aw.cpp:88
#define EKF_AW_K2_FY_WING
Definition: ekf_aw.cpp:239
#define EKF_AW_K1_FX_ELEV
Definition: ekf_aw.cpp:219
matrix::Vector3f rates
Definition: ekf_aw.cpp:52
void ekf_aw_reset(void)
Definition: ekf_aw.cpp:584
#define EKF_AW_AZ_SCHED_GAIN
Definition: ekf_aw.cpp:303
#define EKF_AW_K1_FX_WING
Definition: ekf_aw.cpp:193
struct NedCoor_f ekf_aw_get_wind_ned(void)
Definition: ekf_aw.cpp:1275
#define EKF_AW_K2_FX_PUSH
Definition: ekf_aw.cpp:212
float fz_hover(matrix::Vector< float, 4 > RPM_hover, float *V_a)
Definition: ekf_aw.cpp:1592
float RPM_pusher
Definition: ekf_aw.cpp:54
static struct ekfAwPrivate ekf_aw_private
Definition: ekf_aw.cpp:380
#define EKF_AW_K3_FX_PUSH
Definition: ekf_aw.cpp:215
#define EKF_AW_SKEW_POLY_2
Definition: ekf_aw.cpp:351
matrix::Vector3f wind
Definition: ekf_aw.cpp:45
#define EKF_AW_SKEW_POLY_1
Definition: ekf_aw.cpp:348
void ekf_aw_get_elevator_force(float force[3])
Definition: ekf_aw.cpp:1392
float fx_fuselage(float *skew, float *aoa, float *u)
Definition: ekf_aw.cpp:1456
#define EKF_AW_K3_FZ_WING
Definition: ekf_aw.cpp:272
#define EKF_AW_AOA_MIN_ANGLE
Definition: ekf_aw.cpp:340
#define EKF_AW_K1_FY_WING
Definition: ekf_aw.cpp:236
matrix::Vector3f V_gnd
Definition: ekf_aw.cpp:62
struct FloatVect3 ekf_aw_get_innov_V_gnd(void)
Definition: ekf_aw.cpp:1304
static void init_ekf_aw_state(void)
Definition: ekf_aw.cpp:404
void ekf_aw_get_pusher_force(float force[3])
Definition: ekf_aw.cpp:1410
#define EKF_AW_K2_FX_DRAG
Definition: ekf_aw.cpp:166
#define EKF_AW_K1_FZ_FUSELAGE
Definition: ekf_aw.cpp:253
struct ekfAwMeasurements measurements
Definition: ekf_aw.cpp:80
matrix::Vector3f euler
Definition: ekf_aw.cpp:53
void ekf_aw_get_process_cov(float process_cov[12])
Definition: ekf_aw.cpp:1359
matrix::Vector3f wing
Definition: ekf_aw.cpp:70
#define EKF_AW_VEHICLE_MASS
Definition: ekf_aw.cpp:158
#define EKF_AW_WING_INSTALLED
Definition: ekf_aw.cpp:135
#define EKF_AW_PROPAGATE_OFFSET
Definition: ekf_aw.cpp:150
#define EKF_AW_K2_FX_FUSELAGE
Definition: ekf_aw.cpp:173
#define EKF_AW_AZ_SCHED_START_DEG
Definition: ekf_aw.cpp:306
EKF_Aw_Q Q
Definition: ekf_aw.cpp:85
#define EKF_AW_K3_FY_WING
Definition: ekf_aw.cpp:242
static const matrix::Vector3f gravity(0.f, 0.f, 9.81f)
void ekf_aw_get_wing_force(float force[3])
Definition: ekf_aw.cpp:1383
float rad2deg
Definition: ekf_aw.cpp:389
#define EKF_AW_K1_FX_PUSH
Definition: ekf_aw.cpp:209
#define EKF_AW_DEBUG
Definition: ekf_aw.cpp:356
#define EKF_AW_SKEW_POLY_0
Definition: ekf_aw.cpp:345
void ekf_aw_init(void)
Definition: ekf_aw.cpp:457
#define EKF_AW_Q_ACCEL
Definition: ekf_aw.cpp:93
#define EKF_AW_K3_FX_WING
Definition: ekf_aw.cpp:199
#define EKF_AW_K3_FZ_FUSELAGE
Definition: ekf_aw.cpp:259
float fx_fy_hover(float *RPM_hover_mean, float *V)
Definition: ekf_aw.cpp:1502
struct ekfAwParameters ekf_aw_params
Definition: ekf_aw.cpp:377
void ekf_aw_set_wind(struct NedCoor_f *s)
Definition: ekf_aw.cpp:1433
#define EKF_AW_K4_FY_WING
Definition: ekf_aw.cpp:245
#define EKF_AW_AZ_QUICK_CONV_ACCEL_GAIN
Definition: ekf_aw.cpp:326
matrix::Vector< float, 4 > RPM_hover
Definition: ekf_aw.cpp:55
float fz_elevator(float *elevator_angle, float *V_a)
Definition: ekf_aw.cpp:1562
#define EKF_AW_K2_FX_ELEV
Definition: ekf_aw.cpp:222
struct NedCoor_f ekf_aw_get_offset(void)
Definition: ekf_aw.cpp:1285
#define EKF_AW_R_ACCEL_FILT_X
Definition: ekf_aw.cpp:121
#define EKF_AW_K2_FZ_WING
Definition: ekf_aw.cpp:269
matrix::SquareMatrix< float, EKF_AW_COV_SIZE > EKF_Aw_Cov
Definition: ekf_aw.cpp:19
matrix::Vector3f fuselage
Definition: ekf_aw.cpp:69
float Q_gyro
gyro process noise
Definition: ekf_aw.h:16
float k_fy_wing[5]
Definition: ekf_aw.h:39
bool use_pitot
Definition: ekf_aw.h:49
float k_fx_wing[5]
Definition: ekf_aw.h:32
float R_V_pitot
airspeed measurement noise
Definition: ekf_aw.h:23
bool propagate_offset
Definition: ekf_aw.h:50
float vehicle_mass
Definition: ekf_aw.h:26
float k_fz_fuselage[4]
Definition: ekf_aw.h:42
float k_fz_hover[5]
Definition: ekf_aw.h:44
float k_fx_push[3]
Definition: ekf_aw.h:33
float k_fy_beta
Definition: ekf_aw.h:37
float Q_k
offset process noise
Definition: ekf_aw.h:18
float k_fz_wing[4]
Definition: ekf_aw.h:43
float R_accel_filt[3]
filtered accel measurement noise
Definition: ekf_aw.h:22
float k_fx_elev[3]
Definition: ekf_aw.h:34
float k_fy_v
Definition: ekf_aw.h:38
float R_V_gnd
speed measurement noise
Definition: ekf_aw.h:21
float k_fz_elev[2]
Definition: ekf_aw.h:45
bool quick_convergence
Definition: ekf_aw.h:51
bool use_model[3]
Definition: ekf_aw.h:48
float Q_mu
wind process noise
Definition: ekf_aw.h:17
float k_fx_drag[2]
Definition: ekf_aw.h:29
float k_fx_hover[3]
Definition: ekf_aw.h:31
float k_fx_fuselage[4]
Definition: ekf_aw.h:30
float Q_accel
accel process noise
Definition: ekf_aw.h:15
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
euler angles
angular rates
#define E
static float p[2][2]
static uint32_t s
static float sign(float x)
sign function
Definition: nav_fish.c:232
float x
in meters
vector in North East Down coordinates Units: meters
Architecture independent timing functions.
static float K[9]
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103