Update process noise in body.

Process noise is a function of v and q.

2 files changed, 54 insertions, 0 deletions

diff --git a/src/body.cpp b/src/body.cpp
index edf2047..885b2ad 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -115,6 +115,43 @@ Body::R()
 Matrix<double,STATESIZE,STATESIZE>
 Body::Q (double dt)
 {
+
+    Matrix<double,STATESIZE,3> Ga;
+    Ga.block<3,3>(0,0) = 0.5*dt*dt*Matrix3d::Identity();
+    Ga.block<3,3>(3,0) = dt*Matrix3d::Identity();
+    Ga.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
+
+    Matrix<double,STATESIZE,3> Gb;
+    Gb.block<3,3>(0,0) = -0.5*dt*dt*Matrix3d::Identity();
+    Gb.block<3,3>(3,0) = -dt*Matrix3d::Identity();
+    Gb.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
+
+    Matrix<double,STATESIZE,3> Gw;
+    Gw.block<3,3>(0,0) = 0.5*dt*dt*skewSymmetric(vel());
+    Gw.block<3,3>(3,0) = dt*skewSymmetric(vel());
+    Gb.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
+
+#if STATESIZE==13
+    Matrix<double,STATESIZE,3> Gq;
+    Quaterniond q = qhat();
+
+    Gq.block<6,3>(0,0) = Matrix<double,6,3>::Zero();
+    Gq.block<4,3>(6,0) = 0.5*dt*qomega(q);
+    Gq.block<3,3>(10,0) = Matrix<double,3,3>::Zero();
+#endif
+    Matrix<double,STATESIZE,STATESIZE> Q;
+    Q = Matrix<double,STATESIZE,STATESIZE>::Zero();
+    Q = ACC_STD*ACC_STD*Ga*Ga.transpose();
+    Q += ACC_BIAS_STD*ACC_BIAS_STD*Gb*Gb.transpose();
+    Q += ANG_STD*ANG_STD * Gw * Gw.transpose();
+
+#if STATESIZE==13
+    Q += ANG_STD*ANG_STD *Gq * Gq.transpose();
+#endif
+
+
+
+    /*
     Matrix<double,STATESIZE,STATESIZE> Q;
     Q = Matrix<double,STATESIZE,STATESIZE>::Zero();
     // Qxx
@@ -153,6 +190,7 @@ Body::Q (double dt)
     Q.block<3,3>(3,6) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity();
     Q.block<3,3>(6,3) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity();
 #endif
+*/
     return Q;
 }		/* -----  end of method Body::q  ----- */
 
@@ -249,6 +287,18 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
     return F;
 }		/* -----  end of method Body::F  ----- */
 
+Matrix<double,4,3>
+Body::qomega ( const Quaterniond &q )
+{
+    Matrix<double,4,3> qom;
+    qom << q.w(), -q.z(), q.y(),
+           q.z(), q.w(), -q.x(),
+           -q.y(), q.x(), q.w(),
+           -q.x(),-q.y(),-q.z();
+
+    return qom ;
+}		/* -----  end of method Body::qomega  ----- */
+
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  Body
diff --git a/src/body.h b/src/body.h
index 6641ab5..1e64d86 100644
--- a/src/body.h
+++ b/src/body.h
@@ -16,7 +16,10 @@
 
 
 using Eigen::Matrix;
+using Eigen::Matrix3d;
+using Eigen::Matrix4d;
 using Eigen::Vector3d;
+using Eigen::Vector4d;
 using Eigen::Quaterniond;
 using std::cout;
 using std::cerr;
@@ -69,6 +72,7 @@ class Body
         Matrix<double,1,1> R();
         Matrix<double,3,3> skewSymmetric(const Vector3d &x);
         Matrix<double,4,4> omega(const Vector3d &x);
+        Matrix<double,4,3> qomega(const Quaterniond &q);
         void unicsv();
 
     protected: