diff --git a/src/test/gtest_processorIMU_jacobians.cpp b/src/test/gtest_processorIMU_jacobians.cpp index 2b602015b46afa27361a906d4166ebb57004ab06..8a1687b5e1ab8819af5c25882363dad7f0f0d659 100644 --- a/src/test/gtest_processorIMU_jacobians.cpp +++ b/src/test/gtest_processorIMU_jacobians.cpp @@ -288,13 +288,19 @@ TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_) Eigen::Matrix3s dDq_dwb; new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3); + for(int i=0;i<3;i++){ new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i+3).data() + 3); + dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias; } - ASSERT_FALSE((dDq_dwb - bias_jac.dDq_dwb_).isMuchSmallerThan(1,0.000001)) << "dDq_dwb : \n" << dDq_dwb << "\n bias_jac.dDq_dwb_ :\n" << bias_jac.dDq_dwb_ << - "\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac.dDq_dwb_ - dDq_dwb << "\n R1^T * R2 : \n" << q_in_1.matrix().transpose() * q_in_2.matrix() << std::endl; + ASSERT_TRUE((dDq_dwb - bias_jac.dDq_dwb_).isMuchSmallerThan(1,0.000001)) + << "dDq_dwb : \n" << dDq_dwb + << "\n bias_jac.dDq_dwb_ :\n" << bias_jac.dDq_dwb_ + << "\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac.dDq_dwb_ - dDq_dwb + << "\n R1^T * R2 : \n" << q_in_1.matrix().transpose() * q_in_2.matrix() + << std::endl; } TEST_F(ProcessorIMU_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_) @@ -547,4 +553,4 @@ int main(int argc, char **argv) testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +}