diff options
| -rw-r--r-- | src/body.cpp | 2 | ||||
| -rw-r--r-- | src/body.h | 2 |
2 files changed, 2 insertions, 2 deletions
diff --git a/src/body.cpp b/src/body.cpp index a94c8a2..edf2047 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -140,7 +140,7 @@ Body::Q (double dt) 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<4,4>(6,6) = 0.25*dt*dt*ANG_STD*ANG_STD*Matrix<double,4,4>::Identity(); #else // Qbb Q.block<3,3>(6,6) = dt*dt*ACC_BIAS_STD*ACC_BIAS_STD*Matrix<double,3,3>::Identity(); @@ -5,7 +5,7 @@ #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 ANG_STD 20e-3 #define ACC_BIAS_STD 22e-3 #define ANG_BIAS_STD 7e-6 #define IMU_NOISE 800e-6 /* IMU process noise */ |