diff options
| -rw-r--r-- | src/body.cpp | 39 | ||||
| -rw-r--r-- | src/body.h | 4 | ||||
| -rw-r--r-- | src/feature.cpp | 2 |
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 ----- */ @@ -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 ----- */ |