1 |
|
|
#ifndef _SOT_CORE_CAUSAL_FILTER_H_ |
2 |
|
|
#define _SOT_CORE_CAUSAL_FILTER_H_ |
3 |
|
|
/* |
4 |
|
|
* Copyright 2017-, Rohan Budhirja, LAAS-CNRS |
5 |
|
|
* |
6 |
|
|
* This file is part of sot-torque-control. |
7 |
|
|
* sot-torque-control is free software: you can redistribute it and/or |
8 |
|
|
* modify it under the terms of the GNU Lesser General Public License |
9 |
|
|
* as published by the Free Software Foundation, either version 3 of |
10 |
|
|
* the License, or (at your option) any later version. |
11 |
|
|
* sot-torque-control is distributed in the hope that it will be |
12 |
|
|
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
13 |
|
|
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
14 |
|
|
* GNU Lesser General Public License for more details. You should |
15 |
|
|
* have received a copy of the GNU Lesser General Public License along |
16 |
|
|
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>. |
17 |
|
|
*/ |
18 |
|
|
|
19 |
|
|
/* --------------------------------------------------------------------- */ |
20 |
|
|
/* --- INCLUDE --------------------------------------------------------- */ |
21 |
|
|
/* --------------------------------------------------------------------- */ |
22 |
|
|
#include <Eigen/Core> |
23 |
|
|
|
24 |
|
|
/** \addtogroup Filters |
25 |
|
|
\section subsec_causalfilter CausalFilter |
26 |
|
|
Filter data with an IIR or FIR filter. |
27 |
|
|
|
28 |
|
|
Filter a data sequence, \f$x\f$, using a digital filter. |
29 |
|
|
The filter is a direct form II transposed implementation |
30 |
|
|
of the standard difference equation. |
31 |
|
|
This means that the filter implements: |
32 |
|
|
|
33 |
|
|
\f$ a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)] |
34 |
|
|
- a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)] \f$ |
35 |
|
|
|
36 |
|
|
where \f$m\f$ is the degree of the numerator, |
37 |
|
|
\f$n\f$ is the degree of the denominator, |
38 |
|
|
and \f$N\f$ is the sample number |
39 |
|
|
|
40 |
|
|
|
41 |
|
|
*/ |
42 |
|
|
namespace dynamicgraph { |
43 |
|
|
namespace sot { |
44 |
|
|
|
45 |
|
|
class CausalFilter { |
46 |
|
|
public: |
47 |
|
2 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
48 |
|
|
|
49 |
|
|
/** --- CONSTRUCTOR ---- |
50 |
|
|
\param[in] timestep |
51 |
|
|
\param[in] xSize |
52 |
|
|
\param[in] filter_numerator |
53 |
|
|
\param[in] filter_denominator |
54 |
|
|
|
55 |
|
|
xSize is |
56 |
|
|
*/ |
57 |
|
|
CausalFilter(const double ×tep, const int &xSize, |
58 |
|
|
const Eigen::VectorXd &filter_numerator, |
59 |
|
|
const Eigen::VectorXd &filter_denominator); |
60 |
|
|
|
61 |
|
|
void get_x_dx_ddx(const Eigen::VectorXd &base_x, |
62 |
|
|
Eigen::VectorXd &x_output_dx_ddx); |
63 |
|
|
|
64 |
|
|
void switch_filter(const Eigen::VectorXd &filter_numerator, |
65 |
|
|
const Eigen::VectorXd &filter_denominator); |
66 |
|
|
|
67 |
|
|
private: |
68 |
|
|
/// sampling timestep of the input signal |
69 |
|
|
double m_dt; |
70 |
|
|
/// Size |
71 |
|
|
int m_x_size; |
72 |
|
|
/// Size of the numerator \f$m\f$ |
73 |
|
|
Eigen::VectorXd::Index m_filter_order_m; |
74 |
|
|
/// Size of the denominator \f$n\f$ |
75 |
|
|
Eigen::VectorXd::Index m_filter_order_n; |
76 |
|
|
|
77 |
|
|
/// Coefficients of the numerator \f$b\f$ |
78 |
|
|
Eigen::VectorXd m_filter_numerator; |
79 |
|
|
/// Coefficients of the denominator \f$a\f$ |
80 |
|
|
Eigen::VectorXd m_filter_denominator; |
81 |
|
|
bool m_first_sample; |
82 |
|
|
/// |
83 |
|
|
int m_pt_numerator; |
84 |
|
|
int m_pt_denominator; |
85 |
|
|
Eigen::MatrixXd m_input_buffer; |
86 |
|
|
Eigen::MatrixXd m_output_buffer; |
87 |
|
|
}; // class CausalFilter |
88 |
|
|
} // namespace sot |
89 |
|
|
} // namespace dynamicgraph |
90 |
|
|
#endif /* _SOT_CORE_CAUSAL_FILTER_H_ */ |