15#include <yarp/os/Log.h>
16#include <yarp/math/Math.h>
20using namespace yarp::sig;
21using namespace yarp::math;
31 m=
b.length();
n=
a.length();
32 yAssert((
m>0)&&(
n>0));
56 Vector u_init(y0.length(),0.0);
61 for (
size_t i=0; i<
b.length(); i++)
65 for (
size_t i=0; i<
a.length(); i++)
69 if (fabs(sum_b)>std::numeric_limits<double>::epsilon())
70 u_init=(sum_a/sum_b)*y0;
77 if (fabs(sum_a-
a[0])>std::numeric_limits<double>::epsilon())
78 y_init=
a[0]/(
a[0]-sum_a)*
y;
82 for (
size_t i=0; i<
yold.size(); i++)
85 for (
size_t i=0; i<
uold.size(); i++)
107 m=
b.length();
n=
a.length();
108 yAssert((
m>0)&&(
n>0));
120 if ((num.length()==
b.length()) && (den.length()==
a.length()))
142 yAssert(
y.length()==u.length());
143 for (
size_t j=0; j<
y.length(); j++)
146 for (
size_t i=1; i<
m; i++)
147 for (
size_t j=0; j<
y.length(); j++)
150 for (
size_t i=1; i<
n; i++)
151 for (
size_t j=0; j<
y.length(); j++)
154 for (
size_t j=0; j<
y.length(); j++)
169 rateLowerLim(rL), rateUpperLim(rU)
204 for (
size_t i=0; i<
n; i++)
219 const double sampleTime,
248 if (cutFrequency<=0.0)
284 double tau=1.0/(2.0*
M_PI*
fc);
285 Vector num=cat(
Ts,
Ts);
286 Vector den=cat(2.0*tau+
Ts,
Ts-2.0*tau);
306 yAssert(y0.length()>0);
309 uold.assign(
m,deque<double>());
325 size_t L=v.size()>>1;
326 nth_element(v.begin(),v.begin()+L,v.end());
331 nth_element(v.begin(),v.begin()+L-1,v.end());
332 return 0.5*(v[L]+v[L-1]);
340 yAssert(
y.length()==u.length());
341 for (
size_t i=0; i<
m; i++)
342 uold[i].push_front(u[i]);
344 if (
uold[0].size()>
n)
346 for (
size_t i=0; i<
m; i++)
348 deque<double> tmp=
uold[i];
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Performs filtering on the actual input.
void setCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den)
Sets new filter coefficients.
bool adjustCoeffs(const yarp::sig::Vector &num, const yarp::sig::Vector &den)
Modifies the values of existing filter coefficients without varying their lengths.
void getCoeffs(yarp::sig::Vector &num, yarp::sig::Vector &den)
Returns the current filter coefficients.
Filter(const yarp::sig::Vector &num, const yarp::sig::Vector &den, const yarp::sig::Vector &y0=yarp::sig::Vector(1, 0.0))
Creates a filter with specified numerator and denominator coefficients.
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
void getStates(std::deque< yarp::sig::Vector > &u, std::deque< yarp::sig::Vector > &y)
Returns the current filter states.
std::deque< yarp::sig::Vector > yold
std::deque< yarp::sig::Vector > uold
bool setCutFrequency(const double cutFrequency)
Change the cut frequency of the filter.
bool setSampleTime(const double sampleTime)
Change the sample time of the filter.
FirstOrderLowPassFilter(const double cutFrequency, const double sampleTime, const yarp::sig::Vector &y0=yarp::sig::Vector(1, 0.0))
Creates a filter with specified parameters.
virtual void init(const yarp::sig::Vector &y0)
Internal state reset.
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Performs filtering on the actual input.
virtual ~FirstOrderLowPassFilter()
Destructor.
void setLimits(const yarp::sig::Vector &rL, const yarp::sig::Vector &rU)
Sets new Rate limits.
RateLimiter(const yarp::sig::Vector &rL, const yarp::sig::Vector &rU)
Creates a Rate Limiter which keeps the rate of the input within assigned thresholds.
yarp::sig::Vector rateLowerLim
virtual void init(const yarp::sig::Vector &u0)
Init internal state.
void getLimits(yarp::sig::Vector &rL, yarp::sig::Vector &rU)
Returns the current Rate limits.
yarp::sig::Vector rateUpperLim
virtual const yarp::sig::Vector & filt(const yarp::sig::Vector &u)
Limits the input rate.