Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef TRAJECTORY_TRACKER_FILTER_H
00031 #define TRAJECTORY_TRACKER_FILTER_H
00032
00033 #include <cmath>
00034
00035 namespace trajectory_tracker
00036 {
00037 class Filter
00038 {
00039 public:
00040 enum Type
00041 {
00042 FILTER_HPF,
00043 FILTER_LPF
00044 };
00045
00046 protected:
00047 Type type_;
00048 float time_const_;
00049 float x_;
00050 float out_;
00051 float k_[4];
00052 bool angle_;
00053
00054 public:
00055 Filter(const enum Type type, const float tc, const float out0, const bool angle = false)
00056 {
00057 angle_ = angle;
00058 time_const_ = tc;
00059 type_ = type;
00060 switch (type_)
00061 {
00062 case FILTER_LPF:
00063 k_[3] = -1 / (1.0 + 2 * time_const_);
00064 k_[2] = -k_[3];
00065 k_[1] = (1.0 - 2 * time_const_) * k_[3];
00066 k_[0] = -k_[1] - 1.0;
00067 x_ = (1 - k_[2]) * out0 / k_[3];
00068 break;
00069 case FILTER_HPF:
00070 k_[3] = -1 / (1.0 + 2 * time_const_);
00071 k_[2] = -k_[3] * 2 * time_const_;
00072 k_[1] = (1.0 - 2 * time_const_) * k_[3];
00073 k_[0] = 2 * time_const_ * (-k_[1] + 1.0);
00074 x_ = (1 - k_[2]) * out0 / k_[3];
00075 break;
00076 }
00077 out_ = out0;
00078 }
00079 void set(const float& out0)
00080 {
00081 x_ = (1 - k_[2]) * out0 / k_[3];
00082 out_ = out0;
00083 }
00084 float in(const float& i)
00085 {
00086 float in = i;
00087 assert(std::isfinite(in));
00088
00089 if (angle_)
00090 {
00091 in = out_ + remainder(in - out_, M_PI * 2.0);
00092 }
00093 x_ = k_[0] * in + k_[1] * x_;
00094 out_ = k_[2] * in + k_[3] * x_;
00095
00096 assert(std::isfinite(out_));
00097 return out_;
00098 }
00099 float get()
00100 {
00101 return out_;
00102 }
00103 };
00104 }
00105
00106 #endif // TRAJECTORY_TRACKER_FILTER_H