summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/body.cpp4
-rw-r--r--src/body.h6
-rw-r--r--src/feature.cpp7
-rw-r--r--src/feature.h3
-rw-r--r--src/main.cpp10
-rw-r--r--src/main.h8
-rw-r--r--src/state.cpp4
-rw-r--r--src/state.h6
8 files changed, 27 insertions, 21 deletions
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<double,9,9>
-Body::F ( const Vector3d &ang, const Quaternion<double> &q )
+Body::F ( const Vector3d &ang, const Quaterniond &q )
{
Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
Matrix<double,3,3> Rbw(q.toRotationMatrix());
@@ -169,7 +169,7 @@ Body::F ( const Vector3d &ang, const Quaternion<double> &q )
*--------------------------------------------------------------------------------------
*/
void
-Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &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<double,3,1> &v);
/* ==================== OPERATORS ======================================= */
- Matrix<double,9,9> F(const Vector3d &ang, const Quaternion<double> &q);
+ Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q);
Matrix<double,1,9> H();
Matrix<double,1,1> h();
void motionModel ( const Vector3d &acc, const Vector3d &ang,
- const Quaternion<double> &q, const double dt);
+ const Quaterniond &q, const double dt);
Matrix<double,9,9> Q(double dt);
Matrix<double,1,1> R();
Matrix<double,1,1> S(const Matrix<double,9,9> &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<double,6,1>
+Feature::h ( )
+{
+ Matrix<double,6,1> h;
+ return h;
+} /* ----- end of method Feature::h ----- */
+
Matrix<double,3,9>
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<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang);
Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
- /*
- Matrix<double,6> Hy
Matrix<double,6,1> h();
+ /*
void motionModel ( const Vector3d &acc, const Vector3d &ang,
const Quaterniond &q, const double dt);
Matrix<double,9,9> 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<double> &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<double> &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<double> &q, const ti
}
void
-pvaCallback(const message &msg, State &mu, Quaternion<double> &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<double> &q)
R = AngleAxisd(yaw, Vector3d::UnitZ())
* AngleAxisd(roll, Vector3d::UnitX())
* AngleAxisd(pitch, Vector3d::UnitY());
- q = Quaternion<double>(R);
+ q = Quaterniond(R);
// Set body velocity
if (!seenpva || !seencov || !seenutm) {
@@ -217,7 +217,7 @@ int
main(int argc, char **argv)
{
State mu;
- Quaternion<double> 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<double,9,9> &P, const Quaternion<double> &q);
+void covCallback(const message &msg, Matrix<double,9,9> &P, const Quaterniond &q);
void imgCallback(const message *msg);
-void imuCallback(const message &msg, State &mu, const Quaternion<double> &q, const timestamp dt);
-void pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q);
+void imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt);
+void pvaCallback(const message &msg, Matrix<double,9,1> &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<double,1,1> &z )
*--------------------------------------------------------------------------------------
*/
void
-State::Pkk1 ( const Vector3d &ang, const Quaternion<double> &q, const double dt)
+State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt)
{
Matrix<double,9,9> F,Q;
Q = body->Q(dt);
@@ -80,7 +80,7 @@ State::Pkk1 ( const Vector3d &ang, const Quaternion<double> &q, const double dt)
void
State::motionModel ( const Vector3d &acc, const Vector3d &ang,
- const Quaternion<double> &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<double> &q, const double dt);
+ const Quaterniond &q, const double dt);
void position_covariance(const Matrix<double,3,3> &p);
void velocity_covariance(const Matrix<double,3,3> &p);
void vel(const Matrix<double,3,1> &v);
- void Pkk1 ( const Vector3d &ang, const Quaternion<double> &q, const double dt);
+ void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt);
/* ==================== OPERATORS ======================================= */
void update ( const Matrix<double,1,1> &z);