1/*
2 * @file testAHRS.cpp
3 * @brief Test AHRS
4 * @author Frank Dellaert
5 * @author Chris Beall
6 */
7
8#include "../AHRS.h"
9#include <gtsam/geometry/Rot3.h>
10#include <gtsam/base/Vector.h>
11#include <gtsam/base/Testable.h>
12#include <CppUnitLite/TestHarness.h>
13#include <list>
14
15using namespace std;
16using namespace gtsam;
17
18// stationary interval of gyro U and acc F
19Matrix stationaryU = trans(A: (Matrix(3, 3) << -0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003).finished());
20Matrix stationaryF = trans(A: (Matrix(3, 3) << 0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021).finished());
21double g_e = 9.7963; // Atlanta
22
23/* ************************************************************************* */
24TEST (AHRS, cov) {
25
26 // samples stored by row
27 Matrix A = (Matrix(4, 3) <<
28 1.0, 2.0, 3.0,
29 5.0, 7.0, 0.0,
30 9.0, 4.0, 7.0,
31 6.0, 3.0, 2.0).finished();
32
33 Matrix actual = AHRS::Cov(m: trans(A));
34 Matrix expected = (Matrix(3, 3) <<
35 10.9167, 2.3333, 5.0000,
36 2.3333, 4.6667, -2.6667,
37 5.0000, -2.6667, 8.6667).finished();
38
39 EXPECT(assert_equal(expected, actual, 1e-4));
40}
41
42/* ************************************************************************* */
43TEST (AHRS, covU) {
44
45 Matrix actual = AHRS::Cov(m: 10000*stationaryU);
46 Matrix expected = (Matrix(3, 3) <<
47 33.3333333, -1.66666667, 53.3333333,
48 -1.66666667, 0.333333333, -5.16666667,
49 53.3333333, -5.16666667, 110.333333).finished();
50
51 EXPECT(assert_equal(expected, actual, 1e-4));
52}
53
54/* ************************************************************************* */
55TEST (AHRS, covF) {
56
57 Matrix actual = AHRS::Cov(m: 100*stationaryF);
58 Matrix expected = (Matrix(3, 3) <<
59 63.3808333, -0.432166667, -48.1706667,
60 -0.432166667, 8.31053333, -16.6792667,
61 -48.1706667, -16.6792667, 71.4297333).finished();
62
63 EXPECT(assert_equal(expected, actual, 1e-4));
64}
65
66/* ************************************************************************* */
67TEST (AHRS, constructor) {
68 AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
69}
70
71/* ************************************************************************* */
72// TODO make a testMechanization_bRn2
73TEST(AHRS, Mechanization_integrate) {
74 AHRS ahrs = AHRS(stationaryU, stationaryF, g_e);
75 // const auto [mech, state] = ahrs.initialize(g_e);
76 // Vector u = Vector3(0.05, 0.0, 0.0);
77 // double dt = 2;
78 // Rot3 expected;
79 // Mechanization_bRn2 mech2 = mech.integrate(u, dt);
80 // Rot3 actual = mech2.bRn();
81 // EXPECT(assert_equal(expected, actual));
82}
83
84/* ************************************************************************* */
85/* TODO: currently fails because of problem with ill-conditioned system
86TEST (AHRS, init) {
87 AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
88 std::pair<Mechanization_bRn2, KalmanFilter::State> result = ahrs.initialize(g_e);
89}
90*/
91/* ************************************************************************* */
92int main() {
93 TestResult tr;
94 return TestRegistry::runAllTests(result&: tr);
95}
96/* ************************************************************************* */
97
98