Skip to content
Snippets Groups Projects
Commit 57104289 authored by Dinesh Atchuthan's avatar Dinesh Atchuthan
Browse files

test dDq_dab

parent cf46655a
No related branches found
No related tags found
No related merge requests found
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment