added semi-implicit position integration

jhu-asco · Jul 17, 2015 · 3db87b2 · 3db87b2
1 parent 704f20f
commit 3db87b2
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 11 deletions.
diff --git a/lib/systems/ins.cc b/lib/systems/ins.cc
@@ -11,11 +11,13 @@ using namespace Eigen;
 Ins::Ins() : System<InsState, 15, 6>(InsManifold::Instance()) 
 {
   sv = 3*1e-3;
-  su = 3*1e-6;
+  su  = 3*1e-8;
   sa = 3*1e-12;
-  sra = 0.01;
+  sra = 0.05;
 
-  g0 << 0, 0, 9.81;
+  g0 << 0, 0, 9.80665;
+
+  semiImplicit = true;
 }
 
 
@@ -38,20 +40,35 @@ double Ins::Step(InsState &xb, double t, const InsState &xa,
   xb.R = xa.R*dR;
   xb.bg = xa.bg;
   xb.ba = xa.ba;
-  xb.p = xa.p + dt*xa.v;
+
   xb.v = xa.v + dt*(xa.R*a - g0);
 
+  if (semiImplicit)
+    xb.p = xa.p + dt*xb.v;
+  else
+    xb.p = xa.p + dt*xa.v;
+
   // jacobian
   if (A) {
     Matrix3d D;
     so3.dexp(D, -dt*w);  
 
     A->setIdentity();
     A->topLeftCorner<3,3>() = dR.transpose();
-    A->block<3,3>(0,3) = -dt*D;
-
-    A->block<3,3>(9,12) = dt*Matrix3d::Identity();  // dp = v
-    A->block<3,3>(12,6) = -dt*xa.R;                 // dv = R*a    
+    A->block<3,3>(0,3) = -dt*D;           // dR wrt R (trivialized)
+
+    SO3::Instance().hat(D, a);
+
+    if (semiImplicit) {
+      double dt2 = dt*dt;
+
+      A->block<3,3>(9,0) = -dt2*(xa.R*D);   // dp wrt R
+      A->block<3,3>(9,6) = -dt2*xa.R;       // dp wrt a
+    }
+    A->block<3,3>(9,12) = dt*Matrix3d::Identity();  // dp drt v
+
+    A->block<3,3>(12,0) = -dt*(xa.R*D);   // dv wrt R    
+    A->block<3,3>(12,6) = -dt*xa.R;       // dv wrt a
   }
   return 0;
 }

diff --git a/lib/systems/ins.h b/lib/systems/ins.h
@@ -23,9 +23,9 @@ namespace gcop {
   typedef Matrix<double, 6, 6> Matrix6d;
   typedef Matrix<double, 6, 15> Matrix6x15d;
   typedef Matrix<double, 6, Dynamic> Matrix6Xd;
-  
+
   /**
-   * An INS system
+   * A basic Inertial Navigation System definition of dynamics
    *
    * Author: Marin Kobilarov marin(at)jhu.edu
    */
@@ -46,10 +46,12 @@ namespace gcop {
   double sv; ///< gyro bias white noise stdev (spectral density)
   double su; ///< gyro bias rate-of-change white noise stdev (spectral density)
   double sa; ///< acceleration bias rate-of-change white noise stdev (spectral density)  
-
   double sra; ///< acceleration measurement noise
 
   Vector3d g0;   ///< gravity vector
+
+  bool semiImplicit; ///< whether to use a more accurate semi-implicit update (true by default)
+
   };  
 }