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