Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
wind_estimator.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Johan Maurin, Gautier Hattenberger
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
29 #include "mcu_periph/sys_time.h"
32 #include "generated/modules.h"
33 #include "state.h"
34 #include <string.h>
35 #ifndef SITL // no chibios threads in sim
36 #if !USE_CHIBIOS_RTOS
37 #error Only Chibios is supported
38 #endif
39 #include <ch.h>
40 #include <hal.h>
41 #endif
42 
46 #ifndef WE_UKF_KI
47 #define WE_UKF_KI 0.f // >= 0 to ensure that covariance is positive semi-definite
48 #endif
49 #ifndef WE_UKF_ALPHA
50 #define WE_UKF_ALPHA 0.5f // sigma point dispersion
51 #endif
52 #ifndef WE_UKF_BETA
53 #define WE_UKF_BETA 2.f // 2 is the best choice when distribution is gaussian
54 #endif
55 #ifndef WE_UKF_P0
56 #define WE_UKF_P0 0.2f // initial covariance diagonal element
57 #endif
58 #ifndef WE_UKF_R_GS
59 #define WE_UKF_R_GS 0.5f // ground speed measurement confidence
60 #endif
61 #ifndef WE_UKF_R_VA
62 #define WE_UKF_R_VA 0.5f // airspeed measurement confidence
63 #endif
64 #ifndef WE_UKF_R_AOA
65 #define WE_UKF_R_AOA 0.002f // angle of attack measurement confidence
66 #endif
67 #ifndef WE_UKF_R_SSA
68 #define WE_UKF_R_SSA 0.002f // sideslip angle measurement confidence
69 #endif
70 #ifndef WE_UKF_Q_VA
71 #define WE_UKF_Q_VA 0.1f // airspeed model confidence
72 #endif
73 #ifndef WE_UKF_Q_VA_SCALE
74 #define WE_UKF_Q_VA_SCALE 0.0001f // airspeed scale factor model confidence
75 #endif
76 #ifndef WE_UKF_Q_WIND
77 #define WE_UKF_Q_WIND 0.001f // wind model confidence
78 #endif
79 
80 #ifndef SEND_WIND_ESTIMATOR
81 #define SEND_WIND_ESTIMATOR TRUE
82 #endif
83 
85 
86 static void send_wind_estimator(struct transport_tx *trans, struct link_device *dev)
87 {
88  uint8_t flags = 7; // send all data
89  float upwind = -wind_estimator.wind.z;
90  float airspeed = float_vect3_norm(&wind_estimator.airspeed);
91  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID,
92  &flags,
93  &wind_estimator.wind.y, // east
94  &wind_estimator.wind.x, // north
95  &upwind,
96  &airspeed);
97 }
98 
99 #if PERIODIC_TELEMETRY
101 #endif
102 
103 #ifndef LOG_WIND_ESTIMATOR
104 #define LOG_WIND_ESTIMATOR FALSE
105 #endif
106 
107 #if LOG_WIND_ESTIMATOR
108 #ifndef SITL
110 #define PrintLog sdLogWriteLog
111 #define LogFileIsOpen() (pprzLogFile != -1)
112 #else // SITL: print in a file
113 #include <stdio.h>
114 #define PrintLog fprintf
115 #define LogFileIsOpen() (pprzLogFile != NULL)
116 static FILE* pprzLogFile = NULL;
117 #endif
118 static bool log_we_started;
119 #endif
120 
121 // matrix element
122 #define MAT_EL(_m, _l, _c, _n) _m[_l + _c * _n]
123 
124 // wind estimator public structure
126 
127 // local variables
128 static uint32_t time_step_before; // last periodic time
129 
130 /* Thread declaration
131  * MATLAB UKF is using at least 6.6KB of stack
132  */
133 #ifndef SITL
134 static THD_WORKING_AREA(wa_thd_windestimation, 8 * 1024);
135 static __attribute__((noreturn)) void thd_windestimate(void *arg);
136 
137 static MUTEX_DECL(we_ukf_mtx); // mutex for data acces protection
138 static SEMAPHORE_DECL(we_ukf_sem, 0); // semaaphore for sending signal
139 #endif
140 
141 /*----------------init_calculator---------------------*/
142 /* Init fonction to init different type of variable */
143 /* for the calculator before to calculate */
144 /*----------------------------------------------------*/
145 void init_calculator(void)
146 {
148 
149  // FIXME would be better to force Matlab to do this in initialize function
150  // zero input vector
151  memset(&ukf_U, 0, sizeof(ExtU));
152  // zero output vector
153  memset(&ukf_Y, 0, sizeof(ExtY));
154  // zero internal structure
155  memset(&ukf_DW, 0, sizeof(DW));
156  // zero init structure
157  memset(&ukf_init, 0, sizeof(ukf_init_type));
158  // zero params structure
159  memset(&ukf_params, 0, sizeof(ukf_params_type));
160 
161  ukf_init.x0[6] = 1.0f; // initial airspeed scale factor
162 
163  MAT_EL(ukf_init.P0, 0, 0, 7) = WE_UKF_P0;
164  MAT_EL(ukf_init.P0, 1, 1, 7) = WE_UKF_P0;
165  MAT_EL(ukf_init.P0, 2, 2, 7) = WE_UKF_P0;
166  MAT_EL(ukf_init.P0, 3, 3, 7) = WE_UKF_P0;
167  MAT_EL(ukf_init.P0, 4, 4, 7) = WE_UKF_P0;
168  MAT_EL(ukf_init.P0, 5, 5, 7) = WE_UKF_P0;
169  MAT_EL(ukf_init.P0, 6, 6, 7) = WE_UKF_P0;
170 
171  MAT_EL(ukf_params.R, 0, 0, 6) = powf(WE_UKF_R_GS, 2);
172  MAT_EL(ukf_params.R, 1, 1, 6) = powf(WE_UKF_R_GS, 2);
173  MAT_EL(ukf_params.R, 2, 2, 6) = powf(WE_UKF_R_GS, 2);
174  MAT_EL(ukf_params.R, 3, 3, 6) = powf(WE_UKF_R_VA, 2);
175  MAT_EL(ukf_params.R, 4, 4, 6) = powf(WE_UKF_R_AOA, 2);
176  MAT_EL(ukf_params.R, 5, 5, 6) = powf(WE_UKF_R_SSA, 2);
177 
178  MAT_EL(ukf_params.Q, 0, 0, 7) = powf(WE_UKF_Q_VA, 2);
179  MAT_EL(ukf_params.Q, 1, 1, 7) = powf(WE_UKF_Q_VA, 2);
180  MAT_EL(ukf_params.Q, 2, 2, 7) = powf(WE_UKF_Q_VA, 2);
181  MAT_EL(ukf_params.Q, 3, 3, 7) = powf(WE_UKF_Q_WIND, 2);
182  MAT_EL(ukf_params.Q, 4, 4, 7) = powf(WE_UKF_Q_WIND, 2);
183  MAT_EL(ukf_params.Q, 5, 5, 7) = powf(WE_UKF_Q_WIND, 2);
184  MAT_EL(ukf_params.Q, 6, 6, 7) = powf(WE_UKF_Q_VA_SCALE, 2);
185 
189  ukf_params.dt = WIND_ESTIMATOR_PERIODIC_PERIOD; // actually measured later
190 
192  wind_estimator.reset = false;
193 
201 
202  time_step_before = 0;
203 }
204 
205 // Run one step and save result
206 static inline void wind_estimator_step(void)
207 {
208 #if LOG_WIND_ESTIMATOR
209  if (LogFileIsOpen()) {
210  if (!log_we_started) {
211  // print header with initial parameters
212  int i;
213  PrintLog(pprzLogFile, "# Wind Estimator\n#\n");
214  PrintLog(pprzLogFile, "# Q = diag( ");
215  for (i = 0; i < 7; i++)
216  PrintLog(pprzLogFile, "%.8f ", MAT_EL(ukf_params.Q, i, i, 7));
217  PrintLog(pprzLogFile, ")\n");
218  PrintLog(pprzLogFile, "# R = diag( ");
219  for (i = 0; i < 5; i++)
220  PrintLog(pprzLogFile, "%.8f ", MAT_EL(ukf_params.R, i, i, 5));
221  PrintLog(pprzLogFile, ")\n");
222  PrintLog(pprzLogFile, "# ki = %.5f\n", ukf_init.ki);
223  PrintLog(pprzLogFile, "# alpha = %.5f\n", ukf_init.alpha);
224  PrintLog(pprzLogFile, "# beta = %.5f\n", ukf_init.beta);
225  PrintLog(pprzLogFile, "#\n");
226  PrintLog(pprzLogFile, "p q r ax ay az q1 q2 q3 q4 vkx vky vkz va aoa ssa u v w wx wy wz vas t\n");
227  log_we_started = true;
228  }
229  PrintLog(pprzLogFile, "%.5f %.5f %.5f %.3f %.3f %.3f %.4f %.4f %.4f %.4f %.5f %.5f %.5f %.5f %.5f %.5f ",
230  ukf_U.rates[0],
231  ukf_U.rates[1],
232  ukf_U.rates[2],
233  ukf_U.accel[0],
234  ukf_U.accel[1],
235  ukf_U.accel[2],
236  ukf_U.q[0],
237  ukf_U.q[1],
238  ukf_U.q[2],
239  ukf_U.q[3],
240  ukf_U.vk[0],
241  ukf_U.vk[1],
242  ukf_U.vk[2],
243  ukf_U.va,
244  ukf_U.aoa,
246  );
247  }
248 #endif
249 
250  // estimate wind if airspeed is high enough
251  if (ukf_U.va > 5.0f) {
252  // run estimation
254  // update output structure
261  // set ready flag
263  } else {
264  wind_estimator.airspeed.x = 0.f;
265  wind_estimator.airspeed.y = 0.f;
266  wind_estimator.airspeed.z = 0.f;
267  wind_estimator.wind.x = 0.f;
268  wind_estimator.wind.y = 0.f;
269  wind_estimator.wind.z = 0.f;
270  }
271 
272 #if LOG_WIND_ESTIMATOR
273  if (log_we_started) {
274  PrintLog(pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %d\n",
281  ukf_Y.xout[6],
283  );
284  }
285 #endif
286 }
287 
288 #include "modules/imu/imu.h"
289 /*----------------wind_estimator_periodic-------------*/
290 /* Put Data from State in struct use by the Thread */
291 /*----------------------------------------------------*/
293 {
294  // try to lock mutex without blocking
295  // if it fails, it means that previous computation took too long
296 #ifndef SITL
297  if (chMtxTryLock(&we_ukf_mtx)) {
298 #endif
299  if (wind_estimator.reset) {
300  init_calculator();
301  }
302  // update input vector from state interface
303  ukf_U.rates[0] = stateGetBodyRates_f()->p; // rad/s
304  ukf_U.rates[1] = stateGetBodyRates_f()->q; // rad/s
305  ukf_U.rates[2] = stateGetBodyRates_f()->r; // rad/s
306  // transform data in body frame
307  struct FloatVect3 accel_ned = {
311  };
312 // struct FloatRMat *ned_to_body = stateGetNedToBodyRMat_f();
313 // struct FloatVect3 accel_body;
314 // float_rmat_vmult(&accel_body, ned_to_body, &accel_ned);
315 
317  struct Int32Vect3 *accel_body_i = stateGetAccelBody_i();
318  struct FloatVect3 accel_body;
319  ACCELS_FLOAT_OF_BFP(accel_body, *accel_body_i);
320 
321  struct FloatVect3 tmp = accel_body;
322  //struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
323  //int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
324  // tilt and remove gravity
325  struct FloatVect3 gravity_ned = { 0.f, 0.f, 9.81f };
326  struct FloatVect3 gravity_body = { 0.f, 0.f, 0.f };
327 #if 0
328  struct FloatRMat *ned_to_body_rmat = stateGetNedToBodyRMat_f();
329  float_rmat_vmult(&gravity_body, ned_to_body_rmat, &gravity_ned);
330  VECT3_ADD(accel_body, gravity_body);
331 #else
332  struct FloatEulers *b2i_e = stateGetNedToBodyEulers_f();
333  accel_body.x += -9.81*sinf(b2i_e->theta);
334  accel_body.y += 9.81*cosf(b2i_e->theta)*sinf(b2i_e->phi);
335  accel_body.z += 9.81*cosf(b2i_e->theta)*cosf(b2i_e->phi);
336 #endif
338 
339  ukf_U.accel[0] = accel_body.x; // m/s^2
340  ukf_U.accel[1] = accel_body.y; // m/s^2
341  ukf_U.accel[2] = accel_body.z; // m/s^2
346  ukf_U.vk[0] = stateGetSpeedNed_f()->x; // m/s
347  ukf_U.vk[1] = stateGetSpeedNed_f()->y; // m/s
348  ukf_U.vk[2] = stateGetSpeedNed_f()->z; // m/s
349  ukf_U.va = stateGetAirspeed_f(); // m/s
350  ukf_U.aoa = stateGetAngleOfAttack_f(); // rad.
351  ukf_U.sideslip = stateGetSideslip_f(); // rad.
352 
354  // update input vector from state interface
355  //ukf_U.rates[0] = 0.;
356  //ukf_U.rates[1] = 0.;
357  //ukf_U.rates[2] = 0.;
358  //ukf_U.accel[0] = 0.;
359  //ukf_U.accel[1] = 0.;
360  //ukf_U.accel[2] = 0.;
361  //ukf_U.q[0] = 1.0;
362  //ukf_U.q[1] = 0.;
363  //ukf_U.q[2] = 0.;
364  //ukf_U.q[3] = 0.;
365  //ukf_U.vk[0] = 15.;
366  //ukf_U.vk[1] = 0.;
367  //ukf_U.vk[2] = 0.;
368  //ukf_U.va = 15.;
369  //ukf_U.aoa = 0.;
370  //ukf_U.sideslip = 0.;
371 
372  //DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 16, (float *)(&ukf_U));
373 
374  float msg[] = {
375  tmp.x,
376  tmp.y,
377  tmp.z,
378  gravity_body.x,
379  gravity_body.y,
380  gravity_body.z,
381  accel_body.x,
382  accel_body.y,
383  accel_body.z,
384  accel_ned.x,
385  accel_ned.y,
386  accel_ned.z
387  };
388  DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 12, msg);
389 
390  // compute DT and set input vector
391  if (time_step_before == 0) {
392  ukf_params.dt = WIND_ESTIMATOR_PERIODIC_PERIOD;
394  } else {
397  }
398 #ifndef SITL
399  chMtxUnlock(&we_ukf_mtx);
400  // send signal to computation thread
401  chSemSignal(&we_ukf_sem);
402  }
403 #else
405 #endif
406 }
407 
408 /*----------------- wind_estimator_event -------------*/
409 /* Fonction put data in State */
410 /*----------------------------------------------------*/
412 {
414 #ifndef SITL
415  if (chMtxTryLock(&we_ukf_mtx)) {
416 #endif
417  struct FloatVect2 wind_ne = {
420  };
421  stateSetHorizontalWindspeed_f(&wind_ne); //NEED CHECK
423 
424  // TODO do something with corrected airspeed norm
425 
426 #if SEND_WIND_ESTIMATOR
427  // send over telemetry
428  send_wind_estimator(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
429 #endif
430 
432 #ifndef SITL
433  chMtxUnlock(&we_ukf_mtx);
434  }
435 #endif
436  }
437 }
438 
439 /*------------------thd_windestimate------------------*/
440 /* Thread fonction */
441 /*----------------------------------------------------*/
442 #ifndef SITL
443 static void thd_windestimate(void *arg)
444 {
445  (void) arg;
446  chRegSetThreadName("wind estimation");
447 
448  while (true) {
449  // wait for incoming request signal
450  chSemWait(&we_ukf_sem);
451  // lock state
452  chMtxLock(&we_ukf_mtx);
453  // run estimation step
455  // unlock
456  chMtxUnlock(&we_ukf_mtx);
457  }
458 }
459 #endif
460 
461 /*-----------------wind_estimator_init----------------*/
462 /* Init the Thread and the calculator */
463 /*----------------------------------------------------*/
465 {
466  init_calculator();
467 
468 #if LOG_WIND_ESTIMATOR
469  log_we_started = false;
470 #if SITL
471  // open log file for writing
472  // path should be specified in airframe file
473  uint32_t counter = 0;
474  char filename[512];
475  snprintf(filename, 512, "%s/we_ukf_%05d.csv", STRINGIFY(WE_LOG_PATH), counter);
476  // check availale name
477  while ((pprzLogFile = fopen(filename, "r"))) {
478  fclose(pprzLogFile);
479  snprintf(filename, 512, "%s/we_ukf_%05d.csv", STRINGIFY(WE_LOG_PATH), ++counter);
480  }
481  pprzLogFile = fopen(filename, "w");
482  if (pprzLogFile == NULL) {
483  printf("Failed to open WE log file '%s'\n",filename);
484  } else {
485  printf("Opening WE log file '%s'\n",filename);
486  }
487 #endif
488 #endif
489 
490 #ifndef SITL
491  // Start wind estimation thread
492  chThdCreateStatic(wa_thd_windestimation, sizeof(wa_thd_windestimation),
493  NORMALPRIO + 2, thd_windestimate, NULL);
494 #endif
495 
496 #if PERIODIC_TELEMETRY
497  // register for periodic telemetry (for logging with flight recorder)
499 #endif
500 }
501 
503 {
504  wind_estimator.r_gs = _v;
505  MAT_EL(ukf_params.R, 0, 0, 6) = powf(_v, 2);
506  MAT_EL(ukf_params.R, 1, 1, 6) = powf(_v, 2);
507  MAT_EL(ukf_params.R, 2, 2, 6) = powf(_v, 2);
508 }
509 
511 {
512  wind_estimator.r_va = _v;
513  MAT_EL(ukf_params.R, 3, 3, 6) = powf(_v, 2);
514 }
515 
517 {
518  wind_estimator.r_aoa = _v;
519  MAT_EL(ukf_params.R, 4, 4, 6) = powf(_v, 2);
520 }
521 
523 {
524  wind_estimator.r_ssa = _v;
525  MAT_EL(ukf_params.R, 5, 5, 6) = powf(_v, 2);
526 }
527 
529 {
530  wind_estimator.q_va = _v;
531  MAT_EL(ukf_params.Q, 0, 0, 7) = powf(_v, 2);
532  MAT_EL(ukf_params.Q, 1, 1, 7) = powf(_v, 2);
533  MAT_EL(ukf_params.Q, 2, 2, 7) = powf(_v, 2);
534 }
535 
537 {
538  wind_estimator.q_wind = _v;
539  MAT_EL(ukf_params.Q, 3, 3, 7) = powf(_v, 2);
540  MAT_EL(ukf_params.Q, 4, 4, 7) = powf(_v, 2);
541  MAT_EL(ukf_params.Q, 5, 5, 7) = powf(_v, 2);
542 }
543 
545 {
547  MAT_EL(ukf_params.Q, 6, 6, 7) = powf(_v, 2);
548 }
549 
ukf_init_type ukf_init
ExtY ukf_Y
DW ukf_DW
void UKF_Wind_Estimator_initialize(void)
void UKF_Wind_Estimator_step(void)
ExtU ukf_U
ukf_params_type ukf_params
real32_T aoa
real32_T accel[3]
real32_T sideslip
real32_T xout[7]
real32_T q[4]
real32_T va
real32_T P0[49]
real32_T rates[3]
real32_T vk[3]
real32_T x0[7]
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:98
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
static float float_vect3_norm(struct FloatVect3 *v)
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
euler angles
rotation matrix
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition: state.h:953
static void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition: state.h:1291
static float stateGetSideslip_f(void)
Get sideslip (float).
Definition: state.h:1425
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
static float stateGetAngleOfAttack_f(void)
Get angle of attack (float).
Definition: state.h:1416
static void stateSetVerticalWindspeed_f(float v_windspeed)
Set vertical windspeed (float).
Definition: state.h:1300
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
Inertial Measurement Unit interface.
uint32_t counter
Definition: ins_flow.c:192
Paparazzi floating point algebra.
Paparazzi floating point math for geodetic calculations.
float z
in meters
float x
in meters
float y
in meters
FileDes pprzLogFile
Definition: sdlog_chibios.c:75
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
void wind_estimator_periodic(void)
void wind_estimator_Set_Q_WIND(float _v)
static MUTEX_DECL(we_ukf_mtx)
void wind_estimator_Set_R_AOA(float _v)
#define MAT_EL(_m, _l, _c, _n)
#define WE_UKF_R_SSA
static SEMAPHORE_DECL(we_ukf_sem, 0)
#define WE_UKF_Q_VA
static void send_wind_estimator(struct transport_tx *trans, struct link_device *dev)
#define WE_UKF_KI
Default parameters.
#define WE_UKF_R_AOA
void wind_estimator_Set_Q_VA_SCALE(float _v)
static void thd_windestimate(void *arg)
void wind_estimator_Set_R_SSA(float _v)
void init_calculator(void)
void wind_estimator_Set_R_VA(float _v)
void wind_estimator_Set_R_GS(float _v)
#define WE_UKF_Q_WIND
static THD_WORKING_AREA(wa_thd_windestimation, 8 *1024)
static void wind_estimator_step(void)
#define WE_UKF_P0
void wind_estimator_Set_Q_VA(float _v)
struct WindEstimator wind_estimator
#define WE_UKF_R_VA
#define WE_UKF_Q_VA_SCALE
#define WE_UKF_ALPHA
#define WE_UKF_BETA
void wind_estimator_init(void)
static uint32_t time_step_before
void wind_estimator_event(void)
#define WE_UKF_R_GS
Wind Estimator based on generated library from Matlab.
float q_va
noise associated to airspeed vector model
struct FloatVect3 wind
wind vector in NED frame
float r_va
noise associated to airspeed norm measurement
float q_wind
noise associated to wind vector model
bool data_available
new data available
bool reset
reset filter flag
float r_ssa
noise associated to sideslip angle measurement
float q_va_scale
noise associated to airspeed scale factor model
struct FloatVect3 airspeed
airspeed vector in body frame
float r_gs
noise associated to ground speed measurement
float r_aoa
noise associated to angle of attack measurement