diff --git a/src/test/gtest_processorIMU_jacobians.cpp b/src/test/gtest_processorIMU_jacobians.cpp index d2f4ed4a1b1e9bc3091bb341d18c8f505229706a..7ec522d9c512e72982425e845361f75d5157421a 100644 --- a/src/test/gtest_processorIMU_jacobians.cpp +++ b/src/test/gtest_processorIMU_jacobians.cpp @@ -158,6 +158,21 @@ TEST_F(ProcessorIMU_jacobians_bias, dDv_dwb_) "\ndDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb << std::endl; } +TEST_F(ProcessorIMU_jacobians_bias, dDq_dab) +{ + using namespace wolf; + Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL); + Eigen::Matrix3s dDq_dab; + + 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).data() + 3); + dDq_dab.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias; + } + + EXPECT_TRUE(dDq_dab.isZero(wolf::Constants::EPS)) << "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl; +} + int main(int argc, char **argv) { using namespace wolf;