update

1 files changed, 21 insertions, 11 deletions

diff --git a/src/main.cpp b/src/main.cpp
index 897a319..4e61484 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -50,10 +50,13 @@ covCallback(const message &msg, State &mu, const Quaterniond &q)
     // Rotation from ENU to NED
     Matrix3d Renuned;
     Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
-    mu.position_covariance(Renuned*msg.covariance.position*Renuned.transpose());
+    Matrix3d pcov = Renuned*msg.covariance.position*Renuned.transpose();
+    pcov(2,2) *= 10;
+    mu.position_covariance(pcov);
     mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw);
 }
 
+
 /* 
  * ===  FUNCTION  ======================================================================
  *         Name:  imgCallback
@@ -67,13 +70,9 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
     if (seenutm && seenpva && seencov) {
         std::vector<measurement_t> z;
 
-        Vector3d tip;
-        tip << 0.6*3.43, 0, -0.34;
-        tip = q._transformVector(tip);
-
         measurement_t height;
         height.z_type = HEIGHT;
-        height.height = tip[2];
+        height.height = aboveWater(q);
         z.push_back(height);
 
         strcat(msg.image_names[0],".txt");
@@ -167,19 +166,30 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q)
 void
 utmCallback(const message &msg, State &mu, const Quaterniond &q)
 {
-    Vector3d tip;
-    tip << 0.6*3.43, 0, -0.34;
-    tip = q._transformVector(tip);
+    static int i=0;
     if ((!seenutm || !seencov) && seenpva) {
         seenutm=true;
         UTM utm_water;
         utm_water.northing = msg.utm.northing;
         utm_water.easting = msg.utm.easting;
-        utm_water.up = -tip[2];
+        utm_water.up = -aboveWater(q);
         utm_water.zone_i = msg.utm.zone_i;
         utm_water.zone_c = msg.utm.zone_c;
         mu.pos(utm_water);
-    } 
+    } else {
+        /*
+        i+=1;
+        if (i%18==0) {
+            UTM utm_water;
+            utm_water.northing = msg.utm.northing;
+            utm_water.easting = msg.utm.easting;
+            utm_water.up = -aboveWater(q);
+            utm_water.zone_i = msg.utm.zone_i;
+            utm_water.zone_c = msg.utm.zone_c;
+            mu.pos(utm_water);
+        }
+        */
+    }
     return;
 }