Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
1e_filter.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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 
32 #ifndef ONE_EURO_FILTER_H
33 #define ONE_EURO_FILTER_H
34 
35 #include "std.h"
36 #include <math.h>
37 
43  float hatx;
44  float hatxprev;
45  bool first_time;
46 };
47 
51 struct OneEuroFilter {
52  bool first_time;
53  float rate;
54  float mincutoff;
55  float beta;
56  float dcutoff;
57  struct OneEuroLPFilter xfilt;
58  struct OneEuroLPFilter dxfilt;
60 };
61 
65 static inline void init_1e_filter(struct OneEuroFilter *filter, float rate, float mincutoff, float beta, float dcutoff)
66 {
67  filter->first_time = true;
68  filter->rate = rate;
69  filter->mincutoff = mincutoff;
70  filter->beta = beta;
71  filter->dcutoff = dcutoff;
72  filter->xfilt.hatx = 0.f;
73  filter->xfilt.hatxprev = 0.f;
74  filter->xfilt.first_time = true;
75  filter->dxfilt.hatx = 0.f;
76  filter->dxfilt.hatxprev = 0.f;
77  filter->dxfilt.first_time = true;
78  filter->last_time = 0;
79 }
80 
84 static inline void reset_1e_filter(struct OneEuroFilter *filter)
85 {
86  filter->first_time = true;
87  filter->xfilt.hatx = 0.f;
88  filter->xfilt.hatxprev = 0.f;
89  filter->xfilt.first_time = true;
90  filter->dxfilt.hatx = 0.f;
91  filter->dxfilt.hatxprev = 0.f;
92  filter->dxfilt.first_time = true;
93  filter->last_time = 0;
94 }
95 
99 static inline float compute_1e_filter_lp(struct OneEuroLPFilter *filter, float x, float alpha)
100 {
101  if (filter->first_time) {
102  filter->first_time = false;
103  filter->hatxprev = x;
104  }
105  filter->hatx = alpha * x + (1.f - alpha) * filter->hatxprev;
106  filter->hatxprev = filter->hatx;
107  return filter->hatx;
108 }
109 
113 static inline float compute_1e_filter_alpha(struct OneEuroFilter *filter, float cutoff)
114 {
115  float tau = 1.0f / (2.f * M_PI * cutoff);
116  float te = 1.0f / filter->rate;
117  return 1.0f / (1.0f + tau / te);
118 
119 }
120 
124 static inline float update_1e_filter(struct OneEuroFilter *filter, float x)
125 {
126  float dx;
127  if (filter->first_time) {
128  filter->first_time = false;
129  dx = 0.f;
130  } else {
131  dx = (x - filter->xfilt.hatxprev) * filter->rate;
132  }
133  float edx = compute_1e_filter_lp(&(filter->dxfilt), dx, compute_1e_filter_alpha(filter, filter->dcutoff));
134  float cutoff = filter->mincutoff + filter->beta * fabsf(edx);
135  return compute_1e_filter_lp(&(filter->xfilt), x, compute_1e_filter_alpha(filter, cutoff));
136 }
137 
142 static inline float update_1e_filter_at_time(struct OneEuroFilter *filter, float x, uint32_t timestamp)
143 {
144  if (filter->last_time != 0 && timestamp != filter->last_time) {
145  filter->rate = 1e6f / (float)(timestamp - filter->last_time); // timestamp in us
146  }
147  filter->last_time = timestamp;
148  return update_1e_filter(filter, x);
149 }
150 
151 #endif
152 
float mincutoff
min cutoff freq (in Hz)
Definition: 1e_filter.h:54
static float compute_1e_filter_lp(struct OneEuroLPFilter *filter, float x, float alpha)
Filter a float using the given low-pass filter and the given alpha value.
Definition: 1e_filter.h:99
static float update_1e_filter_at_time(struct OneEuroFilter *filter, float x, uint32_t timestamp)
Filter a float using the given One Euro Filter and the given timestamp.
Definition: 1e_filter.h:142
bool first_time
first time flag
Definition: 1e_filter.h:52
struct OneEuroLPFilter xfilt
low pass filter
Definition: 1e_filter.h:57
float dcutoff
derivative cutoff freq (in Hz)
Definition: 1e_filter.h:56
static void reset_1e_filter(struct OneEuroFilter *filter)
Reset filter (gains and parameters unchanged)
Definition: 1e_filter.h:84
float hatxprev
Definition: 1e_filter.h:44
static void init_1e_filter(struct OneEuroFilter *filter, float rate, float mincutoff, float beta, float dcutoff)
Initialize a 1 Euro Filter instance.
Definition: 1e_filter.h:65
struct OneEuroLPFilter dxfilt
low pass filter for derivative
Definition: 1e_filter.h:58
uint32_t last_time
last update time in ms
Definition: 1e_filter.h:59
static float compute_1e_filter_alpha(struct OneEuroFilter *filter, float cutoff)
Compute Alpha for a given One Euro Filter and a given cutoff frequency.
Definition: 1e_filter.h:113
static float update_1e_filter(struct OneEuroFilter *filter, float x)
Filter a float using the given One Euro Filter.
Definition: 1e_filter.h:124
float beta
cutoff slope
Definition: 1e_filter.h:55
float rate
data update rate (in Hz)
Definition: 1e_filter.h:53
configuration parameters.
Definition: 1e_filter.h:51
simple low pass filter.
Definition: 1e_filter.h:42
float alpha
Definition: textons.c:133
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78