12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

AHRS mechanization small efficiency change (in progress)

release/4.3a0
Frank Dellaert 2013-06-21 00:21:18 +00:00
parent 169eb4af3e
commit 5f5e481394
2 changed files with 27 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

@ -83,14 +83,21 @@ Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector& u,
const double dt) const { const double dt) const {
// integrate rotation nRb based on gyro measurement u and bias x_g // integrate rotation nRb based on gyro measurement u and bias x_g
#ifndef MODEL_NAV_FRAME_ROTATION
// get body to inertial (measured in b) from gyro, subtract bias // get body to inertial (measured in b) from gyro, subtract bias
Vector b_omega_ib = u - x_g_; Vector b_omega_ib = u - x_g_;
// assume nav to inertial through movement is unknown // nav to inertial due to Earth's rotation and our movement on Earth surface
// TODO: figure out how to do this if we need it
Vector b_omega_in = zero(3); Vector b_omega_in = zero(3);
// get angular velocity on bRn measured in body frame // calculate angular velocity on bRn measured in body frame
Vector b_omega_bn = b_omega_in - b_omega_ib; Vector b_omega_bn = b_omega_in - b_omega_ib;
#else
// Assume a non-rotating nav frame (probably an approximation)
// calculate angular velocity on bRn measured in body frame
Vector b_omega_bn = x_g_ - u;
#endif
// convert to navigation frame // convert to navigation frame
Rot3 nRb = bRn_.inverse(); Rot3 nRb = bRn_.inverse();

20
gtsam_unstable/slam/tests/testAHRS.cpp
View File

@ -5,11 +5,12 @@
* @author Chris Beall * @author Chris Beall
*/ */
#include <list> #include "../AHRS.h"
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include "../AHRS.h" #include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <list>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -67,6 +68,21 @@ TEST (AHRS, constructor) {
AHRS ahrs = AHRS(stationaryU,stationaryF,g_e); 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 /* TODO: currently fails because of problem with ill-conditioned system
TEST (AHRS, init) { TEST (AHRS, init) {