changes

7 files changed, 61 insertions, 16 deletions

diff --git a/src/body.cpp b/src/body.cpp
index f909a39..0bbe18f 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -42,6 +42,9 @@ Body::dx ( const Matrix<double,STATESIZE,1> &del )
 {
     X += del;
     clamp();
+#if STATESIZE==13
+    normalizeQuaternion();
+#endif
     return ;
 }		/* -----  end of method Body::dx  ----- */
 
@@ -120,7 +123,7 @@ Body::Q (double dt)
     Q.block<3,3>(3,3) = dt*dt*Matrix<double,3,3>::Identity();
     Q *= IMU_NOISE;
 #if STATESIZE==13
-    Q.block<4,4>(6,6) = dt*dt*IMU_NOISE*Matrix<double,4,4>::Identity();
+    Q.block<4,4>(6,6) = 0.25*dt*dt*IMU_NOISE*Matrix<double,4,4>::Identity();
     Q.block<3,3>(10,10) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::Identity();
 #else
     Q.block<3,3>(6,6) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::Identity();
@@ -195,6 +198,7 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
     v1 = X(3);
     v2 = X(4);
     v3 = X(5);
+    double gw = gravity_world[2];
     F.block<1,4>(0,6) << 2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3, 
                          2*qbw1*v2 - 2*qbw2*v1 + 2*qbw4*v3, 
                          2*qbw1*v3 - 2*qbw3*v1 - 2*qbw4*v2, 
@@ -207,10 +211,10 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
                          2*qbw3*v2 - 2*qbw2*v3 - 2*qbw4*v1,
                          2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3,
                          2*qbw1*v2 - 2*qbw2*v1 + 2*qbw4*v3;
-    F.block<3,4>(3,6) << -2*gravity_world*qbw3,  2*gravity_world*qbw4, -2*gravity_world*qbw1,  2*gravity_world*qbw2,
-                         -2*gravity_world*qbw4, -2*gravity_world*qbw3, -2*gravity_world*qbw2, -2*gravity_world*qbw1,
-                          2*gravity_world*qbw1,  2*gravity_world*qbw2, -2*gravity_world*qbw3, -2*gravity_world*qbw4;
-    F.block<4,4>(6,6) =  omega(ang);
+    F.block<3,4>(3,6) << -2*gw*qbw3,  2*gw*qbw4, -2*gw*qbw1,  2*gw*qbw2,
+                         -2*gw*qbw4, -2*gw*qbw3, -2*gw*qbw2, -2*gw*qbw1,
+                          2*gw*qbw1,  2*gw*qbw2, -2*gw*qbw3, -2*gw*qbw4;
+    F.block<4,4>(6,6) =  0.5*omega(ang);
     F.block<3,3>(3,10) = -Matrix<double,3,3>::Identity();
 #else
     F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity();
@@ -234,7 +238,7 @@ Body::omega ( const Vector3d &x )
     Matrix<double,4,4> Omega;
     Omega.block<3,3>(0,0) = -skewSymmetric(x);
     Omega.block<3,1>(0,3) = x;
-    Omega.block<1,3>(3,0) = x.transpose();
+    Omega.block<1,3>(3,0) = -x.transpose();
     Omega(3,3) = 0;
     return Omega;
 }		/* -----  end of method Body::omega  ----- */
@@ -269,10 +273,15 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond
     W = skewSymmetric(ang);
     A.block<3,3>(3,0) = -W;
     b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world;
+    Matrix<double,6,1> dX;
+    dX = (A*X.segment<3>(3)+b)*dt;
 
     X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt;
 #if STATESIZE==13
-    X.segment<4>(6) += 0.5*omega(ang)*X.segment<4>(6);
+    Matrix<double,4,1> dq;
+    dq = dt*0.5*omega(ang)*X.segment<4>(6);
+    X.segment<4>(6) += dq;
+    normalizeQuaternion();
 #endif
     clamp();
     return ;
@@ -289,17 +298,27 @@ Matrix<double,3,3>
 Body::skewSymmetric ( const Vector3d &x )
 {
     Matrix<double,3,3> y;
-    y << 0.,-x[2], x[1], x[2],0.,-x[0],-x[1],x[0],0.;
+    y << 0.,-x[2], x[1],
+       x[2],0.,-x[0],
+       -x[1],x[0],0.;
     return y;
 }		/* -----  end of method Body::skewSymmetric  ----- */
 
 void
 Body::unicsv ( )
 {
+#if STATESIZE==13
+    printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", utm_i, utm_c,
+            X[0], X[1], -X[2],
+            X[3], X[4], X[5],
+            X[6], X[7], X[8],X[9],
+            X[10],X[11],X[12]);
+#else
     printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f", utm_i, utm_c,
             X[0], X[1], -X[2],
             X[3], X[4], X[5],
             X[6], X[7], X[8]);
+#endif
     return ;
 }		/* -----  end of method Body::unicsv  ----- */
 
@@ -384,6 +403,21 @@ Body::qhat ( const Quaterniond &q )
     return ;
 }		/* -----  end of method Body::qhat  ----- */
 
