changes

7 files changed, 14 insertions, 6 deletions

diff --git a/src/body.h b/src/body.h
index a891e92..6641ab5 100644
--- a/src/body.h
+++ b/src/body.h
@@ -5,7 +5,7 @@
 #include "types.h"
 #define R_HEIGHT 2.5e-3            /* measurement noise of height measurement */
 #define ACC_STD 28e-3
-#define ANG_STD 20e-3
+#define ANG_STD 9e-3
 #define ACC_BIAS_STD 22e-3
 #define ANG_BIAS_STD 7e-6
 #define IMU_NOISE 800e-6            /* IMU process noise */
diff --git a/src/camera.h b/src/camera.h
index d7dd6b6..d038069 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -6,7 +6,7 @@
 
 #define BINNING 0.5 // set the binning factor
 #define YAWCORRECT 2.0
-#define DOYAWCORRECT
+//#define DOYAWCORRECT
 using Eigen::Matrix;
 using Eigen::Vector4d;
 using Eigen::Vector3d;
diff --git a/src/feature.h b/src/feature.h
index 5ffdeb1..eaf7349 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -13,7 +13,7 @@
 #define FEATURECOVRHO 25e-4            /*  */
 #define FEATURECOVRHO_MONO 0.5            /*  */
 #define RHO_0 1./10.            /*  */
-#define INLIER_THRESHOLD 5.9915            /*  */
+#define INLIER_THRESHOLD 2.9915            /*  */
 #define RANSAC_LI_THRESHOLD 4e-5            /*  */
 #define RANSAC_HI_THRESHOLD 5e-2            /*  */
 //#define INITDEPTH
diff --git a/src/main.cpp b/src/main.cpp
index 49b6771..e37c2a9 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -264,6 +264,9 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q)
 #endif
         seenpva=true;
     } 
+#if STATESIZE==13
+        mu.qhat(q);
+#endif
     return;
 }
 
@@ -481,6 +484,7 @@ main(int argc, char **argv)
 #else
                 utmCallback(msg, mu,qbw);
 #endif
+                if (seenutm) mu.unicsv();
                 //printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
                 break;
 
@@ -495,7 +499,6 @@ main(int argc, char **argv)
             case INSPVAS:	
                 att = msg.attitude;
                 pvaCallback(msg, mu, qbw);
-                if (seenutm) mu.unicsv();
                 break;
 
             case RAWIMUS:	
diff --git a/src/main.h b/src/main.h
index 5fa7ee6..94be379 100644
--- a/src/main.h
+++ b/src/main.h
@@ -20,9 +20,10 @@
 
 #define ANGBIASX -2.795871394666666222e-03            /*  */
 #define ANGBIASY 6.984255690000021506e-03
+#define ANGBIASZ 1.418145565750002614e-03
 //#define ANGBIASX 1.019*-2.795871394666666222e-03            /*  */
 //#define ANGBIASY .980*6.984255690000021506e-03
-#define ANGBIASZ 1.418145565750002614e-03
+//#define ANGBIASZ 0.96*1.418145565750002614e-03
 
 //#define ACCBIASX 0.95*-0.03713532
 //#define ACCBIASY 0.65*0.01465135
diff --git a/src/state.cpp b/src/state.cpp
index 0bae8e7..937a653 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -204,6 +204,10 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S,
     // Get the update
     Matrix<double,Dynamic,1> dx;
     dx = K*y;
+    if (dx.segment<3>(3).norm()>0.04) {
+        cerr << "dx: " << dx.head<STATESIZE>().transpose() << endl;
+        return K;
+    }
     assert (dx.rows()==STATESIZE+3*features.size());
     body->dx(dx.segment<STATESIZE>(0));
     {
diff --git a/src/types.h b/src/types.h
index 3c1a962..1248b80 100644
--- a/src/types.h
+++ b/src/types.h
@@ -4,7 +4,7 @@
 #include <Eigen/Dense>
 #define MAXLINE 8192
 #define MAXFILENAME 1024
-#define STATESIZE 9            /* Set to 13 for quaternion estimation, or 9 for quaternion as input  */
+#define STATESIZE 13            /* Set to 13 for quaternion estimation, or 9 for quaternion as input  */
 using Eigen::Matrix;
 using Eigen::Matrix3d;
 using Eigen::Quaterniond;