diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 39 |
1 files changed, 32 insertions, 7 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 ----- */ |