Directory: | ./ |
---|---|
File: | src/filters/causal-filter.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 56 | 75 | 74.7% |
Branches: | 70 | 172 | 40.7% |
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 | 1 | m_filter_order_m(filter_numerator.size()), | |
43 | 1 | m_filter_order_n(filter_denominator.size()), | |
44 | 1 | m_filter_numerator(filter_numerator), | |
45 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | m_filter_denominator(filter_denominator), |
46 | 1 | m_first_sample(true), | |
47 | 1 | m_pt_numerator(0), | |
48 | 1 | m_pt_denominator(0), | |
49 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | m_input_buffer(Eigen::MatrixXd::Zero(xSize, filter_numerator.size())), |
50 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | m_output_buffer( |
51 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | Eigen::MatrixXd::Zero(xSize, filter_denominator.size() - 1)) { |
52 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | assert(timestep > 0.0 && "Timestep should be > 0"); |
53 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | assert(m_filter_numerator.size() == m_filter_order_m); |
54 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
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/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (m_first_sample) { |
61 |
4/6✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
✓ Branch 7 taken 1 times.
|
8 | for (int i = 0; i < m_filter_order_m; i++) m_input_buffer.col(i) = base_x; |
62 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
|
7 | for (int i = 0; i < m_filter_order_n - 1; i++) |
63 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | m_output_buffer.col(i) = |
64 |
5/10✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
|
12 | base_x * m_filter_numerator.sum() / m_filter_denominator.sum(); |
65 | 1 | m_first_sample = false; | |
66 | } | ||
67 | |||
68 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | m_input_buffer.col(m_pt_numerator) = base_x; |
69 | |||
70 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd b(m_filter_order_m); |
71 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd a(m_filter_order_n - 1); |
72 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | b.head(m_pt_numerator + 1) = |
73 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | m_filter_numerator.head(m_pt_numerator + 1).reverse(); |
74 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | b.tail(m_filter_order_m - m_pt_numerator - 1) = |
75 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | m_filter_numerator.tail(m_filter_order_m - m_pt_numerator - 1).reverse(); |
76 | |||
77 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | a.head(m_pt_denominator + 1) = |
78 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | m_filter_denominator.segment(1, m_pt_denominator + 1).reverse(); |
79 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | a.tail(m_filter_order_n - m_pt_denominator - 2) = |
80 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | m_filter_denominator.tail(m_filter_order_n - m_pt_denominator - 2) |
81 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | .reverse(); |
82 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | x_output_dx_ddx.head(m_x_size) = |
83 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
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/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | (m_pt_denominator == 0) ? m_filter_order_n - 2 : m_pt_denominator - 1; |
88 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | x_output_dx_ddx.segment(m_x_size, m_x_size) = |
89 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | (x_output_dx_ddx.head(m_x_size) - m_output_buffer.col(m_pt_denominator)) / |
90 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | m_dt; |
91 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | x_output_dx_ddx.tail(m_x_size) = |
92 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | (x_output_dx_ddx.head(m_x_size) - |
93 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | 2 * m_output_buffer.col(m_pt_denominator) + |
94 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | m_output_buffer.col(m_pt_denominator_prev)) / |
95 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
3 | m_dt / m_dt; |
96 | |||
97 | 1 | m_pt_numerator = | |
98 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
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/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | ? (m_pt_denominator + 1) |
101 | : 0; | ||
102 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | m_output_buffer.col(m_pt_denominator) = x_output_dx_ddx.head(m_x_size); |
103 | 2 | return; | |
104 | 1 | } | |
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 | } | ||
135 |