30 #ifndef TRAJECTORY_TRACKER_FILTER_H 31 #define TRAJECTORY_TRACKER_FILTER_H 56 Filter(
const enum Type type,
const float tc,
const float out0,
const bool angle =
false)
68 x_ = (1 - k_[2]) * out0 / k_[3];
74 k_[0] = 2 * time_const_ * (-k_[1] + 1.0);
75 x_ = (1 - k_[2]) * out0 / k_[3];
80 void set(
const float& out0)
82 x_ = (1 - k_[2]) * out0 / k_[3];
85 float in(
const float& i)
88 assert(std::isfinite(in));
92 in = out_ + remainder(in - out_, M_PI * 2.0);
94 x_ = k_[0] * in + k_[1] *
x_;
95 out_ = k_[2] * in + k_[3] *
x_;
97 assert(std::isfinite(out_));
107 #endif // TRAJECTORY_TRACKER_FILTER_H
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
Filter(const enum Type type, const float tc, const float out0, const bool angle=false)