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

add test dDp_dO

parent 3ef589ba
No related branches found
No related tags found
No related merge requests found
......@@ -327,6 +327,23 @@ TEST_F(ProcessorIMU_jacobians, dDp_dV)
"\ndDp_dV_a - dDp_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV << std::endl;
}
TEST_F(ProcessorIMU_jacobians, dDp_dO)
{
using namespace wolf;
Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
Eigen::Matrix3s dDp_dO;
//dDp_dOx = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
for(int i=0;i<3;i++){
remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
dDp_dO.block<3,1>(0,i) = ((Dq_noisy.matrix() * deltas_jac.delta0_.head(3)) - (Dq0.matrix()* deltas_jac.delta0_.head(3)))/Delta_noise(i+3);
}
EXPECT_TRUE((dDp_dO - deltas_jac.jacobian_delta_preint_.block(0,3,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dO : \n" << dDp_dO << "\n deltas_jac.jacobian_delta_preint_.block(0,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) <<
"\ndDp_dO_a - dDp_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) - dDp_dO << std::endl;
}
TEST_F(ProcessorIMU_jacobians, dDv_dV)
{
using namespace wolf;
......@@ -340,6 +357,8 @@ TEST_F(ProcessorIMU_jacobians, dDv_dV)
"\ndDv_dV_a - dDv_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) - dDv_dV << 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