Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
hf_float.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  * Copyright (C) 2009 Felix Ruess <felix.ruess@gmail.com>
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, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
31 #include "subsystems/imu.h"
32 #include "state.h"
33 #include "subsystems/gps.h"
34 #include <stdlib.h>
36 #include "generated/airframe.h"
37 
38 #ifdef SITL
39 #include <stdio.h>
40 #define DBG_LEVEL 1
41 #define PRINT_DBG(_l, _p) { \
42  if (DBG_LEVEL >= _l) \
43  printf _p; \
44  }
45 #else
46 #define PRINT_DBG(_l, _p) {}
47 #endif
48 
49 
50 #ifndef AHRS_PROPAGATE_FREQUENCY
51 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
52 #endif
53 
54 #ifndef HFF_PRESCALER
55 #if AHRS_PROPAGATE_FREQUENCY == 512
56 #define HFF_PRESCALER 16
57 #elif AHRS_PROPAGATE_FREQUENCY == 500
58 #define HFF_PRESCALER 10
59 #else
60 #error "HFF_PRESCALER not set, needs to be a divisor of AHRS_PROPAGATE_FREQUENCY"
61 #endif
62 #endif
63 
65 #define HFF_FREQ ((AHRS_PROPAGATE_FREQUENCY)/HFF_PRESCALER)
66 #define DT_HFILTER (1./HFF_FREQ)
67 
69 #define INIT_PXX 1.
70 
71 #ifndef HFF_ACCEL_NOISE
72 #define HFF_ACCEL_NOISE 0.5
73 #endif
74 #define Q HFF_ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
75 #define Qdotdot HFF_ACCEL_NOISE*DT_HFILTER
76 
77 //TODO: proper measurement noise
78 #ifndef HFF_R_POS
79 #define HFF_R_POS 8.
80 #endif
81 #ifndef HFF_R_POS_MIN
82 #define HFF_R_POS_MIN 3.
83 #endif
84 
85 #ifndef HFF_R_SPEED
86 #define HFF_R_SPEED 2.
87 #endif
88 #ifndef HFF_R_SPEED_MIN
89 #define HFF_R_SPEED_MIN 1.
90 #endif
91 
92 #ifndef HFF_UPDATE_SPEED
93 #define HFF_UPDATE_SPEED TRUE
94 #endif
95 
96 #ifndef HFF_LOWPASS_CUTOFF_FREQUENCY
97 #define HFF_LOWPASS_CUTOFF_FREQUENCY 14
98 #endif
99 
100 #if HFF_LOWPASS_CUTOFF_FREQUENCY < 8
101 #error "It is not allowed to use a cutoff frequency lower than 8Hz due to overflow issues."
102 #endif
103 
104 /* low pass filter variables */
108 
109 /* gps measurement noise */
111 
112 /*
113 
114  X_x = [ x xdot]
115  X_y = [ y ydot]
116 
117 
118 */
119 /* output filter states */
121 
122 
123 /* last acceleration measurement */
124 static float b2_hff_xdd_meas;
125 static float b2_hff_ydd_meas;
126 
127 /* last velocity measurement */
128 static float b2_hff_xd_meas;
129 static float b2_hff_yd_meas;
130 
131 /* last position measurement */
132 static float b2_hff_x_meas;
133 static float b2_hff_y_meas;
134 
136 static int b2_hff_ps_counter;
137 
138 /*
139  * For GPS lag compensation
140  *
141  *
142  *
143  */
144 #ifdef GPS_LAG
145 /*
146  * GPS_LAG is defined in seconds in airframe file
147  */
148 
150 #define GPS_LAG_N ((int) (GPS_LAG * HFF_FREQ + 0.5))
151 
152 #define GPS_DT_N ((int) (HFF_FREQ / 4))
153 
154 #define GPS_LAG_TOLERANCE 0.08
155 #define GPS_LAG_TOL_N ((int) (GPS_LAG_TOLERANCE * HFF_FREQ + 0.5))
156 
157 /* maximum number of past propagation steps to rerun per ap cycle
158  * make sure GPS_LAG_N/MAX_PP_STEPS < 128
159  * GPS_LAG_N/MAX_PP_STEPS/512 = seconds until re-propagation catches up with the present
160  */
161 #define MAX_PP_STEPS 6
162 
163 /* variables for mean accel buffer */
164 #define ACC_BUF_MAXN (GPS_LAG_N+10)
165 #define INC_ACC_IDX(idx) { idx = (idx + 1) < ACC_BUF_MAXN ? (idx + 1) : 0; }
166 
168 struct FloatVect2 past_accel[ACC_BUF_MAXN];
169 
170 static unsigned int acc_buf_r;
171 static unsigned int acc_buf_w;
172 static unsigned int acc_buf_n;
173 
174 
175 /*
176  * stuff for ringbuffer to store past filter states
177  */
178 #define HFF_RB_MAXN ((int) (GPS_LAG * 4))
179 #define INC_RB_POINTER(ptr) { \
180  if (ptr == &b2_hff_rb[HFF_RB_MAXN-1]) \
181  ptr = b2_hff_rb; \
182  else \
183  ptr++; \
184  }
185 
187 struct HfilterFloat b2_hff_rb[HFF_RB_MAXN];
188 struct HfilterFloat *b2_hff_rb_put;
189 #endif /* GPS_LAG */
190 
192 static int b2_hff_rb_n;
193 
194 
196 static int lag_counter_err;
197 
199 static int save_counter;
200 static int past_save_counter;
201 #define SAVE_NOW 0
202 #define SAVING -1
203 #define SAVE_DONE -2
204 
205 #define HFF_LOST_LIMIT 1000
208 
209 #ifdef GPS_LAG
210 static void b2_hff_get_past_accel(unsigned int back_n);
211 static void b2_hff_rb_put_state(struct HfilterFloat* source);
212 static void b2_hff_rb_drop_last(void);
213 static void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source);
214 #endif
215 
216 
217 static void b2_hff_init_x(float init_x, float init_xdot);
218 static void b2_hff_init_y(float init_y, float init_ydot);
219 
220 static void b2_hff_propagate_x(struct HfilterFloat* hff_work);
221 static void b2_hff_propagate_y(struct HfilterFloat* hff_work);
222 
223 static void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, float Rpos);
224 static void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, float Rpos);
225 
226 static void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel);
227 static void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel);
228 
229 #if PERIODIC_TELEMETRY
231 
232 static void send_hff(void) {
233  DOWNLINK_SEND_HFF(DefaultChannel, DefaultDevice,
234  &b2_hff_state.x,
235  &b2_hff_state.y,
240 }
241 
242 static void send_hff_debug(void) {
243  DOWNLINK_SEND_HFF_DBG(DefaultChannel, DefaultDevice,
244  &b2_hff_x_meas,
245  &b2_hff_y_meas,
248  &b2_hff_state.xP[0][0],
249  &b2_hff_state.yP[0][0],
250  &b2_hff_state.xP[1][1],
251  &b2_hff_state.yP[1][1]);
252 }
253 
254 #ifdef GPS_LAG
255 static void send_hff_gps(void) {
256  DOWNLINK_SEND_HFF_GPS(DefaultChannel, DefaultDevice,
257  &(b2_hff_rb_last->lag_counter),
259  &save_counter);
260 }
261 #endif
262 
263 #endif
264 
265 void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
268  b2_hff_init_x(init_x, init_xdot);
269  b2_hff_init_y(init_y, init_ydot);
270 #ifdef GPS_LAG
271  /* init buffer for past mean accel values */
272  acc_buf_r = 0;
273  acc_buf_w = 0;
274  acc_buf_n = 0;
275  b2_hff_rb_put = b2_hff_rb;
276  b2_hff_rb_last = b2_hff_rb;
277  b2_hff_rb_last->rollback = FALSE;
278  b2_hff_rb_last->lag_counter = 0;
279  b2_hff_state.lag_counter = GPS_LAG_N;
280 #ifdef SITL
281  printf("GPS_LAG: %f\n", GPS_LAG);
282  printf("GPS_LAG_N: %d\n", GPS_LAG_N);
283  printf("GPS_DT_N: %d\n", GPS_DT_N);
284  printf("DT_HFILTER: %f\n", DT_HFILTER);
285  printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N);
286 #endif
287 #else
288  b2_hff_rb_last = &b2_hff_state;
290 #endif
291  b2_hff_rb_n = 0;
293  lag_counter_err = 0;
294  save_counter = -1;
296  b2_hff_ps_counter = 1;
299 
300 #if PERIODIC_TELEMETRY
301  register_periodic_telemetry(DefaultPeriodic, "HFF", send_hff);
302  register_periodic_telemetry(DefaultPeriodic, "HFF_DBG", send_hff_debug);
303 #ifdef GPS_LAG
304  register_periodic_telemetry(DefaultPeriodic, "HFF_GPS", send_hff_gps);
305 #endif
306 #endif
307 
311 }
312 
313 static void b2_hff_init_x(float init_x, float init_xdot) {
314  b2_hff_state.x = init_x;
315  b2_hff_state.xdot = init_xdot;
316  int i, j;
317  for (i=0; i<HFF_STATE_SIZE; i++) {
318  for (j=0; j<HFF_STATE_SIZE; j++)
319  b2_hff_state.xP[i][j] = 0.;
320  b2_hff_state.xP[i][i] = INIT_PXX;
321  }
322 }
323 
324 static void b2_hff_init_y(float init_y, float init_ydot) {
325  b2_hff_state.y = init_y;
326  b2_hff_state.ydot = init_ydot;
327  int i, j;
328  for (i=0; i<HFF_STATE_SIZE; i++) {
329  for (j=0; j<HFF_STATE_SIZE; j++)
330  b2_hff_state.yP[i][j] = 0.;
331  b2_hff_state.yP[i][i] = INIT_PXX;
332  }
333 }
334 
335 #ifdef GPS_LAG
336 static void b2_hff_store_accel_ltp(float x, float y) {
337  past_accel[acc_buf_w].x = x;
338  past_accel[acc_buf_w].y = y;
339  INC_ACC_IDX(acc_buf_w);
340 
341  if (acc_buf_n < ACC_BUF_MAXN) {
342  acc_buf_n++;
343  } else {
344  INC_ACC_IDX(acc_buf_r);
345  }
346 }
347 
348 /* get the accel values from back_n steps ago */
349 static void b2_hff_get_past_accel(unsigned int back_n) {
350  int i;
351  if (back_n > acc_buf_n) {
352  PRINT_DBG(1, ("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n));
353  back_n = acc_buf_n;
354  } else if (back_n == 0) {
355  PRINT_DBG(1, ("Cannot go back zero steps!\n"));
356  return;
357  }
358  if ((int)(acc_buf_w - back_n) < 0)
359  i = acc_buf_w - back_n + ACC_BUF_MAXN;
360  else
361  i = acc_buf_w - back_n;
362  b2_hff_xdd_meas = past_accel[i].x;
363  b2_hff_ydd_meas = past_accel[i].y;
364  PRINT_DBG(3, ("get past accel. buf_n: %2d \tbuf_w: %2d \tback_n: %2d \ti: %2d \txdd: %f \tydd: %f\n", acc_buf_n, acc_buf_w, back_n, i, b2_hff_xdd_meas, b2_hff_ydd_meas));
365 }
366 
367 static void b2_hff_rb_put_state(struct HfilterFloat* source) {
368  /* copy state from source into buffer */
369  b2_hff_set_state(b2_hff_rb_put, source);
370  b2_hff_rb_put->lag_counter = 0;
371  b2_hff_rb_put->rollback = FALSE;
372 
373  /* forward write pointer */
374  INC_RB_POINTER(b2_hff_rb_put);
375 
376  /* increase fill count and forward last pointer if neccessary */
377  if (b2_hff_rb_n < HFF_RB_MAXN) {
378  b2_hff_rb_n++;
379  } else {
380  INC_RB_POINTER(b2_hff_rb_last);
381  }
382  PRINT_DBG(2, ("put state. fill count now: %d\n", b2_hff_rb_n));
383 }
384 
385 static void b2_hff_rb_drop_last(void) {
386  if (b2_hff_rb_n > 0) {
387  INC_RB_POINTER(b2_hff_rb_last);
388  b2_hff_rb_n--;
389  } else {
390  PRINT_DBG(2, ("hff ringbuffer empty!\n"));
391  b2_hff_rb_last->lag_counter = 0;
392  b2_hff_rb_last->rollback = FALSE;
393  }
394  PRINT_DBG(2, ("drop last state. fill count now: %d\n", b2_hff_rb_n));
395 }
396 
397 /* copy source state to dest state */
398 static void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source) {
399  dest->x = source->x;
400  dest->xdot = source->xdot;
401  dest->xdotdot = source->xdotdot;
402  dest->y = source->y;
403  dest->ydot = source->ydot;
404  dest->ydotdot = source->ydotdot;
405  for (int i=0; i < HFF_STATE_SIZE; i++) {
406  for (int j=0; j < HFF_STATE_SIZE; j++) {
407  dest->xP[i][j] = source->xP[i][j];
408  dest->yP[i][j] = source->yP[i][j];
409  }
410  }
411 }
412 
413 static void b2_hff_propagate_past(struct HfilterFloat* hff_past) {
414  PRINT_DBG(1, ("enter propagate past: %d\n", hff_past->lag_counter));
415  /* run max MAX_PP_STEPS propagation steps */
416  for (int i=0; i < MAX_PP_STEPS; i++) {
417  if (hff_past->lag_counter > 0) {
418  b2_hff_get_past_accel(hff_past->lag_counter);
419  PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter));
420  b2_hff_propagate_x(hff_past);
421  b2_hff_propagate_y(hff_past);
422  hff_past->lag_counter--;
423 
424  if (past_save_counter > 0) {
426  PRINT_DBG(2, ("dec past_save_counter: %d\n", past_save_counter));
427  } else if (past_save_counter == SAVE_NOW) {
428  /* next GPS measurement valid at this state -> save */
429  PRINT_DBG(2, ("save past state\n"));
430  b2_hff_rb_put_state(hff_past);
432  } else if (past_save_counter == SAVING) {
433  /* increase lag counter on if next state is already saved */
434  if (hff_past == &b2_hff_rb[HFF_RB_MAXN-1])
435  b2_hff_rb[0].lag_counter++;
436  else
437  (hff_past+1)->lag_counter++;
438  }
439  }
440 
441  /* finished re-propagating the past values */
442  if (hff_past->lag_counter == 0) {
443  b2_hff_set_state(&b2_hff_state, hff_past);
444  b2_hff_rb_drop_last();
446  break;
447  }
448  }
449 }
450 #endif /* GPS_LAG */
451 
452 
453 void b2_hff_propagate(void) {
456 
457 #ifdef GPS_LAG
458  /* continue re-propagating to catch up with the present */
459  if (b2_hff_rb_last->rollback) {
460  b2_hff_propagate_past(b2_hff_rb_last);
461  }
462 #endif
463 
464  /* rotate imu accel measurement to body frame and filter */
465  struct Int32Vect3 acc_meas_body;
466  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
467  INT32_RMAT_TRANSP_VMULT(acc_meas_body, *body_to_imu_rmat, imu.accel);
468 
469  struct Int32Vect3 acc_body_filtered;
470  acc_body_filtered.x = update_butterworth_2_low_pass_int(&filter_x, acc_meas_body.x);
471  acc_body_filtered.y = update_butterworth_2_low_pass_int(&filter_y, acc_meas_body.y);
472  acc_body_filtered.z = update_butterworth_2_low_pass_int(&filter_z, acc_meas_body.z);
473 
474  /* propagate current state if it is time */
475  if (b2_hff_ps_counter == HFF_PRESCALER) {
476  b2_hff_ps_counter = 1;
478  struct Int32Vect3 filtered_accel_ltp;
479  struct Int32RMat* ltp_to_body_rmat = stateGetNedToBodyRMat_i();
480  INT32_RMAT_TRANSP_VMULT(filtered_accel_ltp, (*ltp_to_body_rmat), acc_body_filtered);
481  b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.x);
482  b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.y);
483 #ifdef GPS_LAG
484  b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
485 #endif
486  /*
487  * propagate current state
488  */
491 
492 #ifdef GPS_LAG
493  /* increase lag counter on last saved state */
494  if (b2_hff_rb_n > 0)
495  b2_hff_rb_last->lag_counter++;
496 
497  /* save filter state if needed */
498  if (save_counter == 0) {
499  PRINT_DBG(1, ("save current state\n"));
500  b2_hff_rb_put_state(&b2_hff_state);
501  save_counter = -1;
502  } else if (save_counter > 0) {
503  save_counter--;
504  }
505 #endif
506  }
507  } else {
509  }
510 }
511 
512 void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned) {
514 
515 #if USE_GPS_ACC4R
516  Rgps_pos = (float) gps.pacc / 100.;
519 
520  Rgps_vel = (float) gps.sacc / 100.;
523 #endif
524 
525 #ifdef GPS_LAG
526  if (GPS_LAG_N == 0) {
527 #endif
528 
529  /* update filter state with measurement */
530  b2_hff_update_x(&b2_hff_state, pos_ned->x, Rgps_pos);
531  b2_hff_update_y(&b2_hff_state, pos_ned->y, Rgps_pos);
532 #if HFF_UPDATE_SPEED
533  b2_hff_update_xdot(&b2_hff_state, speed_ned->x, Rgps_vel);
534  b2_hff_update_ydot(&b2_hff_state, speed_ned->y, Rgps_vel);
535 #endif
536 
537 
538 #ifdef GPS_LAG
539  } else if (b2_hff_rb_n > 0) {
540  /* roll back if state was saved approx when GPS was valid */
541  lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N;
542  PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter, lag_counter_err));
543  if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
544  b2_hff_rb_last->rollback = TRUE;
545  b2_hff_update_x(b2_hff_rb_last, pos_ned->x, Rgps_pos);
546  b2_hff_update_y(b2_hff_rb_last, pos_ned->y, Rgps_pos);
547 #if HFF_UPDATE_SPEED
548  b2_hff_update_xdot(b2_hff_rb_last, speed_ned->x, Rgps_vel);
549  b2_hff_update_ydot(b2_hff_rb_last, speed_ned->y, Rgps_vel);
550 #endif
551  past_save_counter = GPS_DT_N-1;// + lag_counter_err;
552  PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter));
553  b2_hff_propagate_past(b2_hff_rb_last);
554  } else if (lag_counter_err >= GPS_DT_N - (GPS_LAG_TOL_N+1)) {
555  /* apparently missed a GPS update, try next saved state */
556  PRINT_DBG(2, ("try next saved state\n"));
557  b2_hff_rb_drop_last();
558  b2_hff_update_gps(pos_ned, speed_ned);
559  }
560  } else if (save_counter < 0) {
561  /* ringbuffer empty -> save output filter state at next GPS validity point in time */
562  save_counter = GPS_DT_N-1 - (GPS_LAG_N % GPS_DT_N);
563  PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
564  }
565 
566 #endif /* GPS_LAG */
567 }
568 
569 
570 void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel) {
571  b2_hff_state.x = pos.x;
572  b2_hff_state.y = pos.y;
573  b2_hff_state.xdot = vel.x;
574  b2_hff_state.ydot = vel.y;
575 #ifdef GPS_LAG
576  while (b2_hff_rb_n > 0) {
577  b2_hff_rb_drop_last();
578  }
579  save_counter = -1;
581 #endif
582 }
583 
584 
585 /*
586  *
587  * Propagation
588  *
589  *
590 
591  F = [ 1 dt
592  0 1 ];
593 
594  B = [ dt^2/2 dt]';
595 
596  Q = [ 0.01 0
597  0 0.01];
598 
599  Xk1 = F * Xk0 + B * accel;
600 
601  Pk1 = F * Pk0 * F' + Q;
602 
603 */
604 static void b2_hff_propagate_x(struct HfilterFloat* hff_work) {
605  /* update state */
606  hff_work->xdotdot = b2_hff_xdd_meas;
607  hff_work->x = hff_work->x + DT_HFILTER * hff_work->xdot + DT_HFILTER*DT_HFILTER/2 * hff_work->xdotdot;
608  hff_work->xdot = hff_work->xdot + DT_HFILTER * hff_work->xdotdot;
609  /* update covariance */
610  const float FPF00 = hff_work->xP[0][0] + DT_HFILTER * ( hff_work->xP[1][0] + hff_work->xP[0][1] + DT_HFILTER * hff_work->xP[1][1] );
611  const float FPF01 = hff_work->xP[0][1] + DT_HFILTER * hff_work->xP[1][1];
612  const float FPF10 = hff_work->xP[1][0] + DT_HFILTER * hff_work->xP[1][1];
613  const float FPF11 = hff_work->xP[1][1];
614 
615  hff_work->xP[0][0] = FPF00 + Q;
616  hff_work->xP[0][1] = FPF01;
617  hff_work->xP[1][0] = FPF10;
618  hff_work->xP[1][1] = FPF11 + Qdotdot;
619 }
620 
621 static void b2_hff_propagate_y(struct HfilterFloat* hff_work) {
622  /* update state */
623  hff_work->ydotdot = b2_hff_ydd_meas;
624  hff_work->y = hff_work->y + DT_HFILTER * hff_work->ydot + DT_HFILTER*DT_HFILTER/2 * hff_work->ydotdot;
625  hff_work->ydot = hff_work->ydot + DT_HFILTER * hff_work->ydotdot;
626  /* update covariance */
627  const float FPF00 = hff_work->yP[0][0] + DT_HFILTER * ( hff_work->yP[1][0] + hff_work->yP[0][1] + DT_HFILTER * hff_work->yP[1][1] );
628  const float FPF01 = hff_work->yP[0][1] + DT_HFILTER * hff_work->yP[1][1];
629  const float FPF10 = hff_work->yP[1][0] + DT_HFILTER * hff_work->yP[1][1];
630  const float FPF11 = hff_work->yP[1][1];
631 
632  hff_work->yP[0][0] = FPF00 + Q;
633  hff_work->yP[0][1] = FPF01;
634  hff_work->yP[1][0] = FPF10;
635  hff_work->yP[1][1] = FPF11 + Qdotdot;
636 }
637 
638 
639 /*
640  *
641  * Update position
642  *
643  *
644 
645  H = [1 0];
646  R = 0.1;
647  // state residual
648  y = pos_measurement - H * Xm;
649  // covariance residual
650  S = H*Pm*H' + R;
651  // kalman gain
652  K = Pm*H'*inv(S);
653  // update state
654  Xp = Xm + K*y;
655  // update covariance
656  Pp = Pm - K*H*Pm;
657 */
658 void b2_hff_update_pos (struct FloatVect2 pos, struct FloatVect2 Rpos) {
659  b2_hff_update_x(&b2_hff_state, pos.x, Rpos.x);
660  b2_hff_update_y(&b2_hff_state, pos.y, Rpos.y);
661 }
662 
663 static void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, float Rpos) {
664  b2_hff_x_meas = x_meas;
665 
666  const float y = x_meas - hff_work->x;
667  const float S = hff_work->xP[0][0] + Rpos;
668  const float K1 = hff_work->xP[0][0] * 1/S;
669  const float K2 = hff_work->xP[1][0] * 1/S;
670 
671  hff_work->x = hff_work->x + K1 * y;
672  hff_work->xdot = hff_work->xdot + K2 * y;
673 
674  const float P11 = (1. - K1) * hff_work->xP[0][0];
675  const float P12 = (1. - K1) * hff_work->xP[0][1];
676  const float P21 = -K2 * hff_work->xP[0][0] + hff_work->xP[1][0];
677  const float P22 = -K2 * hff_work->xP[0][1] + hff_work->xP[1][1];
678 
679  hff_work->xP[0][0] = P11;
680  hff_work->xP[0][1] = P12;
681  hff_work->xP[1][0] = P21;
682  hff_work->xP[1][1] = P22;
683 }
684 
685 static void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, float Rpos) {
686  b2_hff_y_meas = y_meas;
687 
688  const float y = y_meas - hff_work->y;
689  const float S = hff_work->yP[0][0] + Rpos;
690  const float K1 = hff_work->yP[0][0] * 1/S;
691  const float K2 = hff_work->yP[1][0] * 1/S;
692 
693  hff_work->y = hff_work->y + K1 * y;
694  hff_work->ydot = hff_work->ydot + K2 * y;
695 
696  const float P11 = (1. - K1) * hff_work->yP[0][0];
697  const float P12 = (1. - K1) * hff_work->yP[0][1];
698  const float P21 = -K2 * hff_work->yP[0][0] + hff_work->yP[1][0];
699  const float P22 = -K2 * hff_work->yP[0][1] + hff_work->yP[1][1];
700 
701  hff_work->yP[0][0] = P11;
702  hff_work->yP[0][1] = P12;
703  hff_work->yP[1][0] = P21;
704  hff_work->yP[1][1] = P22;
705 }
706 
707 
708 /*
709  *
710  * Update velocity
711  *
712  *
713 
714  H = [0 1];
715  R = 0.1;
716  // state residual
717  yd = vx - H * Xm;
718  // covariance residual
719  S = H*Pm*H' + R;
720  // kalman gain
721  K = Pm*H'*inv(S);
722  // update state
723  Xp = Xm + K*yd;
724  // update covariance
725  Pp = Pm - K*H*Pm;
726 */
727 void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel) {
728  b2_hff_update_xdot(&b2_hff_state, vel.x, Rvel.x);
729  b2_hff_update_ydot(&b2_hff_state, vel.y, Rvel.y);
730 }
731 
732 static void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel) {
733  b2_hff_xd_meas = vel;
734 
735  const float yd = vel - hff_work->xdot;
736  const float S = hff_work->xP[1][1] + Rvel;
737  const float K1 = hff_work->xP[0][1] * 1/S;
738  const float K2 = hff_work->xP[1][1] * 1/S;
739 
740  hff_work->x = hff_work->x + K1 * yd;
741  hff_work->xdot = hff_work->xdot + K2 * yd;
742 
743  const float P11 = -K1 * hff_work->xP[1][0] + hff_work->xP[0][0];
744  const float P12 = -K1 * hff_work->xP[1][1] + hff_work->xP[0][1];
745  const float P21 = (1. - K2) * hff_work->xP[1][0];
746  const float P22 = (1. - K2) * hff_work->xP[1][1];
747 
748  hff_work->xP[0][0] = P11;
749  hff_work->xP[0][1] = P12;
750  hff_work->xP[1][0] = P21;
751  hff_work->xP[1][1] = P22;
752 }
753 
754 static void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel) {
755  b2_hff_yd_meas = vel;
756 
757  const float yd = vel - hff_work->ydot;
758  const float S = hff_work->yP[1][1] + Rvel;
759  const float K1 = hff_work->yP[0][1] * 1/S;
760  const float K2 = hff_work->yP[1][1] * 1/S;
761 
762  hff_work->y = hff_work->y + K1 * yd;
763  hff_work->ydot = hff_work->ydot + K2 * yd;
764 
765  const float P11 = -K1 * hff_work->yP[1][0] + hff_work->yP[0][0];
766  const float P12 = -K1 * hff_work->yP[1][1] + hff_work->yP[0][1];
767  const float P21 = (1. - K2) * hff_work->yP[1][0];
768  const float P22 = (1. - K2) * hff_work->yP[1][1];
769 
770  hff_work->yP[0][0] = P11;
771  hff_work->yP[0][1] = P12;
772  hff_work->yP[1][0] = P21;
773  hff_work->yP[1][1] = P22;
774 }
#define Q
Definition: hf_float.c:74
unsigned short uint16_t
Definition: types.h:16
static float b2_hff_x_meas
Definition: hf_float.c:132
#define HFF_R_POS
Definition: hf_float.c:79
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
#define SAVE_DONE
Definition: hf_float.c:203
static uint8_t dest[]
Definition: w5100.c:98
#define Qdotdot
Definition: hf_float.c:75
static void b2_hff_update_ydot(struct HfilterFloat *hff_work, float vel, float Rvel)
Definition: hf_float.c:754
Horizontal filter (x,y) to estimate position and velocity.
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:51
float xdot
Definition: hf_float.h:41
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:74
void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
Definition: hf_float.c:570
Periodic telemetry system header (includes downlink utility and generated code).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1007
float yP[HFF_STATE_SIZE][HFF_STATE_SIZE]
Definition: hf_float.h:48
uint8_t lag_counter
Definition: hf_float.h:49
Simple first order low pass filter with bilinear transform.
bool_t rollback
Definition: hf_float.h:50
static int b2_hff_rb_n
ringbuffer fill count
Definition: hf_float.c:192
Butterworth2LowPass_int filter_y
Definition: hf_float.c:106
#define HFF_LOWPASS_CUTOFF_FREQUENCY
Definition: hf_float.c:97
float x
Definition: hf_float.h:39
#define SAVE_NOW
Definition: hf_float.c:201
static void b2_hff_init_x(float init_x, float init_xdot)
Definition: hf_float.c:313
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
static int save_counter
counts down the propagation steps until the filter state is saved again
Definition: hf_float.c:199
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
struct HfilterFloat * b2_hff_rb_last
ringbuffer read pointer
Definition: hf_float.c:191
static void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time, int32_t value)
Init a second order Butterworth filter.
#define HFF_R_SPEED_MIN
Definition: hf_float.c:89
float xdotdot
Definition: hf_float.h:42
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
#define HFF_STATE_SIZE
Definition: hf_float.h:36
static float b2_hff_xd_meas
Definition: hf_float.c:128
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
float Rgps_pos
Definition: hf_float.c:110
static float b2_hff_y_meas
Definition: hf_float.c:133
#define FALSE
Definition: imu_chimu.h:141
float ydot
Definition: hf_float.h:45
#define PRINT_DBG(_l, _p)
Definition: hf_float.c:46
static float b2_hff_xdd_meas
Definition: hf_float.c:124
Device independent GPS code (interface)
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
Definition: hf_float.c:265
static uint16_t b2_hff_lost_counter
Definition: hf_float.c:207
static int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, int32_t value)
Update second order Butterworth low pass filter state with a new value(fixed point version)...
static uint16_t b2_hff_lost_limit
Definition: hf_float.c:206
#define HFF_LOST_LIMIT
Definition: hf_float.c:205
float xP[HFF_STATE_SIZE][HFF_STATE_SIZE]
Definition: hf_float.h:47
#define ACCEL_FLOAT_OF_BFP(_ai)
Inertial Measurement Unit interface.
struct HfilterFloat b2_hff_state
Definition: hf_float.c:120
#define TRUE
Definition: imu_chimu.h:144
static int past_save_counter
Definition: hf_float.c:200
#define SAVING
Definition: hf_float.c:202
uint32_t pacc
position accuracy in cm
Definition: gps.h:73
float ydotdot
Definition: hf_float.h:46
float y
Definition: hf_float.h:43
API to get/set the generic vehicle states.
static void b2_hff_update_x(struct HfilterFloat *hff_work, float x_meas, float Rpos)
Definition: hf_float.c:663
void b2_hff_propagate(void)
Definition: hf_float.c:453
static void b2_hff_update_xdot(struct HfilterFloat *hff_work, float vel, float Rvel)
Definition: hf_float.c:732
static void b2_hff_update_y(struct HfilterFloat *hff_work, float y_meas, float Rpos)
Definition: hf_float.c:685
#define DT_HFILTER
Definition: hf_float.c:66
void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
Definition: hf_float.c:512
static void b2_hff_propagate_x(struct HfilterFloat *hff_work)
Definition: hf_float.c:604
static void b2_hff_propagate_y(struct HfilterFloat *hff_work)
Definition: hf_float.c:621
rotation matrix
Butterworth2LowPass_int filter_x
Definition: hf_float.c:105
static void b2_hff_init_y(float init_y, float init_ydot)
Definition: hf_float.c:324
float Rgps_vel
Definition: hf_float.c:110
void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
Definition: hf_float.c:658
#define HFF_R_POS_MIN
Definition: hf_float.c:82
#define HFF_R_SPEED
Definition: hf_float.c:86
static int lag_counter_err
by how many steps the estimated GPS validity point in time differed from GPS_LAG_N ...
Definition: hf_float.c:196
static int b2_hff_ps_counter
counter for hff propagation
Definition: hf_float.c:136
static float b2_hff_ydd_meas
Definition: hf_float.c:125
struct GpsState gps
global GPS state
Definition: gps.c:41
static float b2_hff_yd_meas
Definition: hf_float.c:129
Butterworth2LowPass_int filter_z
Definition: hf_float.c:107
void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
Definition: hf_float.c:727
#define INIT_PXX
initial covariance diagonal
Definition: hf_float.c:69