diff options
| -rw-r--r-- | src/body.cpp | 12 | ||||
| -rw-r--r-- | src/body.h | 2 | ||||
| -rw-r--r-- | src/main.cpp | 2 | ||||
| -rw-r--r-- | src/state.cpp | 6 | ||||
| -rw-r--r-- | 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 ----- */ @@ -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, |