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 #include <cstddef>
00031
00032 #include <gtest/gtest.h>
00033
00034 #include <trajectory_tracker/filter.h>
00035
00036 TEST(Filter, LPFCharacteristic)
00037 {
00038 for (int time_const = 20; time_const < 100; time_const += 20)
00039 {
00040 trajectory_tracker::Filter lpf(trajectory_tracker::Filter::FILTER_LPF, time_const, 0.0);
00041 ASSERT_LT(fabs(lpf.get()), 1e-6);
00042
00043
00044 float ret = 0;
00045 for (int i = 0; i < time_const; ++i)
00046 {
00047 ret = lpf.in(1.0);
00048 }
00049
00050 ASSERT_TRUE(ret == lpf.get());
00051 ASSERT_LT(fabs(ret - (1.0 - expf(-1.0))), 1e-2);
00052
00053 for (int i = time_const; i < time_const * 100; ++i)
00054 {
00055 ret = lpf.in(1.0);
00056 }
00057
00058 ASSERT_TRUE(ret == lpf.get());
00059 ASSERT_LT(fabs(ret - 1.0), 1e-2);
00060
00061
00062 lpf.set(1.0);
00063 ASSERT_LT(fabs(lpf.get() - 1.0), 1e-2);
00064 lpf.in(1.0);
00065 ASSERT_LT(fabs(lpf.get() - 1.0), 1e-2);
00066 }
00067 }
00068
00069 TEST(Filter, HPFCharacteristic)
00070 {
00071 for (int time_const = 20; time_const < 100; time_const += 20)
00072 {
00073 trajectory_tracker::Filter lpf(trajectory_tracker::Filter::FILTER_LPF, time_const, 0.0);
00074 trajectory_tracker::Filter hpf(trajectory_tracker::Filter::FILTER_HPF, time_const, 0.0);
00075
00076
00077 for (int i = 0; i < time_const * 10; ++i)
00078 {
00079 float ret_h, ret_l;
00080 ret_l = lpf.in(1.0);
00081 ret_h = hpf.in(1.0);
00082
00083
00084 ASSERT_LT(fabs(ret_l + ret_h - 1.0), 1e-2);
00085 }
00086 }
00087 }
00088
00089 TEST(Filter, AugleLPF)
00090 {
00091 for (float zero = 0.0; zero < M_PI * 2 * 4; zero += M_PI * 2)
00092 {
00093
00094 const float start1 = zero + 0.5;
00095 const float end1 = zero + M_PI * 2.0 - 0.5;
00096
00097 trajectory_tracker::Filter lpf(trajectory_tracker::Filter::FILTER_LPF, 10, start1);
00098 trajectory_tracker::Filter lpf_angle(trajectory_tracker::Filter::FILTER_LPF, 10, start1, true);
00099 ASSERT_LT(fabs(lpf.get() - start1), 1e-6);
00100 ASSERT_LT(fabs(lpf_angle.get() - start1), 1e-6);
00101
00102 for (int i = 0; i < 100; ++i)
00103 {
00104 lpf.in(end1);
00105 lpf_angle.in(end1);
00106 ASSERT_GT(lpf.get(), start1);
00107 ASSERT_LT(lpf_angle.get(), start1);
00108 }
00109 ASSERT_LT(fabs(lpf.get() - end1), 1e-2);
00110 ASSERT_LT(fabs(lpf_angle.get() - (zero - 0.5)), 1e-2);
00111
00112
00113 const float start2 = zero - 0.5;
00114 const float end2 = zero - M_PI * 2.0 + 0.5;
00115
00116 lpf.set(start2);
00117 lpf_angle.set(start2);
00118 ASSERT_LT(fabs(lpf.get() - start2), 1e-6);
00119 ASSERT_LT(fabs(lpf_angle.get() - start2), 1e-6);
00120
00121 for (int i = 0; i < 100; ++i)
00122 {
00123 lpf.in(end2);
00124 lpf_angle.in(end2);
00125 ASSERT_LT(lpf.get(), start2);
00126 ASSERT_GT(lpf_angle.get(), start2);
00127 }
00128 ASSERT_LT(fabs(lpf.get() - end2), 1e-2);
00129 ASSERT_LT(fabs(lpf_angle.get() - (zero + 0.5)), 1e-2);
00130 }
00131 }
00132
00133 int main(int argc, char** argv)
00134 {
00135 testing::InitGoogleTest(&argc, argv);
00136
00137 return RUN_ALL_TESTS();
00138 }