00001 #include <imu_filter_madgwick/imu_filter.h>
00002 #include <cmath>
00003 #include "test_helpers.h"
00004
00005 #define FILTER_ITERATIONS 10000
00006
00007
00008 template <WorldFrame::WorldFrame FRAME>
00009 void filterStationary(
00010 float Ax, float Ay, float Az,
00011 float Mx, float My, float Mz,
00012 double& q0, double& q1, double& q2, double& q3) {
00013 float dt = 0.1;
00014 float Gx = 0.0, Gy = 0.0, Gz = 0.0;
00015
00016 ImuFilter filter;
00017 filter.setDriftBiasGain(0.0);
00018 filter.setAlgorithmGain(0.1);
00019
00020
00021 filter.setOrientation(q0,q1,q2,q3);
00022 filter.setWorldFrame(FRAME);
00023
00024 for (int i = 0; i < FILTER_ITERATIONS; i++) {
00025 filter.madgwickAHRSupdate(Gx, Gy, Gz, Ax, Ay, Az, Mx, My, Mz, dt);
00026 }
00027
00028 filter.getOrientation(q0,q1,q2,q3);
00029 }
00030
00031
00032 template <WorldFrame::WorldFrame FRAME>
00033 void filterStationary(
00034 float Ax, float Ay, float Az,
00035 double& q0, double& q1, double& q2, double& q3) {
00036 float dt = 0.1;
00037 float Gx = 0.0, Gy = 0.0, Gz = 0.0;
00038
00039 ImuFilter filter;
00040 filter.setDriftBiasGain(0.0);
00041 filter.setAlgorithmGain(0.1);
00042
00043
00044 filter.setOrientation(q0,q1,q2,q3);
00045 filter.setWorldFrame(FRAME);
00046
00047 for (int i = 0; i < FILTER_ITERATIONS; i++) {
00048 filter.madgwickAHRSupdateIMU(Gx, Gy, Gz, Ax, Ay, Az, dt);
00049 }
00050
00051 filter.getOrientation(q0,q1,q2,q3);
00052 }
00053
00054
00055 #define TEST_STATIONARY_ENU(in_am, exp_result) \
00056 TEST(MadgwickTest, Stationary_ENU_ ## in_am){ \
00057 double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
00058 filterStationary<WorldFrame::ENU>(in_am, q0, q1, q2, q3); \
00059 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
00060 ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \
00061 TEST(MadgwickTest, Stationary_ENU_NM_ ## in_am){ \
00062 double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
00063 filterStationary<WorldFrame::ENU>(ACCEL_ONLY(in_am), q0, q1, q2, q3); \
00064 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
00065 ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
00066
00067 #define TEST_STATIONARY_NED(in_am, exp_result) \
00068 TEST(MadgwickTest, Stationary_NED_ ## in_am){ \
00069 double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
00070 filterStationary<WorldFrame::NED>(in_am, q0, q1, q2, q3); \
00071 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
00072 ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \
00073 TEST(MadgwickTest, Stationary_NED_NM_ ## in_am){ \
00074 double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
00075 filterStationary<WorldFrame::NED>(ACCEL_ONLY(in_am), q0, q1, q2, q3); \
00076 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
00077 ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
00078
00079 #define TEST_STATIONARY_NWU(in_am, exp_result) \
00080 TEST(MadgwickTest, Stationary_NWU_ ## in_am){ \
00081 double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
00082 filterStationary<WorldFrame::NWU>(in_am, q0, q1, q2, q3); \
00083 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
00084 ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \
00085 TEST(MadgwickTest, Stationary_NWU_NM_ ## in_am){ \
00086 double q0 = .5, q1 = .5, q2 = .5, q3 = .5; \
00087 filterStationary<WorldFrame::NWU>(ACCEL_ONLY(in_am), q0, q1, q2, q3); \
00088 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
00089 ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
00090
00091 TEST_STATIONARY_NWU(AM_NORTH_EAST_DOWN, QUAT_X_180)
00092 TEST_STATIONARY_NWU(AM_NORTH_WEST_UP, QUAT_IDENTITY)
00093 TEST_STATIONARY_NWU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NWU)
00094 TEST_STATIONARY_NWU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NWU)
00095
00096 TEST_STATIONARY_ENU(AM_EAST_NORTH_UP, QUAT_IDENTITY)
00097 TEST_STATIONARY_ENU(AM_SOUTH_UP_WEST, QUAT_XMYMZ_120)
00098 TEST_STATIONARY_ENU(AM_SOUTH_EAST_UP, QUAT_MZ_90)
00099 TEST_STATIONARY_ENU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_ENU)
00100 TEST_STATIONARY_ENU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_ENU)
00101
00102 TEST_STATIONARY_NED(AM_NORTH_EAST_DOWN, QUAT_IDENTITY)
00103 TEST_STATIONARY_NED(AM_NORTH_WEST_UP, QUAT_X_180)
00104 TEST_STATIONARY_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED)
00105 TEST_STATIONARY_NED(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NED)
00106
00107
00108
00109 TEST(MadgwickTest, TestQuatEqNoZ) {
00110
00111 ASSERT_TRUE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_MZ_90));
00112 ASSERT_FALSE(quat_eq_ex_z(QUAT_IDENTITY, QUAT_X_180));
00113
00114 }
00115
00116
00117
00118 int main(int argc, char **argv){
00119 testing::InitGoogleTest(&argc, argv);
00120 return RUN_ALL_TESTS();
00121 }