From fe4ed819b186d56c7c66e196c4717ddc4ab99415 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Thu, 6 Apr 2017 11:44:05 -0500
Subject: Implement quaternion covariance

Pq = J*Pa*J'

where J is the Jacobian of the euler to quaternion function
---
 src/main.cpp  | 70 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++---
 src/main.h    |  4 +++-
 src/state.cpp |  4 ++--
 src/state.h   |  2 +-
 4 files changed, 73 insertions(+), 7 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index fa0b5fd..49b6771 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -27,6 +27,52 @@ using std::cout;
 using std::endl;
 using std::cerr;
 
+Matrix<double,4,3> attitude_jacobian(double roll, double pitch, double yaw)
+{
+    roll /= 2.;
+    pitch /= 2.;
+    yaw /= 2.;
+    double sphi, cphi, sthe, cthe, spsi, cpsi;
+
+    sphi = sin(roll);
+    cphi = cos(roll);
+
+    sthe = sin(pitch);
+    cthe = cos(pitch);
+
+    spsi = sin(yaw);
+    cpsi = cos(yaw);
+    
+    double dqxdphi, dqxdthe, dqxdpsi;
+    double dqydphi, dqydthe, dqydpsi;
+    double dqzdphi, dqzdthe, dqzdpsi;
+    double dqwdphi, dqwdthe, dqwdpsi;
+
+    dqxdphi = 0.5*(cphi*cthe*cpsi+sphi*sthe*spsi);
+    dqxdthe = 0.5*(-sphi*sthe*cpsi-cphi*cthe*spsi);
+    dqxdpsi = 0.5*(-sphi*cthe*spsi-cphi*sthe*cpsi);
+
+    dqydphi = 0.5*(-sphi*sthe*cpsi+cphi*cthe*spsi);
+    dqydthe = 0.5*(cphi*cthe*cpsi-sphi*sthe*spsi);
+    dqydpsi = 0.5*(-cphi*sthe*spsi+sphi*cthe*cpsi);
+
+    dqzdphi = 0.5*(-sphi*cthe*spsi-cphi*sthe*cpsi);
+    dqzdthe = 0.5*(-cphi*sthe*spsi-sphi*cthe*cpsi);
+    dqzdpsi = 0.5*(cphi*cthe*cpsi-sphi*sthe*spsi);
+
+    dqwdphi = 0.5*(-sphi*cthe*cpsi+cphi*sthe*spsi);
+    dqwdthe = 0.5*(-cphi*sthe*cpsi+sphi*cthe*spsi);
+    dqwdpsi = 0.5*(-cphi*cthe*spsi+sphi*sthe*cpsi);
+
+    Matrix<double,4,3> J;
+    J << dqxdphi, dqxdthe, dqxdpsi,
+         dqydphi, dqydthe, dqydpsi,
+         dqzdphi, dqzdthe, dqzdpsi,
+         dqwdphi, dqwdthe, dqwdpsi;
+
+    return J;
+}
+
 double 
 aboveWater(const Quaterniond &q)
 {
@@ -51,7 +97,7 @@ aboveWater(const Quaterniond &q)
  */
 void
 #if STATESIZE==13
-covCallback(const message &msg, State &mu)
+covCallback(const message &msg, State &mu, const attitude_t &att)
 #else
 covCallback(const message &msg, State &mu, const Quaterniond &q)
 #endif
@@ -72,7 +118,23 @@ covCallback(const message &msg, State &mu, const Quaterniond &q)
     mu.position_covariance(pcov);
     mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw);
 #if STATESIZE==13
