10pct mean error

5 files changed, 25 insertions, 18 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 885b2ad..018075b 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -125,11 +125,12 @@ Body::Q (double dt)
     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();
 
     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());
-    Gb.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
+    Gw.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
 
 #if STATESIZE==13
     Matrix<double,STATESIZE,3> Gq;
diff --git a/src/body.h b/src/body.h
index 1e64d86..09c8e10 100644
--- a/src/body.h
+++ b/src/body.h
@@ -7,7 +7,7 @@
 #define ACC_STD 28e-3
 #define ANG_STD 9e-3
 #define ACC_BIAS_STD 22e-3
-#define ANG_BIAS_STD 7e-6
+#define ANG_BIAS_STD 3e-3
 #define IMU_NOISE 800e-6            /* IMU process noise */
 #define IMU_RANDOMWALK 50e-6      /* Bias process noise */
 //#define DOCLAMP
diff --git a/src/camera.h b/src/camera.h
index 7a1e7ee..9cb8254 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -9,7 +9,7 @@
 #include "types.h"
 #define BINNING 0.5 // set the binning factor
 #define YAWCORRECT 1.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 05c268c..ca6944b 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -7,17 +7,17 @@
 #include "camera.h"
 #include "types.h"
 
-#define FEATURE_NOISE  1           /* Feature process noise */
-#define VIEW_NOISE 1e-2             /*  */
-#define INITIAL_VIEW_NOISE 1e-2           /*  */
-#define REFLECTION_VIEW_NOISE 1e-1            /*  */
-#define FEATURECOVX .001            /*  */
-#define FEATURECOVY .001           /*  */
+#define FEATURE_NOISE  2.0           /* Feature process noise */
+#define VIEW_NOISE 6e-3             /*  */
+#define INITIAL_VIEW_NOISE 1e-3           /*  */
+#define REFLECTION_VIEW_NOISE 6e-3            /*  */
+#define FEATURECOVX .0001            /*  */
+#define FEATURECOVY .0001           /*  */
 #define FEATURECOVRHO 25e-4            /*  */
 #define FEATURECOVRHO_MONO 0.5            /*  */
 #define RHO_0 1./10.            /*  */
 #define RANSAC_LI_THRESHOLD 4e-6            /*  */
-#define RANSAC_HI_THRESHOLD 5e-2            /*  */
+#define RANSAC_HI_THRESHOLD 1e-2            /*  */
 #define INITDEPTH
 
 using cv::Mat;
diff --git a/src/main.h b/src/main.h
index e477b69..6af6b6a 100644
--- a/src/main.h
+++ b/src/main.h
@@ -18,9 +18,14 @@
 //#define ROS_PUBLISH          /* Uncomment to publish ROS node */
 //#define ROS_SUBSCRIBE            /* Uncomment to subscribe to ROS */
 
-#define ANGBIASX -2.795871394666666222e-03            /*  */
-#define ANGBIASY 6.984255690000021506e-03
-#define ANGBIASZ 1.418145565750002614e-03
+#define ANGBIASX 0.001107
+#define ANGBIASY 0.008842
+#define ANGBIASZ -0.001122
+#define ACCBIASX -0.018000
+#define ACCBIASY 0.012100
+#define ACCBIASZ -0.010000
+
+
 //#define ANGBIASX 1.019*-2.795871394666666222e-03            /*  */
 //#define ANGBIASY .980*6.984255690000021506e-03
 //#define ANGBIASZ 0.96*1.418145565750002614e-03
@@ -33,12 +38,13 @@
 //#define ACCBIASY 0.2*0.01465135
 //#define ACCBIASZ -1*-0.00709229
 
-#define ACCBIASX -0.03713532
-#define ACCBIASY 0.01465135
-#define ACCBIASZ -1*-0.00709229
+//#define ACCBIASX -0.03713532
+//#define ACCBIASY 0.01465135
+//#define ACCBIASZ -1*-0.00709229
 
-#define CANOECENTER 0.82            /* center of gravity of canoe  */
-#define CANOEHEIGHT -0.46
+//#define CANOECENTER 0.82            /* center of gravity of canoe  */
+#define CANOECENTER 0.65            /* center of gravity of canoe  */
+#define CANOEHEIGHT -0.50
 //#define CANOEHEIGHT -0.75
 #define DOWNSAMPLE 1            /*  */
 #define HEIGHT_FROM_ATTITUDE    /* use the attitude to measure the height */