From d85c773f7cbc1eddd6c2640a8791a77387276726 Mon Sep 17 00:00:00 2001 From: Kyel Ok Date: Fri, 14 Jun 2013 21:18:22 +0000 Subject: [PATCH] Changes in AHRS - flat trim initialization added, some comments by Frank, aidingAvailablitiy function --- gtsam_unstable/slam/AHRS.cpp | 23 +++++++++++++-- gtsam_unstable/slam/AHRS.h | 5 +++- gtsam_unstable/slam/Mechanization_bRn2.cpp | 34 +++++++++++++--------- gtsam_unstable/slam/Mechanization_bRn2.h | 7 +++-- 4 files changed, 50 insertions(+), 19 deletions(-) diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index cbd1096a9..002c9e3e6 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -24,11 +24,12 @@ Matrix I3 = eye(3); Matrix Z3 = zeros(3, 3); /* ************************************************************************* */ -AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e) : +AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, + bool flat) : KF_(9) { // Initial state - mech0_ = Mechanization_bRn2::initialize(stationaryU, stationaryF, g_e); + mech0_ = Mechanization_bRn2::initialize(stationaryU, stationaryF, g_e, flat); size_t T = stationaryU.cols(); @@ -145,6 +146,18 @@ std::pair AHRS::integrate( return make_pair(newMech, newState); } +/* ************************************************************************* */ +bool AHRS::isAidingAvailable(const Mechanization_bRn2& mech, + const gtsam::Vector& f, const gtsam::Vector& u, double ge) { + + // Subtract the biases + Vector f_ = f - mech.x_a(); + Vector u_ = u - mech.x_g(); + + double mu_f = f_.norm() - ge; + double mu_u = u_.norm(); + return (fabs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * 3.1415926); +} /* ************************************************************************* */ std::pair AHRS::aid( @@ -169,10 +182,14 @@ std::pair AHRS::aid( // F(:,k) = mech.x_a + dx_a - bRn*n_g; // F(:,k) = mech.x_a + dx_a - bRn*(I+P)*n_g; // F(:,k) = mech.x_a + dx_a - b_g - bRn*(rho x n_g); // P = [rho]_x - // b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho); + // Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted + // from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho) + // z = b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho) z = bRn * n_g_ - measured_b_g; + // Now the Jacobian H Matrix b_g = bRn * n_g_cross_; H = collect(3, &b_g, &Z3, &I3); + // And the measurement noise, TODO: should be created once where sigmas_v_a is given R = diag(emul(sigmas_v_a_, sigmas_v_a_)); } diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index 01c61704b..1d7eb85f5 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -43,7 +43,7 @@ public: * @param stationaryF initial interval of accelerator measurements, 3xn matrix * @param g_e if given, initializes gravity with correct value g_e */ - AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e); + AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, bool flat=false); std::pair initialize(double g_e); @@ -51,6 +51,9 @@ public: const Mechanization_bRn2& mech, KalmanFilter::State state, const Vector& u, double dt); + bool isAidingAvailable(const Mechanization_bRn2& mech, + const Vector& f, const Vector& u, double ge); + std::pair aid( const Mechanization_bRn2& mech, KalmanFilter::State state, const Vector& f, bool Farrell=0); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index ca95843af..312814d15 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -12,35 +12,43 @@ namespace gtsam { /* ************************************************************************* */ Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list& U, - const std::list& F, const double g_e) { + const std::list& F, const double g_e, bool flat) { Matrix Umat = Matrix_(3,U.size(), concatVectors(U)); Matrix Fmat = Matrix_(3,F.size(), concatVectors(F)); - return initialize(Umat, Fmat, g_e); + return initialize(Umat, Fmat, g_e, flat); } /* ************************************************************************* */ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, - const Matrix& F, const double g_e) { + const Matrix& F, const double g_e, bool flat) { // estimate gyro bias by averaging gyroscope readings (10.62) Vector x_g = U.rowwise().mean(); - Vector meanF = -F.rowwise().mean(); + Vector meanF = F.rowwise().mean(); - Vector b_g, x_a; + // estimate gravity + Vector b_g; if(g_e == 0) { - // estimate gravity in body frame from accelerometer (10.13) - b_g = meanF; - // estimate accelerometer bias as zero (10.65) - x_a = zero(3); - + if (flat) + // acceleration measured is along the z-axis. + b_g = Vector_(3, 0.0, 0.0, norm_2(meanF)); + else + // acceleration measured is the opposite of gravity (10.13) + b_g = -meanF; } else { - // normalize b_g and attribute remainder to biases - b_g = g_e * meanF/meanF.norm(); - x_a = -(meanF - b_g); + if (flat) + // gravity is downward along the z-axis since we are flat on the ground + b_g = Vector_(3,0.0,0.0,g_e); + else + // normalize b_g and attribute remainder to biases + b_g = - g_e * meanF/meanF.norm(); } + // estimate accelerometer bias + Vector x_a = meanF + b_g; + // initialize roll, pitch (eqns. 10.14, 10.15) double g1=b_g(0); double g2=b_g(1); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index b81e685d4..711a44bf9 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -47,12 +47,15 @@ public: * Initialize the first Mechanization state * @param U a list of gyro measurement vectors * @param F a list of accelerometer measurement vectors + * @param g_e gravity magnitude + * @param flat flag saying whether this is a flat trim init */ static Mechanization_bRn2 initializeVector(const std::list& U, - const std::list& F, const double g_e = 0); + const std::list& F, const double g_e = 0, bool flat=false); + /// Matrix version of initialize static Mechanization_bRn2 initialize(const Matrix& U, - const Matrix& F, const double g_e = 0); + const Matrix& F, const double g_e = 0, bool flat=false); /** * Correct AHRS full state given error state dx