-    mu.quaternion_covariance();
+    Matrix3d Patt;
+    Patt = msg.covariance.attitude;
+    // Swap values for roll, pitch, yaw ordering
+    Patt(0,0) = msg.covariance.attitude(1,1);
+    Patt(1,1) = msg.covariance.attitude(0,0);
+
+    Patt(2,0) = msg.covariance.attitude(2,1);
+    Patt(2,1) = msg.covariance.attitude(2,0);
+
+    Patt(0,2) = msg.covariance.attitude(1,2);
+    Patt(1,2) = msg.covariance.attitude(0,2);
+
+    Matrix4d Pquat;
+    Matrix<double,4,3> Jatt;
+    Jatt = attitude_jacobian(att.roll, att.pitch, att.yaw);
+    Pquat = Jatt*Patt*Jatt.transpose();
+    mu.quaternion_covariance(Pquat);
 #endif
 }
 
@@ -381,6 +443,7 @@ main(int argc, char **argv)
     }
     State mu;
     Quaterniond qbw;
+    attitude_t att;
     // bias in FRD coordinates
     Vector3d acc_bias;
     acc_bias << ACCBIASX, ACCBIASY, ACCBIASZ;
@@ -430,6 +493,7 @@ main(int argc, char **argv)
                 break;
 
             case INSPVAS:	
+                att = msg.attitude;
                 pvaCallback(msg, mu, qbw);
                 if (seenutm) mu.unicsv();
                 break;
@@ -455,7 +519,7 @@ main(int argc, char **argv)
 
             case INSCOVS:
 #if STATESIZE==13
-                covCallback(msg, mu);
+                covCallback(msg, mu, att);
 #else
                 covCallback(msg, mu, qbw);
 #endif
diff --git a/src/main.h b/src/main.h
index f78158d..5fa7ee6 100644
--- a/src/main.h
+++ b/src/main.h
@@ -49,6 +49,7 @@
 
 using Eigen::Matrix;
 using Eigen::Matrix3d;
+using Eigen::Matrix4d;
 using Eigen::Quaterniond;
 using Eigen::Vector3d;
 
@@ -57,7 +58,7 @@ int parseLine(char *line, message *msg);
 timestamp update_dt(const timestamp t, timestamp *t_old);
 
 #if STATESIZE==13
-void covCallback(const message &msg, State &mu);
+void covCallback(const message &msg, State &mu, const attitude_t &att);
 void imgCallback(message &msg, State &mu, Camera &cam);
 void imuCallback(const message &msg, State &mu, const timestamp dt);
 void utmCallback(const message &msg, State &mu);
@@ -68,6 +69,7 @@ void imuCallback(const message &msg, State &mu, const Quaterniond &q, const time
 void utmCallback(const message &msg, State &mu, const Quaterniond &q);
 #endif
 void pvaCallback(const message &msg, Matrix<double,STATESIZE,1> &X, Quaterniond &q);
+Matrix<double,4,3> attitude_jacobian(double roll, double pitch, double yaw);
 
 
 #endif   /* ----- #ifndef main_INC  ----- */
diff --git a/src/state.cpp b/src/state.cpp
index 8b69ca2..0bae8e7 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -1024,9 +1024,9 @@ State::qhat ( const Quaterniond &q )
 
 
 void
-State::quaternion_covariance (  )
+State::quaternion_covariance ( const Matrix<double,4,4> &covq )
 {
-    P.block<4,4>(6,6) = 1e-12*Matrix<double,4,4>::Identity();
+    P.block<4,4>(6,6) = covq;
     return ;
 }		/* -----  end of method State::quaternion_covariance  ----- */
 
diff --git a/src/state.h b/src/state.h
index b04ae9c..cc10263 100644
--- a/src/state.h
+++ b/src/state.h
@@ -74,7 +74,7 @@ class State
                 const std::vector<measurement_t> &z);
         void Pkk1 ( const Vector3d &ang, const double dt);
         void ransacUpdate(std::vector<measurement_t> &z);
-        void quaternion_covariance();
+        void quaternion_covariance(const Matrix<double,4,4> &covq);
         void qhat(const Quaterniond &q);
 #else
         void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q);
-- 
cgit v1.1