update

9 files changed, 76 insertions, 27 deletions

diff --git a/src/body.cpp b/src/body.cpp
index ff54e89..6985278 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -31,7 +31,7 @@ Body::dx ( const Matrix<double,9,1> &del )
     // Constrain the height
     if (X[2]<-1.) {
         X[2]=-1.;
-    } else if (X[2]>-0.2) {
+    } else if (X[2]>-0.3) {
         X[2]=-0.2;
     }
     return ;
@@ -90,7 +90,7 @@ Matrix<double,1,1>
 Body::R()
 {
     Matrix<double,1,1> R;
-    R << 1e-2;
+    R << 1e-3;
     return R;
 }
 
@@ -110,8 +110,8 @@ Body::Q (double dt)
     Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
     Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
     Q.block<3,3>(3,3) = dt*dt*Matrix<double,3,3>::Identity();
-    Q *= 800e-5;
-    Q.block<3,3>(6,6) = dt*dt*5e-7*Matrix<double,3,3>::Identity();
+    Q *= 800e-6;
+    Q.block<3,3>(6,6) = dt*dt*5e-6*Matrix<double,3,3>::Identity();
     return Q;
 }		/* -----  end of method Body::q  ----- */
 
diff --git a/src/feature.cpp b/src/feature.cpp
index 5963ae5..ffce145 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -26,13 +26,20 @@
  *--------------------------------------------------------------------------------------
  */
 Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr, 
-        const Vector3d &xbw, const Quaterniond &q)
+        const Vector3d &xbw, const Quaterniond &q, double z)
 {
-    double z = xbw[2];
     _id = id;
     Vector3d xib;
     xib = findDepth(q,z,xs,xr);
     X = x2p(xib);
+    /*
+    Vector3d pib;
+    xib = xs/xs[0];
+    pib[0] = xib[1];
+    pib[1] = xib[2];
+    pib[2] = .1;
+    X = pib;
+    */
     X0 = X;
     xb0w = xbw;
     q0 = q;
@@ -125,7 +132,7 @@ Feature::Q ( const double dt )
 {
     Matrix<double,3,3> Q;
     Q = Matrix<double,3,3>::Identity();
-    Q *= dt*dt*1e-3;
+    Q *= dt*dt;//*1e-1;
     return Q;
 }		/* -----  end of method Feature::q  ----- */
 
@@ -134,7 +141,7 @@ Feature::R (  )
 {
     Matrix<double,6,6> R;
     R = Matrix<double,6,6>::Identity();
-    R *= 1e-3;
+    R *= 1e-4;
     return R;
 }		/* -----  end of method Feature::R  ----- */
 
@@ -485,8 +492,8 @@ Matrix<double,3,3>
 Feature::P0 ( )
 {
     Matrix<double,3,3> P;
-    double p0 = 1e-2;
-    double p1 = 1e-1;
+    double p0 = 1e-4;
+    double p1 = 1e-4;
     P << p0, 0., 0., 
          0., p0, 0.,
          0., 0., p1;
diff --git a/src/feature.h b/src/feature.h
index fc5b171..c8b835e 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -25,7 +25,7 @@ class Feature
     public:
         /* ====================  LIFECYCLE     ======================================= */
         Feature ( int id, const Vector3d &xs, const Vector3d &xr,
-                const Vector3d &xbw, const Quaterniond &q);
+                const Vector3d &xbw, const Quaterniond &q, double z);
 
         /* ====================  ACCESSORS     ======================================= */
         int id();
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;
 }
 
diff --git a/src/main.h b/src/main.h
index 0746a99..a78bab0 100644
--- a/src/main.h
+++ b/src/main.h
@@ -30,4 +30,5 @@ void imuCallback(const message &msg, State &mu, const Quaterniond &q, const time
 void pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaterniond &q);
 void utmCallback(const message &msg, State &mu, const Quaterniond &q);
 
+
 #endif   /* ----- #ifndef main_INC  ----- */
diff --git a/src/state.cpp b/src/state.cpp
index c33c053..aee0caa 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -21,7 +21,7 @@ State::State ( )
 {
     body = new Body;
     P = Matrix<double,Dynamic,Dynamic>::Zero(9,9);
-    P.block<3,3>(6,6) = 1e-9*Matrix3d::Identity();
+    P.block<3,3>(6,6) = 1e-6*Matrix3d::Identity();
     return ;
 }		/* -----  end of method State::State  ----- */
 
@@ -180,7 +180,7 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S,
 {
     MatrixXd K;
     // P^T is implied here since P is symmetric
-    K = S.partialPivLu().solve(h*P).transpose();
+    K = S.fullPivLu().solve(h*P).transpose();
 
     // Compute P_k|k
     P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P;
@@ -269,7 +269,7 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
             featuresToAdd.push_back(*i);
         }
     }
-    if (featuresToUpdate.size()>0) {
+    if (featuresToUpdate.size()>1) {
 #ifdef  FULLS
         MatrixXd h;
         h = H(q,featuresToUpdate);
@@ -289,11 +289,12 @@ void
 State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
 {
     // Add new features
+    double z = aboveWater(q);
     Vector3d pos = body->ned();
     for (auto i=F.begin(); i!=F.end(); ++i) {
         if (features.size()>MAXFEATURES) break;
         // Create feature
-        Feature *f = new Feature(i->id, i->source, i->reflection, pos, q);
+        Feature *f = new Feature(i->id, i->source, i->reflection, pos, q, z);
         if (!f->sane()) {
             delete f;
             continue;
diff --git a/src/state.h b/src/state.h
index 4001216..89676f3 100644
--- a/src/state.h
+++ b/src/state.h
@@ -9,7 +9,7 @@
 #include "feature.h"
 #include "types.h"
 
-#define MAXFEATURES 50
+#define MAXFEATURES 30
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
 #define FULLS // Comment out to treat each measurement independently.
 using Eigen::Dynamic;
diff --git a/src/types.cpp b/src/types.cpp
new file mode 100644
index 0000000..151fd63
--- /dev/null
+++ b/src/types.cpp
@@ -0,0 +1,29 @@
+/*
+ * =====================================================================================
+ *
+ *       Filename:  types.cpp
+ *
+ *    Description:  
+ *
+ *        Version:  1.0
+ *        Created:  03/27/2017 03:26:51 PM
+ *       Revision:  none
+ *       Compiler:  gcc
+ *
+ *         Author:  Martin Miller (MHM), miller7@illinois.edu
+ *   Organization:  Aerospace Robotics and Controls Lab (ARC)
+ *
+ * =====================================================================================
+ */
+
+
+#include "types.h"
+
+double 
+aboveWater(const Quaterniond &q)
+{
+    Vector3d tip;
+    tip << 0.65*3.43, 0, -0.60;
+    tip = q._transformVector(tip);
+    return tip[2];
+}
diff --git a/src/types.h b/src/types.h
index c284d4a..f32dfea 100644
--- a/src/types.h
+++ b/src/types.h
@@ -79,4 +79,5 @@ typedef struct {
     velocity_t velocity;
     covariance_t covariance;
 } message;
+double aboveWater(const Quaterniond &q);
 #endif   /* ----- #ifndef types_INC  ----- */