process noise

1 files changed, 30 insertions, 12 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 1aafd54..5f5076b 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -115,6 +115,8 @@ Body::R()
 Matrix<double,STATESIZE,STATESIZE>
 Body::Q (double dt)
 {
+    const bool doang=true;
+    const bool doquat=true;
 
     Matrix<double,STATESIZE,3> Ga;
     Ga.block<3,3>(0,0) = 0.5*dt*dt*Matrix3d::Identity();
@@ -122,9 +124,13 @@ Body::Q (double dt)
     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();
+    if (doang) {
+        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();
+    } else {
+        Gb.block<STATESIZE-3,3>(0,0) = Matrix<double,STATESIZE-3,3>::Zero();
+    }
     Gb.block<3,3>(STATESIZE-3,0) = dt*Matrix3d::Identity();
 
     Matrix<double,STATESIZE,3> Gw;
@@ -133,21 +139,33 @@ Body::Q (double dt)
     Gw.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();
+    Matrix<double,STATESIZE,Eigen::Dynamic> Gq;
+    if (doquat) {
+        Gq = Matrix<double,STATESIZE,3>::Zero();
+        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();
+    } else {
+        Gq = Matrix<double,STATESIZE,4>::Zero();
+        Gq.block<6,4>(0,0) = Matrix<double,6,4>::Zero();
+        Gq.block<4,4>(6,0) = dt*Matrix<double,4,4>::Identity();
+        Gq.block<3,4>(10,0) = Matrix<double,3,4>::Zero();
+    }
 #endif
     Matrix<double,STATESIZE,STATESIZE> Q;
     Q = Matrix<double,STATESIZE,STATESIZE>::Zero();
-    Q = acc_std*acc_std*Ga*Ga.transpose();
+    if (doang) {
+        Q += acc_std*acc_std*Ga*Ga.transpose();
+        //Q += ang_std*ang_std*Gw*Gw.transpose();
+    } else {
+        Q += (acc_bias_std+ang_std+acc_std)*(acc_std+acc_bias_std+ang_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();
+    Q += ang_std*ang_std *(Gq+Gw) * (Gq+Gw).transpose();
 #endif