From 0a4594884adcdef15abe861fc64d74b7511c6055 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Wed, 5 Apr 2017 14:08:25 -0500
Subject: Improved process noise modeling

---
 src/body.cpp | 39 ++++++++++++++++++++++++++++++++-------
 1 file changed, 32 insertions(+), 7 deletions(-)

(limited to 'src/body.cpp')

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  ----- */
-- 
cgit v1.1