Improved process noise modeling

3 files changed, 37 insertions, 8 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 0bbe18f..a94c8a2 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -117,16 +117,41 @@ Body::Q (double dt)
 {
     Matrix<double,STATESIZE,STATESIZE> Q;
     Q = Matrix<double,STATESIZE,STATESIZE>::Zero();
-    Q.block<3,3>(0,0) = 0.25*dt*dt*dt*dt*Matrix<double,3,3>::Identity();
-    Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
-    Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
-    Q.block<3,3>(3,3) = dt*dt*Matrix<double,3,3>::Identity();
-    Q *= IMU_NOISE;
+    // Qxx
+    Q.block<3,3>(0,0) = 0.25*dt*dt*dt*dt*ACC_STD*ACC_STD*Matrix<double,3,3>::Identity();
+
+    // Qxv, Qvx
+    Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix3d::Identity()-0.25*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
+    Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix3d::Identity()-0.25*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
+    
+    //Qvv
+    Q.block<3,3>(3,3) = dt*dt*(ACC_STD*ACC_STD+0.25*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix<double,3,3>::Identity();
+
 #if STATESIZE==13
+    // Qbb
+    Q.block<3,3>(10,10) = dt*dt*ACC_BIAS_STD*ACC_BIAS_STD*Matrix<double,3,3>::Identity();
+
+    // Qxb, Qbx
+    Q.block<3,3>(10,0) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
+    Q.block<3,3>(0,10) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
+    
+    // Qvb, Qbv 
+    Q.block<3,3>(3,10) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity();
+    Q.block<3,3>(10,3) = dt*dt*(ACC_STD*ACC_BIAS_STD-0.5*ACC_BIAS_STD*ACC_BIAS_STD)*Matrix3d::Identity();
+    
+    // Qqq
     Q.block<4,4>(6,6) = 0.25*dt*dt*IMU_NOISE*Matrix<double,4,4>::Identity();
-    Q.block<3,3>(10,10) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::Identity();
 #else
-    Q.block<3,3>(6,6) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::Identity();
+    // Qbb
+    Q.block<3,3>(6,6) = dt*dt*ACC_BIAS_STD*ACC_BIAS_STD*Matrix<double,3,3>::Identity();
+
+    // Qxb, Qbx
+    Q.block<3,3>(6,0) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
+    Q.block<3,3>(0,6) = 0.5*dt*dt*dt*ACC_STD*ACC_BIAS_STD*Matrix3d::Identity();
+    
+    // Qvb, Qbv 
+    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  ----- */
diff --git a/src/body.h b/src/body.h
index 4653588..1db9215 100644
--- a/src/body.h
+++ b/src/body.h
@@ -4,6 +4,10 @@
 #include <iostream>
 #include "types.h"
 #define R_HEIGHT 2.5e-3            /* measurement noise of height measurement */
+#define ACC_STD 28e-3
+#define ANG_STD 80e-6
+#define ACC_BIAS_STD 22e-3
+#define ANG_BIAS_STD 7e-6
 #define IMU_NOISE 800e-6            /* IMU process noise */
 #define IMU_RANDOMWALK 50e-6      /* Bias process noise */
 //#define DOCLAMP
diff --git a/src/feature.cpp b/src/feature.cpp
index 85396e3..22bf776 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -148,7 +148,7 @@ Feature::Q ( const double dt )
 {
     Matrix<double,3,3> Q;
     Q = Matrix<double,3,3>::Identity();
-    Q *= dt*dt*dt*dt*FEATURE_NOISE;//*1e-1;
+    Q *= 0.5*dt*dt*dt*dt*FEATURE_NOISE*FEATURE_NOISE;//*1e-1;
     return Q;
 }		/* -----  end of method Feature::q  ----- */