diff --git a/src/test/gtest_processorIMU_jacobians.cpp b/src/test/gtest_processorIMU_jacobians.cpp index 2327c52066040daed15170f07a50f555fb868890..d2f4ed4a1b1e9bc3091bb341d18c8f505229706a 100644 --- a/src/test/gtest_processorIMU_jacobians.cpp +++ b/src/test/gtest_processorIMU_jacobians.cpp @@ -146,6 +146,18 @@ TEST_F(ProcessorIMU_jacobians_bias, dDp_dwb) "\ndDp_dwb_a - dDv_dab_ : \n" << bias_jac.dDp_dwb_ - dDp_dwb << std::endl; } +TEST_F(ProcessorIMU_jacobians_bias, dDv_dwb_) +{ + using namespace wolf; + Eigen::Matrix3s dDv_dwb; + + for(int i=0;i<3;i++) + dDv_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias; + + EXPECT_TRUE((dDv_dwb - bias_jac.dDv_dwb_).isMuchSmallerThan(1,0.000001)) << "dDv_dwb : \n" << dDv_dwb << "\n bias_jac.dDv_dwb_ :\n" << bias_jac.dDv_dwb_ << + "\ndDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb << std::endl; +} + int main(int argc, char **argv) { using namespace wolf;