madgwick_test.cpp
Go to the documentation of this file.
2 #include <cmath>
3 #include "test_helpers.h"
4 
5 #define FILTER_ITERATIONS 10000
6 
7 
8 template <WorldFrame::WorldFrame FRAME>
10  float Ax, float Ay, float Az,
11  float Mx, float My, float Mz,
12  double& q0, double& q1, double& q2, double& q3) {
13  float dt = 0.1;
14  float Gx = 0.0, Gy = 0.0, Gz = 0.0; // Stationary state => Gyro = (0,0,0)
15 
16  ImuFilter filter;
17  filter.setDriftBiasGain(0.0);
18  filter.setAlgorithmGain(0.1);
19 
20  // initialize with some orientation
21  filter.setOrientation(q0,q1,q2,q3);
22  filter.setWorldFrame(FRAME);
23 
24  for (int i = 0; i < FILTER_ITERATIONS; i++) {
25  filter.madgwickAHRSupdate(Gx, Gy, Gz, Ax, Ay, Az, Mx, My, Mz, dt);
26  }
27 
28  filter.getOrientation(q0,q1,q2,q3);
29 }
30 
31 
32 template <WorldFrame::WorldFrame FRAME>
34  float Ax, float Ay, float Az,
35  double& q0, double& q1, double& q2, double& q3) {
36  float dt = 0.1;
37  float Gx = 0.0, Gy = 0.0, Gz = 0.0; // Stationary state => Gyro = (0,0,0)
38 
39  ImuFilter filter;
40  filter.setDriftBiasGain(0.0);
41  filter.setAlgorithmGain(0.1);
42 
43  // initialize with some orientation
44  filter.setOrientation(q0,q1,q2,q3);
45  filter.setWorldFrame(FRAME);
46 
47  for (int i = 0; i < FILTER_ITERATIONS; i++) {
48  filter.madgwickAHRSupdateIMU(Gx, Gy, Gz, Ax, Ay, Az, dt);
49  }
50 
51  filter.getOrientation(q0,q1,q2,q3);
52 }
53 
54 
55 #define TEST_STATIONARY_ENU(in_am, exp_result) \
56  TEST(MadgwickTest, Stationary_ENU_ ## in_am){ \
57  double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
58  filterStationary<WorldFrame::ENU>(in_am, q0, q1, q2, q3); \
59  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
60  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \
61  TEST(MadgwickTest, Stationary_ENU_NM_ ## in_am){ \
62  double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
63  filterStationary<WorldFrame::ENU>(ACCEL_ONLY(in_am), q0, q1, q2, q3); \
64  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
65  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
66 
67 #define TEST_STATIONARY_NED(in_am, exp_result) \
68  TEST(MadgwickTest, Stationary_NED_ ## in_am){ \
69  double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
70  filterStationary<WorldFrame::NED>(in_am, q0, q1, q2, q3); \
71  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
72  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \
73  TEST(MadgwickTest, Stationary_NED_NM_ ## in_am){ \
74  double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
75  filterStationary<WorldFrame::NED>(ACCEL_ONLY(in_am), q0, q1, q2, q3); \
76  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
77  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
78 
79 #define TEST_STATIONARY_NWU(in_am, exp_result) \
80  TEST(MadgwickTest, Stationary_NWU_ ## in_am){ \
81  double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
82  filterStationary<WorldFrame::NWU>(in_am, q0, q1, q2, q3); \
83  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
84  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \
85  TEST(MadgwickTest, Stationary_NWU_NM_ ## in_am){ \
86  double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
87  filterStationary<WorldFrame::NWU>(ACCEL_ONLY(in_am), q0, q1, q2, q3); \
88  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
89  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
90 
95 
101 
103 TEST_STATIONARY_NED(AM_NORTH_WEST_UP, QUAT_X_180)
104 TEST_STATIONARY_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED)
106 
107 
108 
109 TEST(MadgwickTest, TestQuatEqNoZ) {
110 
111  ASSERT_TRUE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_MZ_90));
112  ASSERT_FALSE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_X_180));
113 
114 }
115 
116 
117 
118 int main(int argc, char **argv){
119  testing::InitGoogleTest(&argc, argv);
120  return RUN_ALL_TESTS();
121 }
int main(int argc, char **argv)
#define QUAT_XMYMZ_120
Definition: test_helpers.h:93
#define QUAT_WEST_NORTH_DOWN_RSD_ENU
Definition: test_helpers.h:95
#define AM_NORTH_WEST_UP
Definition: test_helpers.h:76
#define QUAT_NE_NW_UP_RSD_ENU
Definition: test_helpers.h:98
#define QUAT_X_180
Definition: test_helpers.h:92
#define QUAT_WEST_NORTH_DOWN_RSD_NED
Definition: test_helpers.h:96
#define AM_SOUTH_EAST_UP
Definition: test_helpers.h:78
void setOrientation(double q0, double q1, double q2, double q3)
Definition: imu_filter.h:82
void madgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt)
Definition: imu_filter.cpp:175
void getOrientation(double &q0, double &q1, double &q2, double &q3)
Definition: imu_filter.h:65
#define AM_NE_NW_UP_RSD
Definition: test_helpers.h:83
#define TEST_STATIONARY_NWU(in_am, exp_result)
#define AM_NORTH_EAST_DOWN
Definition: test_helpers.h:75
static bool quat_eq_ex_z(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3)
Definition: test_helpers.h:44
void setAlgorithmGain(double gain)
Definition: imu_filter.h:50
#define TEST_STATIONARY_ENU(in_am, exp_result)
void madgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt)
Definition: imu_filter.cpp:258
void setWorldFrame(WorldFrame::WorldFrame frame)
Definition: imu_filter.h:60
#define AM_EAST_NORTH_UP
Definition: test_helpers.h:74
#define QUAT_MZ_90
Definition: test_helpers.h:91
TEST(MadgwickTest, TestQuatEqNoZ)
#define FILTER_ITERATIONS
void filterStationary(float Ax, float Ay, float Az, float Mx, float My, float Mz, double &q0, double &q1, double &q2, double &q3)
#define AM_SOUTH_UP_WEST
Definition: test_helpers.h:77
#define AM_WEST_NORTH_DOWN_RSD
Definition: test_helpers.h:82
#define QUAT_NE_NW_UP_RSD_NWU
Definition: test_helpers.h:97
#define QUAT_NE_NW_UP_RSD_NED
Definition: test_helpers.h:99
void setDriftBiasGain(double zeta)
Definition: imu_filter.h:55
#define TEST_STATIONARY_NED(in_am, exp_result)
#define QUAT_IDENTITY
Definition: test_helpers.h:90
#define QUAT_WEST_NORTH_DOWN_RSD_NWU
Definition: test_helpers.h:94


imu_filter_madgwick
Author(s): Ivan Dryanovski
autogenerated on Thu Jun 18 2020 03:40:00