PX4 Firmware
PX4 Autopilot Software http://px4.io
test_AlphaFilter.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 ECL Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #include <gtest/gtest.h>
35 #include <cmath>
36 #include <matrix/math.hpp>
37 
38 #include "EKF/AlphaFilter.hpp"
39 
40 using matrix::Vector3f;
41 
42 class AlphaFilterTest : public ::testing::Test {
43  public:
46 };
47 
48 TEST_F(AlphaFilterTest, initializeToZero)
49 {
50  ASSERT_EQ(filter_float.getState(), 0.f);
51 }
52 
53 TEST_F(AlphaFilterTest, resetToValue)
54 {
55  const float reset_value = 42.42f;
56  filter_float.reset(reset_value);
57  ASSERT_EQ(filter_float.getState(), reset_value);
58 }
59 
61 {
62  const float input = 0.f;
63  for (int i = 0; i < 10; i++) {
64  filter_float.update(input);
65  }
66  ASSERT_EQ(filter_float.getState(), input);
67 }
68 
69 TEST_F(AlphaFilterTest, runPositive)
70 {
71  // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
72  const float input = 1.f;
73 
74  // WHEN we run the filter 9 times
75  for (int i = 0; i < 9; i++) {
76  filter_float.update(input);
77  }
78 
79  // THEN the state of the filter should have reached 63%
80  ASSERT_NEAR(filter_float.getState(), 0.63f, 0.02);
81 }
82 
83 TEST_F(AlphaFilterTest, runNegative)
84 {
85  // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
86  const float input = -1.f;
87 
88  // WHEN we run the filter 9 times
89  for (int i = 0; i < 9; i++) {
90  filter_float.update(input);
91  }
92 
93  // THEN the state of the filter should have reached 63%
94  ASSERT_NEAR(filter_float.getState(), -0.63f, 0.02);
95 }
96 
98 {
99  // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
100  const float input = 1.f;
101 
102  // WHEN we run the filter 27 times (3 * time constant)
103  for (int i = 0; i < 3 * 9; i++) {
104  filter_float.update(input);
105  }
106 
107  // THEN the state of the filter should have reached 95%
108  ASSERT_NEAR(filter_float.getState(), 0.95f, 0.02f);
109 }
110 
111 TEST_F(AlphaFilterTest, convergence)
112 {
113  // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
114  const float input = 1.f;
115 
116  // WHEN we run the filter 45 times (5 * time constant)
117  for (int i = 0; i < 5 * 9; i++) {
118  filter_float.update(input);
119  }
120 
121  // THEN the state of the filter should have converged to the input
122  ASSERT_NEAR(filter_float.getState(), 1.f, 0.01f);
123 }
124 
125 TEST_F(AlphaFilterTest, convergenceVector3f)
126 {
127  // GIVEN an Vector3f input in a filter with a default time constant of 9 (alpha = 0.1)
128  const Vector3f input = {3.f, 7.f, -11.f};
129 
130  // WHEN we run the filter 45 times (5 * time constant)
131  for (int i = 0; i < 5 * 9; i++) {
132  filter_v3.update(input);
133  }
134 
135  // THEN the state of the filter should have converged to the input (1% error allowed)
136  Vector3f output = filter_v3.getState();
137  for (int i = 0; i < 3; i++) {
138  ASSERT_NEAR(output(i), input(i), fabsf(0.01f * input(i)));
139  }
140 }
141 
142 TEST_F(AlphaFilterTest, convergenceVector3fAlpha)
143 {
144  // GIVEN a Vector3f input in a filter with a defined time constant and the default sampling time
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;
149 
150  // WHEN we run the filter 18 times (1 * time constant)
151  for (int i = 0; i < 18; i++) {
152  filter_v3.update(input, alpha); // dt is assumed equal to 1
153  }
154 
155  // THEN the state of the filter should have reached 65% (2% error allowed)
156  Vector3f output = filter_v3.getState();
157  for (int i = 0; i < 3; i++) {
158  ASSERT_NEAR(output(i), 0.63f * input(i), fabsf(0.02f * input(i)));
159  }
160 }
161 
162 TEST_F(AlphaFilterTest, convergenceVector3fTauDt)
163 {
164  // GIVEN a Vector3f input in a filter with a defined time constant and sampling time
165  const Vector3f input = {51.f, 7.f, -11.f};
166  const float tau = 2.f;
167  const float dt = 0.1f;
168 
169  // WHEN we run the filter (1 * time constant)
170  const float n = tau / dt;
171  for (int i = 0; i < n; i++) {
172  filter_v3.update(input, tau, dt);
173  }
174 
175  // THEN the state of the filter should have reached 65% (2% error allowed)
176  Vector3f output = filter_v3.getState();
177  for (int i = 0; i < 3; i++) {
178  ASSERT_NEAR(output(i), 0.63f * input(i), fabsf(0.02f * input(i)));
179  }
180 
181  // ALSO when the filter is reset to a specified value
182  const Vector3f reset_vector = {-1.f, 71.f, -42.f};
183  filter_v3.reset(reset_vector);
184  output = filter_v3.getState();
185 
186  // THEN the filter should exactly contain those values
187  for (int i = 0; i < 3; i++) {
188  ASSERT_EQ(output(i), reset_vector(i));
189  }
190 }
AlphaFilter< float > filter_float
const T & getState() const
Definition: AlphaFilter.hpp:67
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
void update(const T &input, float tau, float dt)
Definition: AlphaFilter.hpp:50
Vector3< float > Vector3f
Definition: Vector3.hpp:136
float dt
Definition: px4io.c:73
void reset(const T &val)
Definition: AlphaFilter.hpp:48
TEST_F(AlphaFilterTest, initializeToZero)
AlphaFilter< Vector3f > filter_v3