Implement quaternion covariance

Pq = J*Pa*J'

where J is the Jacobian of the euler to quaternion function

1 files changed, 67 insertions, 3 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