process noise flags

3 files changed, 33 insertions, 73 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 5f5076b..fdd41a9 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -115,101 +115,48 @@ Body::R()
 Matrix<double,STATESIZE,STATESIZE>
 Body::Q (double dt)
 {
-    const bool doang=true;
-    const bool doquat=true;
-
     Matrix<double,STATESIZE,3> Ga;
     Ga.block<3,3>(0,0) = 0.5*dt*dt*Matrix3d::Identity();
     Ga.block<3,3>(3,0) = dt*Matrix3d::Identity();
     Ga.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
 
     Matrix<double,STATESIZE,3> Gb;
-    if (doang) {
+    if (donew) {
         Gb.block<3,3>(0,0) = -0.5*dt*dt*Matrix3d::Identity();
         Gb.block<3,3>(3,0) = -dt*Matrix3d::Identity();
         Gb.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
+        Gb.block<3,3>(STATESIZE-3,0) = dt*Matrix3d::Identity();
     } else {
         Gb.block<STATESIZE-3,3>(0,0) = Matrix<double,STATESIZE-3,3>::Zero();
+        Gb.block<3,3>(STATESIZE-3,0) = 0.5*dt*dt*Matrix3d::Identity();
     }
-    Gb.block<3,3>(STATESIZE-3,0) = dt*Matrix3d::Identity();
 
     Matrix<double,STATESIZE,3> Gw;
-    Gw.block<3,3>(0,0) = 0.5*dt*dt*skewSymmetric(vel());
-    Gw.block<3,3>(3,0) = dt*skewSymmetric(vel());
-    Gw.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
-
-#if STATESIZE==13
-    Matrix<double,STATESIZE,Eigen::Dynamic> Gq;
-    if (doquat) {
-        Gq = Matrix<double,STATESIZE,3>::Zero();
-        Quaterniond q = qhat();
-
-        Gq.block<6,3>(0,0) = Matrix<double,6,3>::Zero();
-        Gq.block<4,3>(6,0) = 0.5*dt*qomega(q);
-        Gq.block<3,3>(10,0) = Matrix<double,3,3>::Zero();
+    if (donew) {
+        Gw.block<3,3>(0,0) = 0.5*dt*dt*skewSymmetric(vel());
+        Gw.block<3,3>(3,0) = dt*skewSymmetric(vel());
+        Gw.block<3,3>(STATESIZE-3,0) = Matrix3d::Zero();
     } else {
-        Gq = Matrix<double,STATESIZE,4>::Zero();
-        Gq.block<6,4>(0,0) = Matrix<double,6,4>::Zero();
-        Gq.block<4,4>(6,0) = dt*Matrix<double,4,4>::Identity();
-        Gq.block<3,4>(10,0) = Matrix<double,3,4>::Zero();
+        Gw.block<6,0>(0,0) = Matrix<double,6,0>::Zero();
+        Gw.block<3,3>(STATESIZE-3,0) = Matrix3d::Zero();
     }
+
+#if STATESIZE==13
+    Quaterniond q = qhat();
+    Gw.block<4,3>(6,0) = 0.5*dt*qomega(q);
 #endif
     Matrix<double,STATESIZE,STATESIZE> Q;
     Q = Matrix<double,STATESIZE,STATESIZE>::Zero();
-    if (doang) {
+    if (donew) {
         Q += acc_std*acc_std*Ga*Ga.transpose();
-        //Q += ang_std*ang_std*Gw*Gw.transpose();
+        Q += ang_std*ang_std*Gw*Gw.transpose();
+        Q += acc_bias_std*acc_bias_std*Gb*Gb.transpose();
     } else {
         Q += (acc_bias_std+ang_std+acc_std)*(acc_std+acc_bias_std+ang_std)*Ga*Ga.transpose();
+        Q += ang_std*ang_std*Gw*Gw.transpose();
+        Q += acc_bias_std*acc_bias_std*Gb*Gb.transpose();
     }
-    Q += acc_bias_std*acc_bias_std*Gb*Gb.transpose();
-
-#if STATESIZE==13
-    Q += ang_std*ang_std *(Gq+Gw) * (Gq+Gw).transpose();
-#endif
 
-
-
-    /*
-    Matrix<double,STATESIZE,STATESIZE> Q;
-    Q = Matrix<double,STATESIZE,STATESIZE>::Zero();
-    // Qxx
-    Q.block<3,3>(0,0) = 0.25*dt*dt*dt*dt*ACC_STD*ACC_STD*Matrix<double,3,3>::Identity();
-
-    // Qxv, Qvx
-    Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix3d::Identity()-0.25*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
-    Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix3d::Identity()-0.25*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
-    
-    //Qvv
-    Q.block<3,3>(3,3) = dt*dt*(ACC_STD*ACC_STD+0.25*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix<double,3,3>::Identity();
-
-#if STATESIZE==13
-    // Qbb
-    Q.block<3,3>(10,10) = dt*dt*ACC_BIAS_STD*ACC_BIAS_STD*Matrix<double,3,3>::Identity();
-
-    // Qxb, Qbx
-    Q.block<3,3>(10,0) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
-    Q.block<3,3>(0,10) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
-    
-    // Qvb, Qbv 
-    Q.block<3,3>(3,10) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity();
-    Q.block<3,3>(10,3) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity();
-    
-    // Qqq
-    Q.block<4,4>(6,6) = 0.25*dt*dt*ANG_STD*ANG_STD*Matrix<double,4,4>::Identity();
-#else
-    // Qbb
-    Q.block<3,3>(6,6) = dt*dt*ACC_BIAS_STD*ACC_BIAS_STD*Matrix<double,3,3>::Identity();
-
-    // Qxb, Qbx
-    Q.block<3,3>(6,0) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
-    Q.block<3,3>(0,6) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
-    
-    // Qvb, Qbv 
-    Q.block<3,3>(3,6) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity();
-    Q.block<3,3>(6,3) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity();
-#endif
-*/
     return Q;
 }		/* -----  end of method Body::q  ----- */
 
diff --git a/src/main.cpp b/src/main.cpp
index 04e1910..edfc107 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -51,6 +51,8 @@ double accbiasz = -0.006310;
 double xcorrsrc = 0.9;
 double xcorrref = 0.4;
 
+bool donew=true;
+
 using std::cout;
 using std::endl;
 using std::cerr;
@@ -446,8 +448,8 @@ update_dt(const timestamp t, timestamp *t_old)
 int 
 main(int argc, char **argv)
 {
-    if (argc!=3) {
-        fprintf(stderr, "Usage: %s camera params\n", argv[0]);
+    if (argc<3) {
+        fprintf(stderr, "Usage: %s camera params [donew]\n", argv[0]);
         exit(1);
     }
     State mu;
@@ -501,6 +503,15 @@ main(int argc, char **argv)
     Vector3d acc_bias;
     acc_bias << accbiasx, accbiasy, accbiasz;
     mu.accelerometer_bias(acc_bias);
+
+    // check for donew
+    if (argc==4) {
+        if (!strcmp(argv[3],"false")) {
+            donew=false;
+            cout << "setting old process noise" << endl;
+            exit(EXIT_FAILURE);
+        }
+    }
     // Read sensors from file
     int i=0;
     char line[MAXLINE];
diff --git a/src/types.h b/src/types.h
index 19b69cc..86d9398 100644
--- a/src/types.h
+++ b/src/types.h
@@ -43,6 +43,8 @@ extern double accbiasz;
 extern double xcorrsrc;
 extern double xcorrref;
 
+extern bool donew;
+
 typedef Eigen::Matrix<double,2,1,Eigen::DontAlign> UVector2d;
 // A struct for storing measurements.
 typedef enum {BOTH,REFLECTION,MONO,HEIGHT} measurement_type;