1 files changed, 33 insertions, 50 deletions

diff --git a/src/main.cpp b/src/main.cpp
index c3c6fa9..6cc4317 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -22,6 +22,7 @@
 #include "main.h"
 bool seenutm=false;
 bool seenpva=false;
+bool seencov=false;
 using std::cout;
 using std::endl;
 using std::cerr;
@@ -30,15 +31,15 @@ using std::cerr;
 
 #else      /* -----  not USE_ROS  ----- */
 void
-covCallback(const message &msg, Matrix<double,9,9> &P, const Quaternion<double> &q)
+covCallback(const message &msg, State &mu, const Quaternion<double> &q)
 {
-    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;
-    }
+    if (seencov && seenutm && seenpva) return;
+    seencov=true;
+    Matrix3d Rbw(q.toRotationMatrix());
+    Matrix3d Renuned;
+    Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
+    mu.position_covariance(Renuned*msg.covariance.position*Renuned.transpose());
+    mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw);
 }
 
 void
@@ -48,28 +49,21 @@ imgCallback(const message *msg)
 }
 
 void 
-imuCallback(const message &msg, Matrix<double,9,1> &X, 
-        Matrix<double,9,9> &P, const Quaternion<double> &q,
-        const timestamp dt)
+imuCallback(const message &msg, State &mu, const Quaternion<double> &q, const timestamp dt)
 {
-    if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return;
+    if (!seenutm || !seenpva || !seencov || (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
     Vector3d ang_bias(-2.795871394666666222e-03, 6.984255690000021506e-03, 1.418145565750002614e-03);
-    ang-=ang_bias;
-    Body body;
-    State mu;
+    ang -= ang_bias;
     double dtf = dt.secs+dt.nsecs*1e-9;
 
-    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);
+    mu.motionModel(acc,ang,q,dtf);
 }
 
 void
-pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q)
+pvaCallback(const message &msg, State &mu, Quaternion<double> &q)
 {
     // Update quaternion
     using Eigen::AngleAxisd;
@@ -85,31 +79,24 @@ pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q)
     q = Quaternion<double>(R);
 
     // Set body velocity
-    if (!seenpva) {
+    if (!seenpva || !seencov || !seenutm) {
         Vector3d vw(msg.velocity.north,msg.velocity.east,-msg.velocity.up);
-        X.segment(3,3) = R.transpose()*vw;
+        mu.vel(R.transpose()*vw);
         seenpva=true;
     } 
     return;
 }
 
 void
-utmCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P)
+utmCallback(const message &msg, State &mu)
 {
-    if (!seenutm) {
+    if (!seenutm || !seencov || !seenpva) {
         seenutm=true;
-        X[0] = msg.utm.northing;
-        X[1] = msg.utm.easting;
-        X[2] = -msg.utm.up;
+        mu.enu(msg.utm);
     } 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());
+        Matrix<double,1,1> z;
+        z << -msg.utm.up;
+        mu.update(z);
     }
     return;
 }
@@ -229,11 +216,12 @@ update_dt(const timestamp t, timestamp *t_old)
 int 
 main(int argc, char **argv)
 {
+    State mu;
     Quaternion<double> qbw;
-    Matrix<double,9,1> bodyStateVector;
-    Matrix<double,9,9> P;
     // bias in FRD coordinates
-    bodyStateVector.segment(6,3) << -0.03713532, 1.125*0.01465135, -1*-0.00709229;
+    Vector3d acc_bias;
+    acc_bias << -0.03713532, 1.125*0.01465135, -1*-0.00709229;
+    mu.accelerometer_bias(acc_bias);
     timestamp dt{0,0};
     timestamp t_old{0,0};
 
@@ -249,8 +237,7 @@ while (scanf("%s", line)!=EOF) {
     
     switch ( msg.msg_type ) {
         case BESTUTM:	
-            dt = update_dt(msg.stamp, &t_old);
-            utmCallback(msg,bodyStateVector,P);
+            utmCallback(msg, mu);
             break;
 
         case IMG:	
@@ -258,23 +245,19 @@ while (scanf("%s", line)!=EOF) {
             break;
 
         case INSPVAS:	
-            dt = update_dt(msg.stamp, &t_old);
-            pvaCallback(msg, bodyStateVector, qbw);
-            //std::cout << bodyStateVector.transpose() << std::endl;
-            if (seenutm)
-                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]);
+            pvaCallback(msg, mu, qbw);
+            if (seenutm && seencov && seenpva)
+                mu.unicsv();
             break;
 
         case RAWIMUS:	
             dt = update_dt(msg.stamp, &t_old);
-            imuCallback(msg, bodyStateVector, P, qbw, dt);
+            imuCallback(msg, mu, qbw, dt);
             break;
 
         case INSCOVS:
-            covCallback(msg, P, qbw);
+            covCallback(msg, mu, qbw);
+            break;
 
         default:	
             break;