34 #include <gtest/gtest.h> 55 const float reset_value = 42.42f;
62 const float input = 0.f;
63 for (
int i = 0; i < 10; i++) {
72 const float input = 1.f;
75 for (
int i = 0; i < 9; i++) {
86 const float input = -1.f;
89 for (
int i = 0; i < 9; i++) {
100 const float input = 1.f;
103 for (
int i = 0; i < 3 * 9; i++) {
114 const float input = 1.f;
117 for (
int i = 0; i < 5 * 9; i++) {
128 const Vector3f input = {3.f, 7.f, -11.f};
131 for (
int i = 0; i < 5 * 9; i++) {
137 for (
int i = 0; i < 3; i++) {
138 ASSERT_NEAR(output(i), input(i), fabsf(0.01
f * input(i)));
145 const Vector3f input = {3.f, 7.f, -11.f};
146 const float tau = 18.f;
147 const float dt = 1.f;
148 const float alpha = dt / tau;
151 for (
int i = 0; i < 18; i++) {
157 for (
int i = 0; i < 3; i++) {
158 ASSERT_NEAR(output(i), 0.63
f * input(i), fabsf(0.02
f * input(i)));
165 const Vector3f input = {51.f, 7.f, -11.f};
166 const float tau = 2.f;
167 const float dt = 0.1f;
170 const float n = tau /
dt;
171 for (
int i = 0; i < n; i++) {
177 for (
int i = 0; i < 3; i++) {
178 ASSERT_NEAR(output(i), 0.63
f * input(i), fabsf(0.02
f * input(i)));
182 const Vector3f reset_vector = {-1.f, 71.f, -42.f};
187 for (
int i = 0; i < 3; i++) {
188 ASSERT_EQ(output(i), reset_vector(i));
AlphaFilter< float > filter_float
const T & getState() const
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
void update(const T &input, float tau, float dt)
Vector3< float > Vector3f
TEST_F(AlphaFilterTest, initializeToZero)
AlphaFilter< Vector3f > filter_v3