100 lines
3.0 KiB
C++
100 lines
3.0 KiB
C++
/*
|
|
* @file testAHRS.cpp
|
|
* @brief Test AHRS
|
|
* @author Frank Dellaert
|
|
* @author Chris Beall
|
|
*/
|
|
|
|
#include "../AHRS.h"
|
|
#include <gtsam/geometry/Rot3.h>
|
|
#include <gtsam/base/Vector.h>
|
|
#include <gtsam/base/Testable.h>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include <list>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
// stationary interval of gyro U and acc F
|
|
Matrix stationaryU = trans(Matrix_(3,3,-0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003));
|
|
Matrix stationaryF = trans(Matrix_(3,3,0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021));
|
|
double g_e = 9.7963; // Atlanta
|
|
|
|
/* ************************************************************************* */
|
|
TEST (AHRS, cov) {
|
|
|
|
// samples stored by row
|
|
Matrix A = Matrix_(4, 3,
|
|
1.0, 2.0, 3.0,
|
|
5.0, 7.0, 0.0,
|
|
9.0, 4.0, 7.0,
|
|
6.0, 3.0, 2.0);
|
|
|
|
Matrix actual = cov(trans(A));
|
|
Matrix expected = Matrix_(3, 3,
|
|
10.9167, 2.3333, 5.0000,
|
|
2.3333, 4.6667, -2.6667,
|
|
5.0000, -2.6667, 8.6667);
|
|
|
|
EXPECT(assert_equal(expected, actual, 1e-4));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST (AHRS, covU) {
|
|
|
|
Matrix actual = cov(10000*stationaryU);
|
|
Matrix expected = Matrix_(3, 3,
|
|
33.3333333, -1.66666667, 53.3333333,
|
|
-1.66666667, 0.333333333, -5.16666667,
|
|
53.3333333, -5.16666667, 110.333333);
|
|
|
|
EXPECT(assert_equal(expected, actual, 1e-4));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST (AHRS, covF) {
|
|
|
|
Matrix actual = cov(100*stationaryF);
|
|
Matrix expected = Matrix_(3, 3,
|
|
63.3808333, -0.432166667, -48.1706667,
|
|
-0.432166667, 8.31053333, -16.6792667,
|
|
-48.1706667, -16.6792667, 71.4297333);
|
|
|
|
EXPECT(assert_equal(expected, actual, 1e-4));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST (AHRS, constructor) {
|
|
AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// TODO make a testMechanization_bRn2
|
|
TEST (AHRS, Mechanization_integrate) {
|
|
AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
|
|
Mechanization_bRn2 mech;
|
|
KalmanFilter::State state;
|
|
// boost::tie(mech,state) = ahrs.initialize(g_e);
|
|
// Vector u = Vector_(3,0.05,0.0,0.0);
|
|
// double dt = 2;
|
|
// Rot3 expected;
|
|
// Mechanization_bRn2 mech2 = mech.integrate(u,dt);
|
|
// Rot3 actual = mech2.bRn();
|
|
// EXPECT(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
/* TODO: currently fails because of problem with ill-conditioned system
|
|
TEST (AHRS, init) {
|
|
AHRS ahrs = AHRS(stationaryU,stationaryF,g_e);
|
|
std::pair<Mechanization_bRn2, KalmanFilter::State> result = ahrs.initialize(g_e);
|
|
}
|
|
*/
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|
|
|