+void
+Body::normalizeQuaternion (  )
+{
+    Quaterniond q = qhat();
+    Matrix<double,4,1> qv;
+    qv[0] = q.x();
+    qv[1] = q.y();
+    qv[2] = q.z();
+    qv[3] = q.w();
+    qv /= qv.norm();
+    X.segment<4>(6) = qv;
+    //qhat(q.normalized());
+    return ;
+}		/* -----  end of method Body::normalizequaternion  ----- */
+
 #endif
 
 
diff --git a/src/body.h b/src/body.h
index 23ed2ca..4653588 100644
--- a/src/body.h
+++ b/src/body.h
@@ -14,6 +14,9 @@
 using Eigen::Matrix;
 using Eigen::Vector3d;
 using Eigen::Quaterniond;
+using std::cout;
+using std::cerr;
+using std::endl;
 
 /*
  * =====================================================================================
@@ -32,6 +35,7 @@ class Body
         Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, double dt);
         Quaterniond qhat();
         void qhat(const Quaterniond &q);
+        void normalizeQuaternion();
 #else
         Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, const Quaterniond &q, double dt);
 #endif
diff --git a/src/camera.h b/src/camera.h
index ea006ec..d7dd6b6 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -5,7 +5,7 @@
 #include <yaml-cpp/yaml.h>
 
 #define BINNING 0.5 // set the binning factor
-#define YAWCORRECT 7.0
+#define YAWCORRECT 2.0
 #define DOYAWCORRECT
 using Eigen::Matrix;
 using Eigen::Vector4d;
diff --git a/src/feature.h b/src/feature.h
index 82a48d3..5ffdeb1 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -16,7 +16,7 @@
 #define INLIER_THRESHOLD 5.9915            /*  */
 #define RANSAC_LI_THRESHOLD 4e-5            /*  */
 #define RANSAC_HI_THRESHOLD 5e-2            /*  */
-#define INITDEPTH
+//#define INITDEPTH
 
 using Eigen::Dynamic;
 using Eigen::Matrix;
diff --git a/src/main.cpp b/src/main.cpp
index 11bf6dd..fa0b5fd 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -57,6 +57,7 @@ covCallback(const message &msg, State &mu, const Quaterniond &q)
 #endif
 {
     if (seencov && seenutm && seenpva) return;
+    if (!seenpva) return;
     seencov=true;
     // Rotation from body to world
 #if STATESIZE==13
@@ -153,7 +154,7 @@ imuCallback(const message &msg, State &mu, const timestamp dt)
 imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt)
 #endif
 {
-    if (!seenutm || !seenpva || !seencov || (dt.secs==0 && dt.nsecs==0)) return;
+    if (dt.secs==0 && dt.nsecs==0) return;
     Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z);
     Vector3d ang(msg.angular_velocity.y,msg.angular_velocity.x,-msg.angular_velocity.z);
     // WARNING: This is the bias for 1112-1
@@ -190,6 +191,7 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q)
       * AngleAxisd(roll, Vector3d::UnitX())
       *  AngleAxisd(pitch, Vector3d::UnitY());
     q = Quaterniond(R);
+    //cout <<q.x() <<" " << q.y() << " " << q.z() << " " << q.w() << endl;
 
     // Set body velocity
     if (!seenpva || !seencov || !seenutm) {
@@ -396,7 +398,11 @@ main(int argc, char **argv)
     ros::Publisher ekfPose = n.advertise<geometry_msgs::PoseStamped>("ekfPose", 100);
 #else      /* -----  not ROS_PUBLISH  ----- */
     // Print unicsv header
-    printf("GPS,UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
+#if STATESIZE==13
+    printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Quat-X,Quat-Y,Quat-Z,Quat-W,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
+#else
+    printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
+#endif
 #endif     /* -----  not ROS_PUBLISH  ----- */
     // Read sensors from file
     int i=0;
@@ -412,8 +418,7 @@ main(int argc, char **argv)
 #else
                 utmCallback(msg, mu,qbw);
 #endif
-                printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
-                mu.unicsv();
+                //printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
                 break;
 
             case IMG:	
@@ -426,6 +431,7 @@ main(int argc, char **argv)
 
             case INSPVAS:	
                 pvaCallback(msg, mu, qbw);
+                if (seenutm) mu.unicsv();
                 break;
 
             case RAWIMUS:	
diff --git a/src/main.h b/src/main.h
index 521bce0..f78158d 100644
--- a/src/main.h
+++ b/src/main.h
@@ -20,7 +20,8 @@
 
 #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 ACCBIASX 0.95*-0.03713532
diff --git a/src/state.cpp b/src/state.cpp
index e53ad29..8b69ca2 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -1026,7 +1026,7 @@ State::qhat ( const Quaterniond &q )
 void
 State::quaternion_covariance (  )
 {
-    P.block<4,4>(6,6) = 1e-5*Matrix<double,4,4>::Identity();
+    P.block<4,4>(6,6) = 1e-12*Matrix<double,4,4>::Identity();
     return ;
 }		/* -----  end of method State::quaternion_covariance  ----- */