Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
hf_float.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  * Copyright (C) 2009 Felix Ruess <felix.ruess@gmail.com>
6  *
7  * This file is part of paparazzi.
8  *
9  * paparazzi is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2, or (at your option)
12  * any later version.
13  *
14  * paparazzi is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with paparazzi; see the file COPYING. If not, write to
21  * the Free Software Foundation, 59 Temple Place - Suite 330,
22  * Boston, MA 02111-1307, USA.
23  */
24 
26 #include "subsystems/ins.h"
27 #include "subsystems/imu.h"
28 #include "subsystems/ahrs.h"
29 #include "subsystems/gps.h"
30 #include <stdlib.h>
31 
32 #include "generated/airframe.h"
33 
34 #ifdef SITL
35 #include <stdio.h>
36 #define DBG_LEVEL 1
37 #define PRINT_DBG(_l, _p) { \
38  if (DBG_LEVEL >= _l) \
39  printf _p; \
40 }
41 #else
42 #define PRINT_DBG(_l, _p) {}
43 #endif
44 
45 
46 /* initial covariance diagonal */
47 #define INIT_PXX 1.
48 /* process noise (is the same for x and y)*/
49 #ifndef HFF_ACCEL_NOISE
50 #define HFF_ACCEL_NOISE 0.5
51 #endif
52 #define Q HFF_ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
53 #define Qdotdot HFF_ACCEL_NOISE*DT_HFILTER
54 
55 //TODO: proper measurement noise
56 #ifndef HFF_R_POS
57 #define HFF_R_POS 8.
58 #endif
59 #ifndef HFF_R_POS_MIN
60 #define HFF_R_POS_MIN 3.
61 #endif
62 
63 #ifndef HFF_R_SPEED
64 #define HFF_R_SPEED 2.
65 #endif
66 #ifndef HFF_R_SPEED_MIN
67 #define HFF_R_SPEED_MIN 1.
68 #endif
69 
70 /* gps measurement noise */
72 
73 /*
74 
75  X_x = [ x xdot]
76  X_y = [ y ydot]
77 
78 
79 */
80 /* output filter states */
82 
83 
84 /* last acceleration measurement */
87 
88 /* last velocity measurement */
91 
92 /* last position measurement */
95 
96 /* counter for hff propagation*/
98 
99 
100 /*
101  * accel(in body frame) buffer
102  */
103 #define ACC_RB_MAXN 64
104 struct AccBuf {
106  int r; /* pos to read from, oldest measurement */
107  int w; /* pos to write to */
108  int n; /* number of elements in rb */
109  int size;
110 };
113 
116  acc_body.w = (acc_body.w + 1) < acc_body.size ? (acc_body.w + 1) : 0;
117 
118  /* once the buffer is full it always has the last acc_body.size accel measurements */
119  if (acc_body.n < acc_body.size) {
120  acc_body.n++;
121  } else {
122  acc_body.r = (acc_body.r + 1) < acc_body.size ? (acc_body.r + 1) : 0;
123  }
124 }
125 
126 /* compute the mean of the last n accel measurements */
128  struct Int32Vect3 sum;
129  int i, j;
130 
131  INT_VECT3_ZERO(sum);
132 
133  if (n > 1) {
134  if (n > acc_body.n) {
135  n = acc_body.n;
136  }
137  for (i = 1; i <= n; i++) {
138  j = (acc_body.w - i) > 0 ? (acc_body.w - i) : (acc_body.w - i + acc_body.size);
139  VECT3_ADD(sum, acc_body.buf[j]);
140  }
141  VECT3_SDIV(acc_body_mean, sum, n);
142  } else {
144  }
145 }
146 
147 /*
148  * For GPS lag compensation
149  *
150  *
151  *
152  */
153 #ifdef GPS_LAG
154 /*
155  * GPS_LAG is defined in seconds in airframe file
156  */
157 
158 /* number of propagaton steps to redo according to GPS_LAG */
159 #define GPS_LAG_N ((int) (GPS_LAG * HFF_FREQ + 0.5))
160 /* number of propagation steps between two GPS updates */
161 #define GPS_DT_N ((int) (HFF_FREQ / 4))
162 /* tolerance of the GPS lag accuracy is +- GPS_LAG_TOLERANCE seconds */
163 #define GPS_LAG_TOLERANCE 0.08
164 #define GPS_LAG_TOL_N ((int) (GPS_LAG_TOLERANCE * HFF_FREQ + 0.5))
165 
166 /* maximum number of past propagation steps to rerun per ap cycle
167  * make sure GPS_LAG_N/MAX_PP_STEPS < 128
168  * GPS_LAG_N/MAX_PP_STEPS/512 = seconds until re-propagation catches up with the present
169  */
170 #define MAX_PP_STEPS 6
171 
172 /* variables for mean accel buffer */
173 #define ACC_BUF_MAXN (GPS_LAG_N+10)
174 #define INC_ACC_IDX(idx) { idx = (idx + 1) < ACC_BUF_MAXN ? (idx + 1) : 0; }
175 
176 struct FloatVect2 past_accel[ACC_BUF_MAXN]; /* buffer with past mean accel values for redoing the propagation */
177 unsigned int acc_buf_r; /* pos to read from, oldest measurement */
178 unsigned int acc_buf_w; /* pos to write to */
179 unsigned int acc_buf_n; /* number of elements in buffer */
180 
181 
182 /*
183  * stuff for ringbuffer to store past filter states
184  */
185 #define HFF_RB_MAXN ((int) (GPS_LAG * 4))
186 #define INC_RB_POINTER(ptr) { \
187  if (ptr == &b2_hff_rb[HFF_RB_MAXN-1]) \
188  ptr = b2_hff_rb; \
189  else \
190  ptr++; \
191  }
192 
193 struct HfilterFloat b2_hff_rb[HFF_RB_MAXN]; /* ringbuffer with state and covariance when GPS was valid */
194 struct HfilterFloat *b2_hff_rb_put; /* write pointer */
195 #endif /* GPS_LAG */
196 
197 struct HfilterFloat *b2_hff_rb_last; /* read pointer */
198 int b2_hff_rb_n; /* fill count */
199 
200 
201 /* by how many steps the estimated GPS validity point in time differed from GPS_LAG_N */
203 
204 /* counts down the propagation steps until the filter state is saved again */
207 #define SAVE_NOW 0
208 #define SAVING -1
209 #define SAVE_DONE -2
210 
213 
214 #ifdef GPS_LAG
215 static inline void b2_hff_get_past_accel(unsigned int back_n);
216 static inline void b2_hff_rb_put_state(struct HfilterFloat* source);
217 static inline void b2_hff_rb_drop_last(void);
218 static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source);
219 #endif
220 
221 
222 
223 static inline void b2_hff_init_x(float init_x, float init_xdot);
224 static inline void b2_hff_init_y(float init_y, float init_ydot);
225 
226 static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work);
227 static inline void b2_hff_propagate_y(struct HfilterFloat* hff_work);
228 
229 static inline void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, float Rpos);
230 static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, float Rpos);
231 
232 static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel);
233 static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel);
234 
235 
236 
237 void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
240  b2_hff_init_x(init_x, init_xdot);
241  b2_hff_init_y(init_y, init_ydot);
242  /* init buffer for mean accel calculation */
243  acc_body.r = 0;
244  acc_body.w = 0;
245  acc_body.n = 0;
247 #ifdef GPS_LAG
248  /* init buffer for past mean accel values */
249  acc_buf_r = 0;
250  acc_buf_w = 0;
251  acc_buf_n = 0;
252  b2_hff_rb_put = b2_hff_rb;
253  b2_hff_rb_last = b2_hff_rb;
254  b2_hff_rb_last->rollback = FALSE;
255  b2_hff_rb_last->lag_counter = 0;
256  b2_hff_state.lag_counter = GPS_LAG_N;
257 #ifdef SITL
258  printf("GPS_LAG: %f\n", GPS_LAG);
259  printf("GPS_LAG_N: %d\n", GPS_LAG_N);
260  printf("GPS_DT_N: %d\n", GPS_DT_N);
261  printf("DT_HFILTER: %f\n", DT_HFILTER);
262  printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N);
263 #endif
264 #else
265  b2_hff_rb_last = &b2_hff_state;
267 #endif
268  b2_hff_rb_n = 0;
270  lag_counter_err = 0;
271  save_counter = -1;
273  b2_hff_ps_counter = 1;
276 }
277 
278 static inline void b2_hff_init_x(float init_x, float init_xdot) {
279  b2_hff_state.x = init_x;
280  b2_hff_state.xdot = init_xdot;
281  int i, j;
282  for (i=0; i<HFF_STATE_SIZE; i++) {
283  for (j=0; j<HFF_STATE_SIZE; j++)
284  b2_hff_state.xP[i][j] = 0.;
285  b2_hff_state.xP[i][i] = INIT_PXX;
286  }
287 
288 }
289 
290 static inline void b2_hff_init_y(float init_y, float init_ydot) {
291  b2_hff_state.y = init_y;
292  b2_hff_state.ydot = init_ydot;
293  int i, j;
294  for (i=0; i<HFF_STATE_SIZE; i++) {
295  for (j=0; j<HFF_STATE_SIZE; j++)
296  b2_hff_state.yP[i][j] = 0.;
297  b2_hff_state.yP[i][i] = INIT_PXX;
298  }
299 }
300 
301 #ifdef GPS_LAG
302 static inline void b2_hff_store_accel_ltp(float x, float y) {
303  past_accel[acc_buf_w].x = x;
304  past_accel[acc_buf_w].y = y;
305  INC_ACC_IDX(acc_buf_w);
306 
307  if (acc_buf_n < ACC_BUF_MAXN) {
308  acc_buf_n++;
309  } else {
310  INC_ACC_IDX(acc_buf_r);
311  }
312 }
313 
314 /* get the accel values from back_n steps ago */
315 static inline void b2_hff_get_past_accel(unsigned int back_n) {
316  int i;
317  if (back_n > acc_buf_n) {
318  PRINT_DBG(1, ("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n));
319  back_n = acc_buf_n;
320  } else if (back_n == 0) {
321  PRINT_DBG(1, ("Cannot go back zero steps!\n"));
322  return;
323  }
324  if ((int)(acc_buf_w - back_n) < 0)
325  i = acc_buf_w - back_n + ACC_BUF_MAXN;
326  else
327  i = acc_buf_w - back_n;
328  b2_hff_xdd_meas = past_accel[i].x;
329  b2_hff_ydd_meas = past_accel[i].y;
330  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));
331 }
332 
333 static inline void b2_hff_rb_put_state(struct HfilterFloat* source) {
334  /* copy state from source into buffer */
335  b2_hff_set_state(b2_hff_rb_put, source);
336  b2_hff_rb_put->lag_counter = 0;
337  b2_hff_rb_put->rollback = FALSE;
338 
339  /* forward write pointer */
340  INC_RB_POINTER(b2_hff_rb_put);
341 
342  /* increase fill count and forward last pointer if neccessary */
343  if (b2_hff_rb_n < HFF_RB_MAXN) {
344  b2_hff_rb_n++;
345  } else {
346  INC_RB_POINTER(b2_hff_rb_last);
347  }
348  PRINT_DBG(2, ("put state. fill count now: %d\n", b2_hff_rb_n));
349 }
350 
351 static inline void b2_hff_rb_drop_last(void) {
352  if (b2_hff_rb_n > 0) {
353  INC_RB_POINTER(b2_hff_rb_last);
354  b2_hff_rb_n--;
355  } else {
356  PRINT_DBG(2, ("hff ringbuffer empty!\n"));
357  b2_hff_rb_last->lag_counter = 0;
358  b2_hff_rb_last->rollback = FALSE;
359  }
360  PRINT_DBG(2, ("drop last state. fill count now: %d\n", b2_hff_rb_n));
361 }
362 
363 
364 /* copy source state to dest state */
365 static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source) {
366  dest->x = source->x;
367  dest->xdot = source->xdot;
368  dest->xdotdot = source->xdotdot;
369  dest->y = source->y;
370  dest->ydot = source->ydot;
371  dest->ydotdot = source->ydotdot;
372  for (int i=0; i < HFF_STATE_SIZE; i++) {
373  for (int j=0; j < HFF_STATE_SIZE; j++) {
374  dest->xP[i][j] = source->xP[i][j];
375  dest->yP[i][j] = source->yP[i][j];
376  }
377  }
378 }
379 
380 static inline void b2_hff_propagate_past(struct HfilterFloat* hff_past) {
381  PRINT_DBG(1, ("enter propagate past: %d\n", hff_past->lag_counter));
382  /* run max MAX_PP_STEPS propagation steps */
383  for (int i=0; i < MAX_PP_STEPS; i++) {
384  if (hff_past->lag_counter > 0) {
385  b2_hff_get_past_accel(hff_past->lag_counter);
386  PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter));
387  b2_hff_propagate_x(hff_past);
388  b2_hff_propagate_y(hff_past);
389  hff_past->lag_counter--;
390 
391  if (past_save_counter > 0) {
393  PRINT_DBG(2, ("dec past_save_counter: %d\n", past_save_counter));
394  } else if (past_save_counter == SAVE_NOW) {
395  /* next GPS measurement valid at this state -> save */
396  PRINT_DBG(2, ("save past state\n"));
397  b2_hff_rb_put_state(hff_past);
399  } else if (past_save_counter == SAVING) {
400  /* increase lag counter on if next state is already saved */
401  if (hff_past == &b2_hff_rb[HFF_RB_MAXN-1])
402  b2_hff_rb[0].lag_counter++;
403  else
404  (hff_past+1)->lag_counter++;
405  }
406  }
407 
408  /* finished re-propagating the past values */
409  if (hff_past->lag_counter == 0) {
410  b2_hff_set_state(&b2_hff_state, hff_past);
411  b2_hff_rb_drop_last();
413  break;
414  }
415  }
416 }
417 #endif /* GPS_LAG */
418 
419 
420 
421 void b2_hff_propagate(void) {
424 
425 #ifdef GPS_LAG
426  /* continue re-propagating to catch up with the present */
427  if (b2_hff_rb_last->rollback) {
428  b2_hff_propagate_past(b2_hff_rb_last);
429  }
430 #endif
431 
432  /* store body accelerations for mean computation */
434 
435  /* propagate current state if it is time */
436  if (b2_hff_ps_counter == HFF_PRESCALER) {
437  b2_hff_ps_counter = 1;
438 
440  /* compute float ltp mean acceleration */
441  b2_hff_compute_accel_body_mean(HFF_PRESCALER);
442  struct Int32Vect3 mean_accel_ltp;
444  b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
445  b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
446 #ifdef GPS_LAG
447  b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
448 #endif
449 
450  /*
451  * propagate current state
452  */
455 
456  /* update ins state from horizontal filter */
463 
464 #ifdef GPS_LAG
465  /* increase lag counter on last saved state */
466  if (b2_hff_rb_n > 0)
467  b2_hff_rb_last->lag_counter++;
468 
469  /* save filter state if needed */
470  if (save_counter == 0) {
471  PRINT_DBG(1, ("save current state\n"));
472  b2_hff_rb_put_state(&b2_hff_state);
473  save_counter = -1;
474  } else if (save_counter > 0) {
475  save_counter--;
476  }
477 #endif
478  }
479  } else {
481  }
482 }
483 
484 
485 
486 
487 void b2_hff_update_gps(void) {
489 
490 #if USE_GPS_ACC4R
491  Rgps_pos = (float) gps.pacc / 100.;
492  if (Rgps_pos < HFF_R_POS_MIN)
494 
495  Rgps_vel = (float) gps.sacc / 100.;
498 #endif
499 
500 #ifdef GPS_LAG
501  if (GPS_LAG_N == 0) {
502 #endif
503 
504  /* update filter state with measurement */
505  b2_hff_update_x(&b2_hff_state, ins_gps_pos_m_ned.x, Rgps_pos);
506  b2_hff_update_y(&b2_hff_state, ins_gps_pos_m_ned.y, Rgps_pos);
507 #ifdef HFF_UPDATE_SPEED
508  b2_hff_update_xdot(&b2_hff_state, ins_gps_speed_m_s_ned.x, Rgps_vel);
509  b2_hff_update_ydot(&b2_hff_state, ins_gps_speed_m_s_ned.y, Rgps_vel);
510 #endif
511 
512  /* update ins state */
519 
520 #ifdef GPS_LAG
521  } else if (b2_hff_rb_n > 0) {
522  /* roll back if state was saved approx when GPS was valid */
523  lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N;
524  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));
525  if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
526  b2_hff_rb_last->rollback = TRUE;
527  b2_hff_update_x(b2_hff_rb_last, ins_gps_pos_m_ned.x, Rgps_pos);
528  b2_hff_update_y(b2_hff_rb_last, ins_gps_pos_m_ned.y, Rgps_pos);
529 #ifdef HFF_UPDATE_SPEED
530  b2_hff_update_xdot(b2_hff_rb_last, ins_gps_speed_m_s_ned.x, Rgps_vel);
531  b2_hff_update_ydot(b2_hff_rb_last, ins_gps_speed_m_s_ned.y, Rgps_vel);
532 #endif
533  past_save_counter = GPS_DT_N-1;// + lag_counter_err;
534  PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter));
535  b2_hff_propagate_past(b2_hff_rb_last);
536  } else if (lag_counter_err >= GPS_DT_N - (GPS_LAG_TOL_N+1)) {
537  /* apparently missed a GPS update, try next saved state */
538  PRINT_DBG(2, ("try next saved state\n"));
539  b2_hff_rb_drop_last();
541  }
542  } else if (save_counter < 0) {
543  /* ringbuffer empty -> save output filter state at next GPS validity point in time */
544  save_counter = GPS_DT_N-1 - (GPS_LAG_N % GPS_DT_N);
545  PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
546  }
547 
548 #endif /* GPS_LAG */
549 }
550 
551 
552 void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel) {
553  b2_hff_state.x = pos.x;
554  b2_hff_state.y = pos.y;
555  b2_hff_state.xdot = vel.x;
556  b2_hff_state.ydot = vel.y;
557 #ifdef GPS_LAG
558  while (b2_hff_rb_n > 0) {
559  b2_hff_rb_drop_last();
560  }
561  save_counter = -1;
563 #endif
564 }
565 
566 
567 /*
568  *
569  * Propagation
570  *
571  *
572 
573  F = [ 1 dt
574  0 1 ];
575 
576  B = [ dt^2/2 dt]';
577 
578  Q = [ 0.01 0
579  0 0.01];
580 
581  Xk1 = F * Xk0 + B * accel;
582 
583  Pk1 = F * Pk0 * F' + Q;
584 
585 */
586 static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work) {
587  /* update state */
588  hff_work->xdotdot = b2_hff_xdd_meas;
589  hff_work->x = hff_work->x + DT_HFILTER * hff_work->xdot + DT_HFILTER*DT_HFILTER/2 * hff_work->xdotdot;
590  hff_work->xdot = hff_work->xdot + DT_HFILTER * hff_work->xdotdot;
591  /* update covariance */
592  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] );
593  const float FPF01 = hff_work->xP[0][1] + DT_HFILTER * hff_work->xP[1][1];
594  const float FPF10 = hff_work->xP[1][0] + DT_HFILTER * hff_work->xP[1][1];
595  const float FPF11 = hff_work->xP[1][1];
596 
597  hff_work->xP[0][0] = FPF00 + Q;
598  hff_work->xP[0][1] = FPF01;
599  hff_work->xP[1][0] = FPF10;
600  hff_work->xP[1][1] = FPF11 + Qdotdot;
601 }
602 
603 static inline void b2_hff_propagate_y(struct HfilterFloat* hff_work) {
604  /* update state */
605  hff_work->ydotdot = b2_hff_ydd_meas;
606  hff_work->y = hff_work->y + DT_HFILTER * hff_work->ydot + DT_HFILTER*DT_HFILTER/2 * hff_work->ydotdot;
607  hff_work->ydot = hff_work->ydot + DT_HFILTER * hff_work->ydotdot;
608  /* update covariance */
609  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] );
610  const float FPF01 = hff_work->yP[0][1] + DT_HFILTER * hff_work->yP[1][1];
611  const float FPF10 = hff_work->yP[1][0] + DT_HFILTER * hff_work->yP[1][1];
612  const float FPF11 = hff_work->yP[1][1];
613 
614  hff_work->yP[0][0] = FPF00 + Q;
615  hff_work->yP[0][1] = FPF01;
616  hff_work->yP[1][0] = FPF10;
617  hff_work->yP[1][1] = FPF11 + Qdotdot;
618 }
619 
620 
621 /*
622  *
623  * Update position
624  *
625  *
626 
627  H = [1 0];
628  R = 0.1;
629  // state residual
630  y = pos_measurement - H * Xm;
631  // covariance residual
632  S = H*Pm*H' + R;
633  // kalman gain
634  K = Pm*H'*inv(S);
635  // update state
636  Xp = Xm + K*y;
637  // update covariance
638  Pp = Pm - K*H*Pm;
639 */
640 void b2_hff_update_pos (struct FloatVect2 pos, struct FloatVect2 Rpos) {
641  b2_hff_update_x(&b2_hff_state, pos.x, Rpos.x);
642  b2_hff_update_y(&b2_hff_state, pos.y, Rpos.y);
643 }
644 
645 static inline void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, float Rpos) {
646  b2_hff_x_meas = x_meas;
647 
648  const float y = x_meas - hff_work->x;
649  const float S = hff_work->xP[0][0] + Rpos;
650  const float K1 = hff_work->xP[0][0] * 1/S;
651  const float K2 = hff_work->xP[1][0] * 1/S;
652 
653  hff_work->x = hff_work->x + K1 * y;
654  hff_work->xdot = hff_work->xdot + K2 * y;
655 
656  const float P11 = (1. - K1) * hff_work->xP[0][0];
657  const float P12 = (1. - K1) * hff_work->xP[0][1];
658  const float P21 = -K2 * hff_work->xP[0][0] + hff_work->xP[1][0];
659  const float P22 = -K2 * hff_work->xP[0][1] + hff_work->xP[1][1];
660 
661  hff_work->xP[0][0] = P11;
662  hff_work->xP[0][1] = P12;
663  hff_work->xP[1][0] = P21;
664  hff_work->xP[1][1] = P22;
665 }
666 
667 static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, float Rpos) {
668  b2_hff_y_meas = y_meas;
669 
670  const float y = y_meas - hff_work->y;
671  const float S = hff_work->yP[0][0] + Rpos;
672  const float K1 = hff_work->yP[0][0] * 1/S;
673  const float K2 = hff_work->yP[1][0] * 1/S;
674 
675  hff_work->y = hff_work->y + K1 * y;
676  hff_work->ydot = hff_work->ydot + K2 * y;
677 
678  const float P11 = (1. - K1) * hff_work->yP[0][0];
679  const float P12 = (1. - K1) * hff_work->yP[0][1];
680  const float P21 = -K2 * hff_work->yP[0][0] + hff_work->yP[1][0];
681  const float P22 = -K2 * hff_work->yP[0][1] + hff_work->yP[1][1];
682 
683  hff_work->yP[0][0] = P11;
684  hff_work->yP[0][1] = P12;
685  hff_work->yP[1][0] = P21;
686  hff_work->yP[1][1] = P22;
687 }
688 
689 
690 
691 
692 /*
693  *
694  * Update velocity
695  *
696  *
697 
698  H = [0 1];
699  R = 0.1;
700  // state residual
701  yd = vx - H * Xm;
702  // covariance residual
703  S = H*Pm*H' + R;
704  // kalman gain
705  K = Pm*H'*inv(S);
706  // update state
707  Xp = Xm + K*yd;
708  // update covariance
709  Pp = Pm - K*H*Pm;
710 */
711 void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel) {
712  b2_hff_update_xdot(&b2_hff_state, vel.x, Rvel.x);
713  b2_hff_update_ydot(&b2_hff_state, vel.y, Rvel.y);
714 }
715 
716 static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel) {
717  b2_hff_xd_meas = vel;
718 
719  const float yd = vel - hff_work->xdot;
720  const float S = hff_work->xP[1][1] + Rvel;
721  const float K1 = hff_work->xP[0][1] * 1/S;
722  const float K2 = hff_work->xP[1][1] * 1/S;
723 
724  hff_work->x = hff_work->x + K1 * yd;
725  hff_work->xdot = hff_work->xdot + K2 * yd;
726 
727  const float P11 = -K1 * hff_work->xP[1][0] + hff_work->xP[0][0];
728  const float P12 = -K1 * hff_work->xP[1][1] + hff_work->xP[0][1];
729  const float P21 = (1. - K2) * hff_work->xP[1][0];
730  const float P22 = (1. - K2) * hff_work->xP[1][1];
731 
732  hff_work->xP[0][0] = P11;
733  hff_work->xP[0][1] = P12;
734  hff_work->xP[1][0] = P21;
735  hff_work->xP[1][1] = P22;
736 }
737 
738 static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel) {
739  b2_hff_yd_meas = vel;
740 
741  const float yd = vel - hff_work->ydot;
742  const float S = hff_work->yP[1][1] + Rvel;
743  const float K1 = hff_work->yP[0][1] * 1/S;
744  const float K2 = hff_work->yP[1][1] * 1/S;
745 
746  hff_work->y = hff_work->y + K1 * yd;
747  hff_work->ydot = hff_work->ydot + K2 * yd;
748 
749  const float P11 = -K1 * hff_work->yP[1][0] + hff_work->yP[0][0];
750  const float P12 = -K1 * hff_work->yP[1][1] + hff_work->yP[0][1];
751  const float P21 = (1. - K2) * hff_work->yP[1][0];
752  const float P22 = (1. - K2) * hff_work->yP[1][1];
753 
754  hff_work->yP[0][0] = P11;
755  hff_work->yP[0][1] = P12;
756  hff_work->yP[1][0] = P21;
757  hff_work->yP[1][1] = P22;
758 }
#define Q
Definition: hf_float.c:52
unsigned short uint16_t
Definition: types.h:16
float b2_hff_x_meas
Definition: hf_float.c:93
#define HFF_R_POS
Definition: hf_float.c:57
int n
Definition: hf_float.c:108
#define SAVE_DONE
Definition: hf_float.c:209
Attitude and Heading Reference System interface.
#define Qdotdot
Definition: hf_float.c:53
static void b2_hff_compute_accel_body_mean(uint8_t n)
Definition: hf_float.c:127
static void b2_hff_update_ydot(struct HfilterFloat *hff_work, float vel, float Rvel)
Definition: hf_float.c:738
float xdot
Definition: hf_float.h:55
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:73
void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
Definition: hf_float.c:552
float yP[HFF_STATE_SIZE][HFF_STATE_SIZE]
Definition: hf_float.h:62
struct AccBuf acc_body
Definition: hf_float.c:111
uint8_t lag_counter
Definition: hf_float.h:63
#define ACCEL_BFP_OF_REAL(_af)
struct Int32RMat body_to_imu_rmat
rotation from body to imu frame as a rotation matrix
Definition: imu.h:52
bool_t rollback
Definition: hf_float.h:64
int b2_hff_rb_n
Definition: hf_float.c:198
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
float x
Definition: hf_float.h:53
struct NedCoor_i ins_ltp_accel
Definition: ins.c:79
#define SAVE_NOW
Definition: hf_float.c:207
static void b2_hff_init_x(float init_x, float init_xdot)
Definition: hf_float.c:278
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
int save_counter
Definition: hf_float.c:205
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
Device independent INS code.
struct HfilterFloat * b2_hff_rb_last
Definition: hf_float.c:197
#define HFF_R_SPEED_MIN
Definition: hf_float.c:67
float xdotdot
Definition: hf_float.h:56
#define HFF_STATE_SIZE
Definition: hf_float.h:30
float b2_hff_xd_meas
Definition: hf_float.c:89
struct NedCoor_i ins_ltp_pos
Definition: ins.c:77
float Rgps_pos
Definition: hf_float.c:71
float b2_hff_y_meas
Definition: hf_float.c:94
int w
Definition: hf_float.c:107
#define FALSE
Definition: imu_chimu.h:141
struct Int32RMat ltp_to_body_rmat
Rotation from LocalTangentPlane to body frame as Rotation Matrix.
Definition: ahrs.h:51
float ydot
Definition: hf_float.h:59
#define PRINT_DBG(_l, _p)
Definition: hf_float.c:42
float b2_hff_xdd_meas
Definition: hf_float.c:85
Device independent GPS code (interface)
struct NedCoor_i ins_ltp_speed
Definition: ins.c:78
#define DT_HFILTER
Definition: hf_float.h:48
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
Definition: hf_float.c:237
uint16_t b2_hff_lost_counter
Definition: hf_float.c:212
uint16_t b2_hff_lost_limit
Definition: hf_float.c:211
float xP[HFF_STATE_SIZE][HFF_STATE_SIZE]
Definition: hf_float.h:61
void b2_hff_update_gps(void)
Definition: hf_float.c:487
int size
Definition: hf_float.c:109
#define INT_VECT3_ZERO(_v)
#define ACCEL_FLOAT_OF_BFP(_ai)
Inertial Measurement Unit interface.
#define HFF_LOST_LIMIT
Definition: hf_float.h:83
struct HfilterFloat b2_hff_state
Definition: hf_float.c:81
struct Int32Vect3 buf[ACC_RB_MAXN]
Definition: hf_float.c:105
#define TRUE
Definition: imu_chimu.h:144
void b2_hff_store_accel_body(void)
Definition: hf_float.c:114
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:120
int past_save_counter
Definition: hf_float.c:206
#define SAVING
Definition: hf_float.c:208
uint32_t pacc
position accuracy in cm
Definition: gps.h:72
float ydotdot
Definition: hf_float.h:60
unsigned char uint8_t
Definition: types.h:14
float y
Definition: hf_float.h:57
static void b2_hff_update_x(struct HfilterFloat *hff_work, float x_meas, float Rpos)
Definition: hf_float.c:645
void b2_hff_propagate(void)
Definition: hf_float.c:421
static void b2_hff_update_xdot(struct HfilterFloat *hff_work, float vel, float Rvel)
Definition: hf_float.c:716
static void b2_hff_update_y(struct HfilterFloat *hff_work, float y_meas, float Rpos)
Definition: hf_float.c:667
static void b2_hff_propagate_x(struct HfilterFloat *hff_work)
Definition: hf_float.c:586
#define ACC_RB_MAXN
Definition: hf_float.c:103
struct Int32Vect3 acc_body_mean
Definition: hf_float.c:112
static void b2_hff_propagate_y(struct HfilterFloat *hff_work)
Definition: hf_float.c:603
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:113
static void b2_hff_init_y(float init_y, float init_ydot)
Definition: hf_float.c:290
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
float Rgps_vel
Definition: hf_float.c:71
void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
Definition: hf_float.c:640
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:169
#define HFF_R_POS_MIN
Definition: hf_float.c:60
#define HFF_R_SPEED
Definition: hf_float.c:64
int lag_counter_err
Definition: hf_float.c:202
#define SPEED_BFP_OF_REAL(_af)
int b2_hff_ps_counter
Definition: hf_float.c:97
float b2_hff_ydd_meas
Definition: hf_float.c:86
struct GpsState gps
global GPS state
Definition: gps.c:31
float b2_hff_yd_meas
Definition: hf_float.c:90
int r
Definition: hf_float.c:106
void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
Definition: hf_float.c:711
#define INIT_PXX
Definition: hf_float.c:47
#define POS_BFP_OF_REAL(_af)