30 #include <Eigen/Dense> 42 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
82 period = std::max(period, 2 * dt_);
83 cutoffPeriod_ = period;
104 double x = (cutoffPeriod_ <= dt_) ? 1. : dt_ / cutoffPeriod_;
105 T discVel = (newPos - pos_) / dt_;
106 T newVel = x * discVel + (1. - x) * vel_;
130 double cutoffPeriod_ = 0.;
void updatePositionOnly(const T &newPos)
Update position only.
void update(const T &newPos)
Update velocity estimate from new position value.
Compute velocity by finite difference of position measurements, applying a low-pass filter to it...
double cutoffPeriod() const
Get cutoff period.
LowPassVelocityFilter(double dt, double period)
Constructor with cutoff period.
Utility functions and classes.
const T & vel()
Get filtered velocity.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW LowPassVelocityFilter(double dt)
Constructor.
void cutoffPeriod(double period)
Set cutoff period.
void reset(T pos)
Reset position to an initial rest value.