summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorMartin Miller2017-04-03 15:16:50 -0500
committerMartin Miller2017-04-03 15:16:50 -0500
commite274f2347393e7cbf1f755c9ed1593bd31bd1370 (patch)
treedac2c7de2fe7f4e10d6c551892dcde4d4c4adc20 /src
parent27e98b8b0aedca320e45b5184b3e426e5913e1a3 (diff)
downloadrefslam-e274f2347393e7cbf1f755c9ed1593bd31bd1370.zip
refslam-e274f2347393e7cbf1f755c9ed1593bd31bd1370.tar.gz
Add quaternion accessor to Body.
Diffstat (limited to 'src')
-rw-r--r--src/body.cpp27
-rw-r--r--src/body.h1
2 files changed, 22 insertions, 6 deletions
diff --git a/src/body.cpp b/src/body.cpp
index 99ea1ed..f7440b5 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -203,11 +203,11 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
#ifdef FULLSTATE
Matrix<double,13,13> F = Matrix<double,13,13>::Zero();
double qbw1,qbw2,qbw3,qbw4;
- qbw1 = X(6);
- qbw2 = X(7);
- qbw3 = X(8);
- qbw4 = X(9);
- Quaterniond qbw(qbw4,qbw1,qbw2,qbw3);
+ Quaterniond qbw = qhat();
+ qbw1 = qbw.x();
+ qbw2 = qbw.y();
+ qbw3 = qbw.z();
+ qbw4 = qbw.w();
Matrix<double,3,3> Rbw(qbw.toRotationMatrix());
#else
Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
@@ -291,7 +291,7 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond
b = Matrix<double,6,1>::Zero();
#ifdef FULLSTATE
- Quaterniond qbw(X(9),X(6),X(7),X(8));
+ Quaterniond qbw qhat();
Matrix<double,3,3> Rbw(qbw.toRotationMatrix());
#else
Matrix<double,3,3> Rbw(q.toRotationMatrix());
@@ -382,3 +382,18 @@ Body::asVector ( const Matrix<double,9,1> &m )
return ;
} /* ----- end of method Body::asVector ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: Body
+ * Method: Body :: qhat
+ * Description: Returns the current quaternion estimate.
+ *--------------------------------------------------------------------------------------
+ */
+Quaterniond
+Body::qhat ( )
+{
+ Quaterniond qbw(X(9),X(6),X(7),X(8));
+ return qbw;
+} /* ----- end of method Body::qhat ----- */
+
diff --git a/src/body.h b/src/body.h
index b973b00..5544bba 100644
--- a/src/body.h
+++ b/src/body.h
@@ -37,6 +37,7 @@ class Body
Matrix<double,1,13> H();
Matrix<double,13,13> Q(double dt);
Matrix<double,1,1> S(const Matrix<double,13,13> &Pxx);
+ Quaterniond qhat();
#else
Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q, double dt);
Matrix<double,9,1> asVector();