Changes in AHRS - flat trim initialization added, some comments by Frank, aidingAvailablitiy function

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<Mechanization_bRn2, KalmanFilter::State> 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<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
@@ -169,10 +182,14 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> 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_));
   }
 

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<Mechanization_bRn2, KalmanFilter::State> 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<Mechanization_bRn2, KalmanFilter::State> aid(
       const Mechanization_bRn2& mech, KalmanFilter::State state,
       const Vector& f, bool Farrell=0);

gtsam_unstable/slam/Mechanization_bRn2.cpp

@@ -12,35 +12,43 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list<Vector>& U,
-    const std::list<Vector>& F, const double g_e) {
+    const std::list<Vector>& 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);

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<Vector>& U,
-      const std::list<Vector>& F, const double g_e = 0);
+      const std::list<Vector>& 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