Commit 72aec306 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[filters] Adding causal-filter.

parent d2f0477b
/*
* Copyright 2017-, Rohan Budhirja, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <Eigen/Core>
/** \addtogroup Filters
\section subsec_causalfilter CausalFilter
Filter data with an IIR or FIR filter.
Filter a data sequence, \f$x\f$, using a digital filter.
The filter is a direct form II transposed implementation
of the standard difference equation.
This means that the filter implements:
\f$ a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)]
- a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)] \f$
where \f$m\f$ is the degree of the numerator,
\f$n\f$ is the degree of the denominator,
and \f$N\f$ is the sample number
*/
class CausalFilter
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/** --- CONSTRUCTOR ----
\param[in] timestep
\param[in] xSize
\param[in] filter_numerator
\param[in] filter_denominator
xSize is
*/
CausalFilter(const double &timestep,
const int& xSize,
const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator);
void get_x_dx_ddx(const Eigen::VectorXd& base_x,
Eigen::VectorXd& x_output_dx_ddx);
void switch_filter(const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator);
private:
/// sampling timestep of the input signal
double m_dt;
/// Size
int m_x_size;
/// Size of the numerator \f$m\f$
Eigen::Index m_filter_order_m;
/// Size of the denominator \f$n\f$
Eigen::Index m_filter_order_n;
/// Coefficients of the numerator \f$b\f$
Eigen::VectorXd m_filter_numerator;
/// Coefficients of the denominator \f$a\f$
Eigen::VectorXd m_filter_denominator;
bool m_first_sample;
///
int m_pt_numerator;
int m_pt_denominator;
Eigen::MatrixXd m_input_buffer;
Eigen::MatrixXd m_output_buffer;
}; // class CausalFilter
/*
* Copyright 2017-, Rohan Budhiraja LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
#include <sot/core/causal-filter.hh>
/*
Filter data with an IIR or FIR filter.
Filter a data sequence, x, using a digital filter. The filter is a direct form II transposed implementation of the standard difference equation.
This means that the filter implements:
a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)]
- a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)]
where m is the degree of the numerator, n is the degree of the denominator, and N is the sample number
*/
CausalFilter::CausalFilter(const double &timestep,
const int& xSize,
const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator)
: m_dt(timestep)
, m_x_size(xSize)
, m_filter_order_m(filter_numerator.size())
, m_filter_order_n(filter_denominator.size())
, m_filter_numerator(filter_numerator)
, m_filter_denominator(filter_denominator)
, m_first_sample(true)
, m_pt_numerator(0)
, m_pt_denominator(0)
, m_input_buffer(Eigen::MatrixXd::Zero(xSize, filter_numerator.size()))
, m_output_buffer(Eigen::MatrixXd::Zero(xSize, filter_denominator.size()-1))
{
assert(timestep>0.0 && "Timestep should be > 0");
assert(m_filter_numerator.size() == m_filter_order_m);
assert(m_filter_denominator.size() == m_filter_order_n);
}
void CausalFilter::get_x_dx_ddx(const Eigen::VectorXd& base_x,
Eigen::VectorXd& x_output_dx_ddx)
{
//const dynamicgraph::Vector &base_x = m_xSIN(iter);
if(m_first_sample)
{
for(int i=0;i<m_filter_order_m; i++)
m_input_buffer.col(i) = base_x;
for(int i=0;i<m_filter_order_n-1; i++)
m_output_buffer.col(i) = base_x*m_filter_numerator.sum()/
m_filter_denominator.sum();
m_first_sample = false;
}
m_input_buffer.col(m_pt_numerator) = base_x;
Eigen::VectorXd b(m_filter_order_m);
Eigen::VectorXd a(m_filter_order_n-1);
b.head(m_pt_numerator+1) = m_filter_numerator.head(m_pt_numerator+1).reverse();
b.tail(m_filter_order_m-m_pt_numerator-1) =
m_filter_numerator.tail(m_filter_order_m-m_pt_numerator-1).reverse();
a.head(m_pt_denominator+1) =
m_filter_denominator.segment(1, m_pt_denominator+1).reverse();
a.tail(m_filter_order_n-m_pt_denominator-2) =
m_filter_denominator.tail(m_filter_order_n-m_pt_denominator-2).reverse();
x_output_dx_ddx.head(m_x_size) =
(m_input_buffer*b-m_output_buffer*a)/m_filter_denominator[0];
//Finite Difference
Eigen::Index m_pt_denominator_prev = (m_pt_denominator == 0) ?
m_filter_order_n-2 : m_pt_denominator-1;
x_output_dx_ddx.segment(m_x_size,m_x_size) =
(x_output_dx_ddx.head(m_x_size)-m_output_buffer.col(m_pt_denominator))/m_dt;
x_output_dx_ddx.tail(m_x_size) =
(x_output_dx_ddx.head(m_x_size)-2*m_output_buffer.col(m_pt_denominator)
+m_output_buffer.col(m_pt_denominator_prev))/m_dt/m_dt;
m_pt_numerator = (m_pt_numerator+1) < m_filter_order_m ? (m_pt_numerator+1) : 0;
m_pt_denominator = (m_pt_denominator+1) < m_filter_order_n-1 ?
(m_pt_denominator+1) : 0;
m_output_buffer.col(m_pt_denominator) = x_output_dx_ddx.head(m_x_size);
return;
}
void CausalFilter::switch_filter(const Eigen::VectorXd& filter_numerator,
const Eigen::VectorXd& filter_denominator)
{
Eigen::Index filter_order_m = filter_numerator.size();
Eigen::Index filter_order_n = filter_denominator.size();
Eigen::VectorXd current_x(m_input_buffer.col(m_pt_numerator));
m_input_buffer.resize(Eigen::NoChange, filter_order_m);
m_output_buffer.resize(Eigen::NoChange, filter_order_n-1);
for(int i=0;i<filter_order_m; i++)
m_input_buffer.col(i) = current_x;
for(int i=0;i<filter_order_n-1; i++)
m_output_buffer.col(i) = current_x*
filter_numerator.sum()/filter_denominator.sum();
m_filter_order_m = filter_order_m;
m_filter_numerator.resize(filter_order_m);
m_filter_numerator = filter_numerator;
m_filter_order_n = filter_order_n;
m_filter_denominator.resize(filter_order_n);
m_filter_denominator = filter_denominator;
m_pt_numerator = 0;
m_pt_denominator = 0;
return;
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment