| 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 |