filters.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2  * BSD 3-Clause License
3  *
4  * Copyright (c) 2021, Qiayuan Liao
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright notice, this
11  * list of conditions and the following disclaimer.
12  *
13  * * Redistributions in binary form must reproduce the above copyright notice,
14  * this list of conditions and the following disclaimer in the documentation
15  * and/or other materials provided with the distribution.
16  *
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE
25  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
26  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
30  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  *******************************************************************************/
33 
34 #include "rm_common/filters/filters.h"
35 #include "rm_common/math_utilities.h"
36 #include <cmath>
37 #include <cstring>
38 
39 template <typename T>
40 MovingAverageFilter<T>::MovingAverageFilter(int num_data) : num_data_(num_data), idx_(0), sum_(0.0)
41 {
42  buffer_ = new T[num_data_];
43  memset((void*)buffer_, 0.0, sizeof(T) * num_data_);
44 }
45 
46 template <typename T>
47 void MovingAverageFilter<T>::input(T input_value)
48 {
49  sum_ -= buffer_[idx_];
50  sum_ += input_value;
51  buffer_[idx_] = input_value;
52  ++idx_;
53  idx_ %= num_data_;
54 }
55 
56 template <typename T>
57 T MovingAverageFilter<T>::output()
58 {
59  return sum_ / num_data_;
60 }
61 
62 template <typename T>
63 void MovingAverageFilter<T>::clear()
64 {
65  sum_ = 0.0;
66  memset((void*)buffer_, 0.0, sizeof(T) * num_data_);
67 }
68 
69 template <typename T>
70 MovingAverageFilter<T>::~MovingAverageFilter()
71 {
72  delete[] buffer_;
73 }
74 
75 template class MovingAverageFilter<double>;
76 template class MovingAverageFilter<float>;
77 
78 /*============================================================================*/
79 
80 template <typename T>
81 ButterworthFilter<T>::ButterworthFilter(int num_sample, T dt, T cutoff_frequency)
82 {
83  mNumSample_ = num_sample;
84  mDt_ = dt;
85  mCutoffFreq_ = cutoff_frequency;
86 
87  mpBuffer_ = new T[num_sample];
88  memset((void*)mpBuffer_, 0, sizeof(T) * num_sample);
89 
90  mCurIdx_ = 0;
91 }
92 
93 template <typename T>
94 ButterworthFilter<T>::~ButterworthFilter()
95 {
96  delete[] mpBuffer_;
97 }
98 
99 template <typename T>
100 void ButterworthFilter<T>::input(T input_value)
101 {
102  int j;
103  T sqrt_2 = sqrt(2.);
104  T value = 0;
105  for (j = mNumSample_ - 2; j >= 0; j--)
106  {
107  mpBuffer_[j + 1] = mpBuffer_[j];
108  }
109 
110  mpBuffer_[0] = input_value;
111  for (j = 0; j < mNumSample_; j++)
112  {
113  T t = (T)j * mDt_;
114  value += sqrt_2 / mCutoffFreq_ * mpBuffer_[j] * exp(-1. / sqrt_2 * t) * sin(mCutoffFreq_ / sqrt_2 * t) * mDt_;
115  }
116  mValue_ = value;
117 }
118 
119 template <typename T>
120 T ButterworthFilter<T>::output()
121 {
122  return mValue_;
123 }
124 
125 template <typename T>
126 void ButterworthFilter<T>::clear()
127 {
128  for (int i(0); i < mNumSample_; ++i)
129  {
130  mpBuffer_[i] = 0.0;
131  }
132 }
133 
134 template class ButterworthFilter<double>;
135 template class ButterworthFilter<float>;
136 
137 /*============================================================================*/
138 
139 template <typename T>
140 DigitalLpFilter<T>::DigitalLpFilter(T w_c, T t_s)
141 {
142  Lpf_in_prev_[0] = Lpf_in_prev_[1] = 0;
143  Lpf_out_prev_[0] = Lpf_out_prev_[1] = 0;
144  Lpf_in1_ = 0, Lpf_in2_ = 0, Lpf_in3_ = 0, Lpf_out1_ = 0, Lpf_out2_ = 0;
145  float den = 2500 * t_s * t_s * w_c * w_c + 7071 * t_s * w_c + 10000;
146 
147  Lpf_in1_ = 2500 * t_s * t_s * w_c * w_c / den;
148  Lpf_in2_ = 5000 * t_s * t_s * w_c * w_c / den;
149  Lpf_in3_ = 2500 * t_s * t_s * w_c * w_c / den;
150  Lpf_out1_ = -(5000 * t_s * t_s * w_c * w_c - 20000) / den;
151  Lpf_out2_ = -(2500 * t_s * t_s * w_c * w_c - 7071 * t_s * w_c + 10000) / den;
152 }
153 
154 template <typename T>
155 DigitalLpFilter<T>::~DigitalLpFilter() = default;
156 
157 template <typename T>
158 void DigitalLpFilter<T>::input(T lpf_in)
159 {
160  lpf_out_ = Lpf_in1_ * lpf_in + Lpf_in2_ * Lpf_in_prev_[0] + Lpf_in3_ * Lpf_in_prev_[1] + // input component
161  Lpf_out1_ * Lpf_out_prev_[0] + Lpf_out2_ * Lpf_out_prev_[1]; // output component
162  Lpf_in_prev_[1] = Lpf_in_prev_[0];
163  Lpf_in_prev_[0] = lpf_in;
164  Lpf_out_prev_[1] = Lpf_out_prev_[0];
165  Lpf_out_prev_[0] = lpf_out_;
166 }
167 
168 template <typename T>
169 T DigitalLpFilter<T>::output()
170 {
171  return lpf_out_;
172 }
173 
174 template <typename T>
175 void DigitalLpFilter<T>::clear()
176 {
177  Lpf_in_prev_[1] = 0;
178  Lpf_in_prev_[0] = 0;
179  Lpf_out_prev_[1] = 0;
180  Lpf_out_prev_[0] = 0;
181 }
182 
183 template class DigitalLpFilter<double>;
184 template class DigitalLpFilter<float>;
185 
186 /*============================================================================*/
187 
188 template <typename T>
189 DerivLpFilter<T>::DerivLpFilter(T w_c, T t_s)
190 {
191  Lpf_in_prev_[0] = 0;
192  Lpf_in_prev_[1] = 0;
193  Lpf_out_prev_[0] = 0;
194  Lpf_out_prev_[1] = 0;
195  Lpf_in1_ = 0;
196  Lpf_in2_ = 0;
197  Lpf_in3_ = 0;
198  Lpf_out1_ = 0;
199  Lpf_out2_ = 0;
200  T a = 1.4142;
201  T den = 4 + 2 * a * w_c * t_s + t_s * t_s * w_c * w_c;
202 
203  Lpf_in1_ = 2 * t_s * w_c * w_c / den;
204  Lpf_in2_ = 0;
205  Lpf_in3_ = -2. * t_s * w_c * w_c / den;
206  Lpf_out1_ = -1. * (-8 + t_s * t_s * w_c * w_c * 2) / den;
207  Lpf_out2_ = -1. * (4 - 2 * a * w_c * t_s + t_s * t_s * w_c * w_c) / den;
208  lpf_out_ = 0.0;
209  clear();
210 }
211 
212 template <typename T>
213 DerivLpFilter<T>::~DerivLpFilter() = default;
214 
215 template <typename T>
216 void DerivLpFilter<T>::input(T lpf_in)
217 {
218  // static int i(0);
219  lpf_out_ = Lpf_in1_ * lpf_in + Lpf_in2_ * Lpf_in_prev_[0] + Lpf_in3_ * Lpf_in_prev_[1] + // input component
220  Lpf_out1_ * Lpf_out_prev_[0] + Lpf_out2_ * Lpf_out_prev_[1]; // output component
221 
222  Lpf_in_prev_[1] = Lpf_in_prev_[0];
223  Lpf_in_prev_[0] = lpf_in;
224  Lpf_out_prev_[1] = Lpf_out_prev_[0];
225  Lpf_out_prev_[0] = lpf_out_;
226  // ++i;
227 }
228 
229 template <typename T>
230 T DerivLpFilter<T>::output()
231 {
232  return lpf_out_;
233 }
234 
235 template <typename T>
236 void DerivLpFilter<T>::clear()
237 {
238  Lpf_in_prev_[1] = 0;
239  Lpf_in_prev_[0] = 0;
240  Lpf_out_prev_[1] = 0;
241  Lpf_out_prev_[0] = 0;
242 }
243 
244 template class DerivLpFilter<double>;
245 template class DerivLpFilter<float>;
246 
247 /*============================================================================*/
248 
249 template <typename T>
250 FF01Filter<T>::FF01Filter(float t_s, float w_c)
251 {
252  Lpf_in_prev_[0] = Lpf_in_prev_[1] = 0;
253  Lpf_out_prev_[0] = Lpf_out_prev_[1] = 0;
254  Lpf_in1_ = 0, Lpf_in2_ = 0, Lpf_in3_ = 0, Lpf_out1_ = 0, Lpf_out2_ = 0;
255  T a = 1.4142;
256  T den = 4 + 2 * a * w_c * t_s + t_s * t_s * w_c * w_c;
257  T J = 0.00008;
258  T B = 0.0002;
259 
260  Lpf_in1_ = B * t_s * t_s * w_c * w_c + 2 * J * t_s * w_c * w_c;
261  Lpf_in2_ = 2 * B * t_s * t_s * w_c * w_c;
262  Lpf_in3_ = B * t_s * t_s * w_c * w_c - 2 * J * t_s * w_c * w_c;
263  Lpf_out1_ = -1. * (-8 + t_s * t_s * w_c * w_c * 2) / den;
264  Lpf_out2_ = -1. * (4 - 2 * a * w_c * t_s + t_s * t_s * w_c * w_c) / den;
265 }
266 
267 template <typename T>
268 FF01Filter<T>::~FF01Filter() = default;
269 
270 template <typename T>
271 void FF01Filter<T>::input(T lpf_in)
272 {
273  lpf_out_ = Lpf_in1_ * lpf_in + Lpf_in2_ * Lpf_in_prev_[0] + Lpf_in3_ * Lpf_in_prev_[1] + // input component
274  Lpf_out1_ * Lpf_out_prev_[0] + Lpf_out2_ * Lpf_out_prev_[1]; // output component
275  Lpf_in_prev_[1] = Lpf_in_prev_[0];
276  Lpf_in_prev_[0] = lpf_in;
277  Lpf_out_prev_[1] = Lpf_out_prev_[0];
278  Lpf_out_prev_[0] = lpf_out_;
279 }
280 
281 template <typename T>
282 T FF01Filter<T>::output()
283 {
284  return lpf_out_;
285 }
286 
287 template <typename T>
288 void FF01Filter<T>::clear()
289 {
290  Lpf_in_prev_[1] = 0;
291  Lpf_in_prev_[0] = 0;
292  Lpf_out_prev_[1] = 0;
293  Lpf_out_prev_[0] = 0;
294 }
295 
296 template class FF01Filter<float>;
297 template class FF01Filter<double>;
298 
299 /*============================================================================*/
300 
301 template <typename T>
302 FF02Filter<T>::FF02Filter(float t_s, float w_c)
303 {
304  T J = 0.003216;
305 
306  Lpf_in_prev_[0] = Lpf_in_prev_[1] = 0;
307  Lpf_out_prev_[0] = Lpf_out_prev_[1] = 0;
308  Lpf_in1_ = 0, Lpf_in2_ = 0, Lpf_in3_ = 0, Lpf_out1_ = 0, Lpf_out2_ = 0;
309 
310  T a = 1.4142;
311  T den = 4 + 2 * a * w_c * t_s + t_s * t_s * w_c * w_c;
312 
313  Lpf_in1_ = J * 2 * t_s * w_c * w_c / den;
314  Lpf_in2_ = 0;
315  Lpf_in3_ = -2. * J * t_s * w_c * w_c / den;
316  Lpf_out1_ = -1. * (-8 + t_s * t_s * w_c * w_c * 2) / den;
317  Lpf_out2_ = -1. * (4 - 2 * a * w_c * t_s + t_s * t_s * w_c * w_c) / den;
318 
319  clear();
320 }
321 
322 template <typename T>
323 FF02Filter<T>::~FF02Filter() = default;
324 
325 template <typename T>
326 void FF02Filter<T>::input(T lpf_in)
327 {
328  lpf_out_ = Lpf_in1_ * lpf_in + Lpf_in2_ * Lpf_in_prev_[0] + Lpf_in3_ * Lpf_in_prev_[1] + // input component
329  Lpf_out1_ * Lpf_out_prev_[0] + Lpf_out2_ * Lpf_out_prev_[1]; // output component
330  Lpf_in_prev_[0] = lpf_in;
331  Lpf_in_prev_[1] = Lpf_in_prev_[0];
332  Lpf_out_prev_[0] = lpf_out_;
333  Lpf_out_prev_[1] = Lpf_out_prev_[0];
334 }
335 
336 template <typename T>
337 T FF02Filter<T>::output()
338 {
339  return lpf_out_;
340 }
341 
342 template <typename T>
343 void FF02Filter<T>::clear()
344 {
345  Lpf_in_prev_[1] = 0;
346  Lpf_in_prev_[0] = 0;
347  Lpf_out_prev_[1] = 0;
348  Lpf_out_prev_[0] = 0;
349 }
350 
351 template class FF02Filter<float>;
352 template class FF02Filter<double>;
353 
354 /*============================================================================*/
355 
356 template <typename T>
357 AverageFilter<T>::AverageFilter(T dt, T t_const, T limit) : dt_(dt), t_const_(t_const), limit_(limit)
358 {
359  est_value_ = 0.;
360 }
361 
362 template <typename T>
363 AverageFilter<T>::~AverageFilter()
364 {
365  est_value_ = 0;
366 }
367 
368 template <typename T>
369 void AverageFilter<T>::clear()
370 {
371  est_value_ = 0.;
372 }
373 
374 template <typename T>
375 void AverageFilter<T>::input(T input)
376 {
377  T update_value = input - est_value_;
378  if (fabs(update_value) > limit_)
379  {
380  update_value = 0.;
381  }
382  est_value_ += (dt_ / (dt_ + t_const_)) * update_value;
383 }
384 
385 template <typename T>
386 T AverageFilter<T>::output()
387 {
388  return est_value_;
389 }
390 
391 template class AverageFilter<float>;
392 template class AverageFilter<double>;
393 
394 /*============================================================================*/
395 
396 template <typename T>
397 RampFilter<T>::RampFilter(T acc, T dt)
398 {
399  acc_ = acc;
400  dt_ = dt;
401  RampFilter::clear();
402 }
403 
404 template <typename T>
405 void RampFilter<T>::input(T input_value)
406 {
407  last_value_ += minAbs(input_value - last_value_, acc_ * dt_);
408 }
409 
410 template <typename T>
411 void RampFilter<T>::clear()
412 {
413  last_value_ = 0.;
414 }
415 
416 template <typename T>
417 void RampFilter<T>::clear(T last_value)
418 {
419  last_value_ = last_value;
420 }
421 
422 template <typename T>
423 void RampFilter<T>::setAcc(T acc)
424 {
425  acc_ = acc;
426 }
427 
428 template <typename T>
429 T RampFilter<T>::output()
430 {
431  return last_value_;
432 }
433 
434 template class RampFilter<float>;
435 template class RampFilter<double>;
436 
437 /*============================================================================*/
438 
439 template <typename T>
440 OneEuroFilter<T>::OneEuroFilter(double _freq, T _mincutoff, T _beta, T _dcutoff)
441  : freq(_freq), mincutoff(_mincutoff), beta(_beta), dcutoff(_dcutoff)
442 {
443  firsttime = true;
444  x_prev = 0;
445  hatxprev = 0;
446  dhatxprev = 0;
447  filtered_val = 0;
448 };
449 
450 template <typename T>
451 OneEuroFilter<T>::~OneEuroFilter() = default;
452 
453 template <typename T>
454 void OneEuroFilter<T>::input(T input_value)
455 {
456  T dx = 0;
457  if (firsttime)
458  dhatxprev = dx;
459  else
460  dx = (input_value - x_prev) * freq;
461  T edx = alpha(dcutoff, freq) * dx + (1 - alpha(dcutoff, freq)) * dhatxprev;
462  dhatxprev = edx;
463  T cutoff = mincutoff + beta * std::abs(static_cast<double>(edx));
464 
465  if (firsttime)
466  hatxprev = input_value;
467  filtered_val = alpha(cutoff, freq) * input_value + (1 - alpha(cutoff, freq)) * hatxprev;
468  hatxprev = filtered_val;
469  firsttime = false;
470 }
471 
472 template <typename T>
473 T OneEuroFilter<T>::output()
474 {
475  return filtered_val;
476 }
477 
478 template <typename T>
479 void OneEuroFilter<T>::clear()
480 {
481  firsttime = true;
482  x_prev = 0;
483  hatxprev = 0;
484  dhatxprev = 0;
485 }
486 
487 template class OneEuroFilter<float>;
488 template class OneEuroFilter<double>;
OneEuroFilter::clear
void clear()
Definition: filters.cpp:479
DigitalLpFilter::~DigitalLpFilter
~DigitalLpFilter()
RampFilter
Definition: filters.h:208
DigitalLpFilter::clear
void clear()
Definition: filters.cpp:175
AverageFilter::output
T output()
Definition: filters.cpp:386
MovingAverageFilter::clear
void clear()
Definition: filters.cpp:63
MovingAverageFilter
Definition: filters.h:87
DerivLpFilter::~DerivLpFilter
~DerivLpFilter()
DigitalLpFilter::DigitalLpFilter
DigitalLpFilter(T w_c, T t_s)
Definition: filters.cpp:140
OneEuroFilter::output
T output()
Definition: filters.cpp:473
ButterworthFilter::output
T output()
Definition: filters.cpp:120
FF01Filter::clear
virtual void clear()
Definition: filters.cpp:288
RampFilter::setAcc
void setAcc(T acc)
Definition: filters.cpp:423
minAbs
T minAbs(T a, T b)
Definition: math_utilities.h:54
ButterworthFilter
Definition: filters.h:51
MovingAverageFilter::input
void input(T input_value)
Definition: filters.cpp:47
math_utilities.h
OneEuroFilter::~OneEuroFilter
~OneEuroFilter()
AverageFilter::~AverageFilter
~AverageFilter()
Definition: filters.cpp:363
FF01Filter::input
virtual void input(T input_value)
Definition: filters.cpp:271
ButterworthFilter::input
void input(T input_value)
Definition: filters.cpp:100
AverageFilter::AverageFilter
AverageFilter(T dt, T t_const, T limit)
Definition: filters.cpp:357
MovingAverageFilter::~MovingAverageFilter
~MovingAverageFilter()
Definition: filters.cpp:70
filters.h
AverageFilter
Definition: filters.h:191
ButterworthFilter::~ButterworthFilter
~ButterworthFilter()
Definition: filters.cpp:94
OneEuroFilter::x_prev
T x_prev
Definition: filters.h:239
OneEuroFilter
Definition: filters.h:226
OneEuroFilter::dhatxprev
T dhatxprev
Definition: filters.h:239
RampFilter::RampFilter
RampFilter(T acc, T dt)
Definition: filters.cpp:397
FF01Filter::output
virtual T output()
Definition: filters.cpp:282
FF02Filter::FF02Filter
FF02Filter(float t_s, float w_c)
Definition: filters.cpp:302
DigitalLpFilter::output
T output()
Definition: filters.cpp:169
DerivLpFilter::input
void input(T input_value)
Definition: filters.cpp:216
AverageFilter::input
void input(T input_value)
Definition: filters.cpp:375
ButterworthFilter::ButterworthFilter
ButterworthFilter(int num_sample, T dt, T cutoff_frequency)
Definition: filters.cpp:81
RampFilter::clear
void clear()
Definition: filters.cpp:411
OneEuroFilter::input
void input(T input_value)
Definition: filters.cpp:454
FF01Filter::FF01Filter
FF01Filter(float t_s, float w_c)
Definition: filters.cpp:250
OneEuroFilter::OneEuroFilter
OneEuroFilter(double _freq, T _mincutoff, T _beta, T _dcutoff)
Definition: filters.cpp:440
FF02Filter::input
void input(T input_value)
Definition: filters.cpp:326
AverageFilter::clear
void clear()
Definition: filters.cpp:369
FF01Filter
Definition: filters.h:157
FF02Filter::output
T output()
Definition: filters.cpp:337
DigitalLpFilter
Definition: filters.h:70
RampFilter::output
T output()
Definition: filters.cpp:429
AverageFilter::est_value_
T est_value_
Definition: filters.h:201
ButterworthFilter::clear
void clear()
Definition: filters.cpp:126
FF01Filter::~FF01Filter
virtual ~FF01Filter()
DerivLpFilter::DerivLpFilter
DerivLpFilter(T w_c, T t_s)
Definition: filters.cpp:189
DerivLpFilter::clear
void clear()
Definition: filters.cpp:236
RampFilter::input
void input(T input_value)
Definition: filters.cpp:405
MovingAverageFilter::buffer_
T * buffer_
Definition: filters.h:97
OneEuroFilter::firsttime
bool firsttime
Definition: filters.h:237
FF02Filter
Definition: filters.h:174
DigitalLpFilter::input
void input(T input_value)
Definition: filters.cpp:158
FF02Filter::clear
void clear()
Definition: filters.cpp:343
MovingAverageFilter::num_data_
int num_data_
Definition: filters.h:98
DerivLpFilter
Definition: filters.h:140
FF02Filter::~FF02Filter
~FF02Filter()
OneEuroFilter::hatxprev
T hatxprev
Definition: filters.h:239
MovingAverageFilter::output
T output()
Definition: filters.cpp:57
OneEuroFilter::filtered_val
T filtered_val
Definition: filters.h:240
DerivLpFilter::output
T output()
Definition: filters.cpp:230
MovingAverageFilter::MovingAverageFilter
MovingAverageFilter(int num_data)
Definition: filters.cpp:40
t
geometry_msgs::TransformStamped t
alpha
T alpha(T cutoff, double freq)
Definition: math_utilities.h:73

rm_common
Author(s):
autogenerated on Tue May 6 2025 02:23:36