1 files changed, 38 insertions, 26 deletions

diff --git a/src/main.cpp b/src/main.cpp
index 838bab8..c3c6fa9 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -22,7 +22,6 @@
 #include "main.h"
 bool seenutm=false;
 bool seenpva=false;
-Eigen::Matrix<double,9,9> P;
 using std::cout;
 using std::endl;
 using std::cerr;
@@ -31,13 +30,15 @@ using std::cerr;
 
 #else      /* -----  not USE_ROS  ----- */
 void
-covCallback(const message &msg, Eigen::Matrix<double,9,9> &P, const Eigen::Quaternion<double> &q)
+covCallback(const message &msg, Matrix<double,9,9> &P, const Quaternion<double> &q)
 {
-    Eigen::Matrix3d Rbw(q.toRotationMatrix());
-    Eigen::Matrix3d Renuned;
-    Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
-    P.block(0,0,3,3) = Renuned*msg.covariance.position*Renuned.transpose();
-    P.block(3,3,3,3) = Rbw.transpose()*msg.covariance.velocity*Rbw;
+    if (seenutm==false || seenpva==false) {
+        Matrix3d Rbw(q.toRotationMatrix());
+        Matrix3d Renuned;
+        Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
+        P.block(0,0,3,3) = Renuned*msg.covariance.position*Renuned.transpose();
+        P.block(3,3,3,3) = Rbw.transpose()*msg.covariance.velocity*Rbw;
+    }
 }
 
 void
@@ -47,12 +48,11 @@ imgCallback(const message *msg)
 }
 
 void 
-imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, 
-        Eigen::Matrix<double,9,9> &P, const Eigen::Quaternion<double> &q,
+imuCallback(const message &msg, Matrix<double,9,1> &X, 
+        Matrix<double,9,9> &P, const Quaternion<double> &q,
         const timestamp dt)
 {
     if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return;
-    using Eigen::Vector3d;
     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
@@ -62,43 +62,54 @@ imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X,
     State mu;
     double dtf = dt.secs+dt.nsecs*1e-9;
 
-    Eigen::Matrix<double,9,9> F;
+    Matrix<double,9,9> F;
     F = body.F(X,ang,q);
     mu.Pkk1(P,F,body.Q(dtf));
     body.motionModel(X,acc,ang,q,dtf);
 }
 
 void
-pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q)
+pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q)
 {
+    // Update quaternion
     using Eigen::AngleAxisd;
-    using Eigen::Vector3d;
     double roll, pitch, yaw;
     attitude_t A=msg.attitude;
     roll = M_PI*msg.attitude.roll/180.;
     pitch = M_PI*msg.attitude.pitch/180.;
     yaw = M_PI*(msg.attitude.yaw)/180.;
-    Eigen::Matrix3d R;
+    Matrix3d R;
     R = AngleAxisd(yaw, Vector3d::UnitZ())
       * AngleAxisd(roll, Vector3d::UnitX())
       *  AngleAxisd(pitch, Vector3d::UnitY());
-    q = Eigen::Quaternion<double>(R);
+    q = Quaternion<double>(R);
+
+    // Set body velocity
     if (!seenpva) {
         Vector3d vw(msg.velocity.north,msg.velocity.east,-msg.velocity.up);
         X.segment(3,3) = R.transpose()*vw;
         seenpva=true;
-    }
+    } 
     return;
 }
 
 void
-utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X)
+utmCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P)
 {
     if (!seenutm) {
         seenutm=true;
         X[0] = msg.utm.northing;
         X[1] = msg.utm.easting;
         X[2] = -msg.utm.up;
+    } else { // Perform Z measurement
+        State mu;
+        Body body;
+        Matrix<double,1,1> z,h;
+        Matrix<double,1,9> H;
+        z(0) = -msg.utm.up;
+        h = body.h(X);
+        body.H(H);
+        mu.update(X,P,H,h,z,body.R());
     }
     return;
 }
@@ -135,8 +146,6 @@ parseLine(char *line, message *data)
         rfn = strsep(&line, ",");
         sscanf(rfn, "%s", &data->image_names+1);
     } else if (!strcmp(msg_type,"inscovs")) {
-        using Eigen::Map;
-        using Eigen::Matrix3d;
         double pos[9];
         double att[9];
         double vel[9];
@@ -220,18 +229,18 @@ update_dt(const timestamp t, timestamp *t_old)
 int 
 main(int argc, char **argv)
 {
-    Eigen::Quaternion<double> qbw;
-    Eigen::Matrix<double,9,1> bodyStateVector;
-    Eigen::Matrix<double,9,9> P;
+    Quaternion<double> qbw;
+    Matrix<double,9,1> bodyStateVector;
+    Matrix<double,9,9> P;
     // bias in FRD coordinates
-    bodyStateVector.segment(6,3) << 0.975*-0.03713532, 0.9*0.01465135, -0.00709229;
+    bodyStateVector.segment(6,3) << -0.03713532, 1.125*0.01465135, -1*-0.00709229;
     timestamp dt{0,0};
     timestamp t_old{0,0};
 
 #ifdef  USE_ROS
     
 #else      /* -----  not USE_ROS  ----- */
-printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East\n");
+printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z\n");
 // Read sensors from file
 char line[MAXLINE];
 while (scanf("%s", line)!=EOF) {
@@ -241,7 +250,7 @@ while (scanf("%s", line)!=EOF) {
     switch ( msg.msg_type ) {
         case BESTUTM:	
             dt = update_dt(msg.stamp, &t_old);
-            utmCallback(msg,bodyStateVector);
+            utmCallback(msg,bodyStateVector,P);
             break;
 
         case IMG:	
@@ -253,7 +262,10 @@ while (scanf("%s", line)!=EOF) {
             pvaCallback(msg, bodyStateVector, qbw);
             //std::cout << bodyStateVector.transpose() << std::endl;
             if (seenutm)
-                printf("%d,%c,%f,%f\n", msg.utm.zone_i,msg.utm.zone_c,bodyStateVector[0],bodyStateVector[1]);
+                printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", msg.utm.zone_i,msg.utm.zone_c,
+                        bodyStateVector[0],bodyStateVector[1],-bodyStateVector[2],
+                        bodyStateVector[3],bodyStateVector[4],bodyStateVector[5],
+                        bodyStateVector[6],bodyStateVector[7],bodyStateVector[8]);
             break;
 
         case RAWIMUS: