From 0862825c7e37a45c343559b7c18a21fc0bb965f9 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Sat, 18 Mar 2017 23:01:15 -0500 Subject: Start Feature::h() method --- src/body.cpp | 4 ++-- src/body.h | 6 +++--- src/feature.cpp | 7 +++++++ src/feature.h | 3 +-- src/main.cpp | 10 +++++----- src/main.h | 8 ++++---- src/state.cpp | 4 ++-- src/state.h | 6 +++--- 8 files changed, 27 insertions(+), 21 deletions(-) (limited to 'src') diff --git a/src/body.cpp b/src/body.cpp index 2832ea4..3796c41 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -149,7 +149,7 @@ Body::h ( ) *-------------------------------------------------------------------------------------- */ Matrix -Body::F ( const Vector3d &ang, const Quaternion &q ) +Body::F ( const Vector3d &ang, const Quaterniond &q ) { Matrix F = Matrix::Zero(); Matrix Rbw(q.toRotationMatrix()); @@ -169,7 +169,7 @@ Body::F ( const Vector3d &ang, const Quaternion &q ) *-------------------------------------------------------------------------------------- */ void -Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaternion &q, const double dt) +Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt) { Vector3d bias(X.segment(6,3)); Vector3d gravity_world(0.,0.,9.80655); diff --git a/src/body.h b/src/body.h index f201683..6742788 100644 --- a/src/body.h +++ b/src/body.h @@ -6,7 +6,7 @@ using Eigen::Matrix; using Eigen::Vector3d; -using Eigen::Quaternion; +using Eigen::Quaterniond; /* * ===================================================================================== @@ -29,11 +29,11 @@ class Body void vel(const Matrix &v); /* ==================== OPERATORS ======================================= */ - Matrix F(const Vector3d &ang, const Quaternion &q); + Matrix F(const Vector3d &ang, const Quaterniond &q); Matrix H(); Matrix h(); void motionModel ( const Vector3d &acc, const Vector3d &ang, - const Quaternion &q, const double dt); + const Quaterniond &q, const double dt); Matrix Q(double dt); Matrix R(); Matrix S(const Matrix &P); diff --git a/src/feature.cpp b/src/feature.cpp index 0d79c47..ad554a1 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -18,6 +18,13 @@ */ #include "feature.h" +Matrix +Feature::h ( ) +{ + Matrix h; + return h; +} /* ----- end of method Feature::h ----- */ + Matrix Feature::Fx ( ) diff --git a/src/feature.h b/src/feature.h index d6c43d0..077a617 100644 --- a/src/feature.h +++ b/src/feature.h @@ -28,9 +28,8 @@ class Feature Matrix Fy( const Vector3d &vel, const Vector3d &ang); Matrix Hx( const Vector3d &pos, const Quaterniond &q); Matrix Hy( const Vector3d &pos, const Quaterniond &q); - /* - Matrix Hy Matrix h(); + /* void motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); Matrix Q(double dt); diff --git a/src/main.cpp b/src/main.cpp index 6cc4317..5d4ea71 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -31,7 +31,7 @@ using std::cerr; #else /* ----- not USE_ROS ----- */ void -covCallback(const message &msg, State &mu, const Quaternion &q) +covCallback(const message &msg, State &mu, const Quaterniond &q) { if (seencov && seenutm && seenpva) return; seencov=true; @@ -49,7 +49,7 @@ imgCallback(const message *msg) } void -imuCallback(const message &msg, State &mu, const Quaternion &q, const timestamp dt) +imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt) { 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); @@ -63,7 +63,7 @@ imuCallback(const message &msg, State &mu, const Quaternion &q, const ti } void -pvaCallback(const message &msg, State &mu, Quaternion &q) +pvaCallback(const message &msg, State &mu, Quaterniond &q) { // Update quaternion using Eigen::AngleAxisd; @@ -76,7 +76,7 @@ pvaCallback(const message &msg, State &mu, Quaternion &q) R = AngleAxisd(yaw, Vector3d::UnitZ()) * AngleAxisd(roll, Vector3d::UnitX()) * AngleAxisd(pitch, Vector3d::UnitY()); - q = Quaternion(R); + q = Quaterniond(R); // Set body velocity if (!seenpva || !seencov || !seenutm) { @@ -217,7 +217,7 @@ int main(int argc, char **argv) { State mu; - Quaternion qbw; + Quaterniond qbw; // bias in FRD coordinates Vector3d acc_bias; acc_bias << -0.03713532, 1.125*0.01465135, -1*-0.00709229; diff --git a/src/main.h b/src/main.h index 2fef291..9e1b656 100644 --- a/src/main.h +++ b/src/main.h @@ -14,16 +14,16 @@ using Eigen::Matrix; using Eigen::Matrix3d; -using Eigen::Quaternion; +using Eigen::Quaterniond; using Eigen::Vector3d; int parseLine(char *line, message *msg); timestamp update_dt(const timestamp t, timestamp *t_old); -void covCallback(const message &msg, Matrix &P, const Quaternion &q); +void covCallback(const message &msg, Matrix &P, const Quaterniond &q); void imgCallback(const message *msg); -void imuCallback(const message &msg, State &mu, const Quaternion &q, const timestamp dt); -void pvaCallback(const message &msg, Matrix &X, Quaternion &q); +void imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt); +void pvaCallback(const message &msg, Matrix &X, Quaterniond &q); void utmCallback(const message &msg, State &mu); #endif /* ----- #ifndef main_INC ----- */ diff --git a/src/state.cpp b/src/state.cpp index 304e5ed..f12ec77 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -67,7 +67,7 @@ State::update ( const Matrix &z ) *-------------------------------------------------------------------------------------- */ void -State::Pkk1 ( const Vector3d &ang, const Quaternion &q, const double dt) +State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt) { Matrix F,Q; Q = body->Q(dt); @@ -80,7 +80,7 @@ State::Pkk1 ( const Vector3d &ang, const Quaternion &q, const double dt) void State::motionModel ( const Vector3d &acc, const Vector3d &ang, - const Quaternion &q, const double dt) + const Quaterniond &q, const double dt) { Pkk1(ang,q,dt); body->motionModel(acc,ang,q,dt); diff --git a/src/state.h b/src/state.h index 1efb0c2..c91c1ae 100644 --- a/src/state.h +++ b/src/state.h @@ -8,7 +8,7 @@ using Eigen::Matrix; using Eigen::Dynamic; using Eigen::Vector3d; -using Eigen::Quaternion; +using Eigen::Quaterniond; /* * ===================================================================================== * Class: State @@ -27,11 +27,11 @@ class State void accelerometer_bias(const Vector3d &b); void enu(const UTM &utm); void motionModel(const Vector3d &acc, const Vector3d &ang, - const Quaternion &q, const double dt); + const Quaterniond &q, const double dt); void position_covariance(const Matrix &p); void velocity_covariance(const Matrix &p); void vel(const Matrix &v); - void Pkk1 ( const Vector3d &ang, const Quaternion &q, const double dt); + void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); /* ==================== OPERATORS ======================================= */ void update ( const Matrix &z); -- cgit v1.1