| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Copyright 2017-, Rohan Budhiraja LAAS-CNRS |
||
3 |
* |
||
4 |
* This file is part of sot-torque-control. |
||
5 |
* sot-torque-control is free software: you can redistribute it and/or |
||
6 |
* modify it under the terms of the GNU Lesser General Public License |
||
7 |
* as published by the Free Software Foundation, either version 3 of |
||
8 |
* the License, or (at your option) any later version. |
||
9 |
* sot-torque-control is distributed in the hope that it will be |
||
10 |
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
||
11 |
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
12 |
* GNU Lesser General Public License for more details. You should |
||
13 |
* have received a copy of the GNU Lesser General Public License along |
||
14 |
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>. |
||
15 |
*/ |
||
16 |
|||
17 |
#include <iostream> |
||
18 |
#include <sot/core/causal-filter.hh> |
||
19 |
|||
20 |
using namespace dynamicgraph::sot; |
||
21 |
/* |
||
22 |
Filter data with an IIR or FIR filter. |
||
23 |
|||
24 |
Filter a data sequence, x, using a digital filter. The filter is a direct form |
||
25 |
II transposed implementation of the standard difference equation. This means |
||
26 |
that the filter implements: |
||
27 |
|||
28 |
a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)] |
||
29 |
- a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)] |
||
30 |
|||
31 |
where m is the degree of the numerator, n is the degree of the denominator, and |
||
32 |
N is the sample number |
||
33 |
|||
34 |
*/ |
||
35 |
|||
36 |
1 |
CausalFilter::CausalFilter(const double ×tep, const int &xSize, |
|
37 |
const Eigen::VectorXd &filter_numerator, |
||
38 |
1 |
const Eigen::VectorXd &filter_denominator) |
|
39 |
|||
40 |
1 |
: m_dt(timestep), |
|
41 |
1 |
m_x_size(xSize), |
|
42 |
2 |
m_filter_order_m(filter_numerator.size()), |
|
43 |
2 |
m_filter_order_n(filter_denominator.size()), |
|
44 |
m_filter_numerator(filter_numerator), |
||
45 |
m_filter_denominator(filter_denominator), |
||
46 |
m_first_sample(true), |
||
47 |
m_pt_numerator(0), |
||
48 |
m_pt_denominator(0), |
||
49 |
✓✗ | 1 |
m_input_buffer(Eigen::MatrixXd::Zero(xSize, filter_numerator.size())), |
50 |
m_output_buffer( |
||
51 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
Eigen::MatrixXd::Zero(xSize, filter_denominator.size() - 1)) { |
52 |
✗✓ | 1 |
assert(timestep > 0.0 && "Timestep should be > 0"); |
53 |
✓✗✗✓ |
1 |
assert(m_filter_numerator.size() == m_filter_order_m); |
54 |
✓✗✗✓ |
1 |
assert(m_filter_denominator.size() == m_filter_order_n); |
55 |
1 |
} |
|
56 |
|||
57 |
1 |
void CausalFilter::get_x_dx_ddx(const Eigen::VectorXd &base_x, |
|
58 |
Eigen::VectorXd &x_output_dx_ddx) { |
||
59 |
// const dynamicgraph::Vector &base_x = m_xSIN(iter); |
||
60 |
✓✗ | 1 |
if (m_first_sample) { |
61 |
✓✓✓✗ ✓✗ |
8 |
for (int i = 0; i < m_filter_order_m; i++) m_input_buffer.col(i) = base_x; |
62 |
✓✓ | 7 |
for (int i = 0; i < m_filter_order_n - 1; i++) |
63 |
✓✗ | 6 |
m_output_buffer.col(i) = |
64 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
12 |
base_x * m_filter_numerator.sum() / m_filter_denominator.sum(); |
65 |
1 |
m_first_sample = false; |
|
66 |
} |
||
67 |
|||
68 |
✓✗✓✗ |
1 |
m_input_buffer.col(m_pt_numerator) = base_x; |
69 |
|||
70 |
✓✗ | 2 |
Eigen::VectorXd b(m_filter_order_m); |
71 |
✓✗ | 2 |
Eigen::VectorXd a(m_filter_order_n - 1); |
72 |
✓✗ | 1 |
b.head(m_pt_numerator + 1) = |
73 |
✓✗✓✗ ✓✗ |
2 |
m_filter_numerator.head(m_pt_numerator + 1).reverse(); |
74 |
✓✗ | 1 |
b.tail(m_filter_order_m - m_pt_numerator - 1) = |
75 |
✓✗✓✗ ✓✗ |
2 |
m_filter_numerator.tail(m_filter_order_m - m_pt_numerator - 1).reverse(); |
76 |
|||
77 |
✓✗ | 1 |
a.head(m_pt_denominator + 1) = |
78 |
✓✗✓✗ ✓✗ |
2 |
m_filter_denominator.segment(1, m_pt_denominator + 1).reverse(); |
79 |
✓✗ | 1 |
a.tail(m_filter_order_n - m_pt_denominator - 2) = |
80 |
✓✗ | 1 |
m_filter_denominator.tail(m_filter_order_n - m_pt_denominator - 2) |
81 |
✓✗✓✗ |
2 |
.reverse(); |
82 |
✓✗ | 1 |
x_output_dx_ddx.head(m_x_size) = |
83 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
(m_input_buffer * b - m_output_buffer * a) / m_filter_denominator[0]; |
84 |
|||
85 |
// Finite Difference |
||
86 |
1 |
Eigen::VectorXd::Index m_pt_denominator_prev = |
|
87 |
✓✗ | 1 |
(m_pt_denominator == 0) ? m_filter_order_n - 2 : m_pt_denominator - 1; |
88 |
✓✗ | 1 |
x_output_dx_ddx.segment(m_x_size, m_x_size) = |
89 |
✓✗✓✗ ✓✗ |
1 |
(x_output_dx_ddx.head(m_x_size) - m_output_buffer.col(m_pt_denominator)) / |
90 |
✓✗✓✗ |
2 |
m_dt; |
91 |
✓✗ | 1 |
x_output_dx_ddx.tail(m_x_size) = |
92 |
✓✗✓✗ |
1 |
(x_output_dx_ddx.head(m_x_size) - |
93 |
✓✗✓✗ ✓✗ |
2 |
2 * m_output_buffer.col(m_pt_denominator) + |
94 |
✓✗ | 1 |
m_output_buffer.col(m_pt_denominator_prev)) / |
95 |
✓✗✓✗ ✓✗ |
3 |
m_dt / m_dt; |
96 |
|||
97 |
1 |
m_pt_numerator = |
|
98 |
✓✗ | 1 |
(m_pt_numerator + 1) < m_filter_order_m ? (m_pt_numerator + 1) : 0; |
99 |
2 |
m_pt_denominator = (m_pt_denominator + 1) < m_filter_order_n - 1 |
|
100 |
✓✗ | 1 |
? (m_pt_denominator + 1) |
101 |
: 0; |
||
102 |
✓✗✓✗ ✓✗ |
1 |
m_output_buffer.col(m_pt_denominator) = x_output_dx_ddx.head(m_x_size); |
103 |
2 |
return; |
|
104 |
} |
||
105 |
|||
106 |
void CausalFilter::switch_filter(const Eigen::VectorXd &filter_numerator, |
||
107 |
const Eigen::VectorXd &filter_denominator) { |
||
108 |
Eigen::VectorXd::Index filter_order_m = filter_numerator.size(); |
||
109 |
Eigen::VectorXd::Index filter_order_n = filter_denominator.size(); |
||
110 |
|||
111 |
Eigen::VectorXd current_x(m_input_buffer.col(m_pt_numerator)); |
||
112 |
|||
113 |
m_input_buffer.resize(Eigen::NoChange, filter_order_m); |
||
114 |
m_output_buffer.resize(Eigen::NoChange, filter_order_n - 1); |
||
115 |
|||
116 |
for (int i = 0; i < filter_order_m; i++) m_input_buffer.col(i) = current_x; |
||
117 |
|||
118 |
for (int i = 0; i < filter_order_n - 1; i++) |
||
119 |
m_output_buffer.col(i) = |
||
120 |
current_x * filter_numerator.sum() / filter_denominator.sum(); |
||
121 |
|||
122 |
m_filter_order_m = filter_order_m; |
||
123 |
m_filter_numerator.resize(filter_order_m); |
||
124 |
m_filter_numerator = filter_numerator; |
||
125 |
|||
126 |
m_filter_order_n = filter_order_n; |
||
127 |
m_filter_denominator.resize(filter_order_n); |
||
128 |
m_filter_denominator = filter_denominator; |
||
129 |
|||
130 |
m_pt_numerator = 0; |
||
131 |
m_pt_denominator = 0; |
||
132 |
|||
133 |
return; |
||
134 |
} |
| Generated by: GCOVR (Version 4.2) |