From f172db4323c271aa16b883f8a3c571614c53691c Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Mon, 27 Mar 2017 12:58:46 -0500
Subject: Change {State,Body}::enu() to pos()

---
 src/body.cpp  | 12 ++++++------
 src/body.h    |  2 +-
 src/main.cpp  |  2 +-
 src/state.cpp |  6 +++---
 src/state.h   |  2 +-
 5 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/body.cpp b/src/body.cpp
index ae43e58..1fd66d4 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -62,12 +62,12 @@ Body::S ( const Matrix<double,9,9> &Pxx )
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  Body
- *      Method:  Body :: enu
- * Description:  Stores the position input as ENU. Internally stored as NED.
+ *      Method:  Body :: pos
+ * Description:  Stores the UTM position in NED format.
  *--------------------------------------------------------------------------------------
  */
 void
-Body::enu ( const UTM &utm )
+Body::pos ( const UTM &utm )
 {
     utm_c = utm.zone_c;
     utm_i = utm.zone_i;
@@ -75,7 +75,7 @@ Body::enu ( const UTM &utm )
     X[1] = utm.easting;
     X[2] = -utm.up;
     return ;
-}		/* -----  end of method Body::enu  ----- */
+}		/* -----  end of method Body::pos  ----- */
 
 
 /*
@@ -89,7 +89,7 @@ Matrix<double,1,1>
 Body::R()
 {
     Matrix<double,1,1> R;
-    R << 1e-6;
+    R << 1e-2;
     return R;
 }
 
@@ -110,7 +110,7 @@ Body::Q (double dt)
     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-6*Matrix<double,3,3>::Identity();
+    Q.block<3,3>(6,6) = dt*dt*5e-7*Matrix<double,3,3>::Identity();
     return Q;
 }		/* -----  end of method Body::q  ----- */
 
diff --git a/src/body.h b/src/body.h
index 38497e6..4bb9013 100644
--- a/src/body.h
+++ b/src/body.h
@@ -26,7 +26,7 @@ class Body
 
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias( const Vector3d &b);
-        void enu( const UTM &utm);
+        void pos( const UTM &utm);
         void update(const Matrix<double,9,1> &dx);
         void vel(const Matrix<double,3,1> &v);
 
diff --git a/src/main.cpp b/src/main.cpp
index 1fa6ec0..bb2a3b3 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -178,7 +178,7 @@ utmCallback(const message &msg, State &mu, const Quaterniond &q)
         utm_water.up = -tip[2];
         utm_water.zone_i = msg.utm.zone_i;
         utm_water.zone_c = msg.utm.zone_c;
-        mu.enu(utm_water);
+        mu.pos(utm_water);
     } 
     return;
 }
diff --git a/src/state.cpp b/src/state.cpp
index 998be20..281d22e 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -33,13 +33,13 @@ State::vel ( const Matrix<double,3,1> &v )
 }		/* -----  end of method State::vel  ----- */
 
 void
-State::enu ( const UTM &utm )
+State::pos ( const UTM &utm )
 {
     utm_c = utm.zone_c;
     utm_i = utm.zone_i;
-    body->enu(utm);
+    body->pos(utm);
     return ;
-}		/* -----  end of method State::enu  ----- */
+}		/* -----  end of method State::pos  ----- */
 
 /*
  *--------------------------------------------------------------------------------------
diff --git a/src/state.h b/src/state.h
index 7a5835d..ea8cfef 100644
--- a/src/state.h
+++ b/src/state.h
@@ -47,7 +47,7 @@ class State
 
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias(const Vector3d &b);
-        void enu(const UTM &utm);
+        void pos(const UTM &utm);
         void feature_update( const std::vector<measurement_t> & z, const Quaterniond &q);
         void initializePi(int i, const Matrix<double,3,3> &Pi);
         void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
-- 
cgit v1.1