From 96be3401e127fcaf986d7c23a8f974fa4d376890 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Wed, 7 Dec 2016 12:05:11 +0100
Subject: [PATCH] added test dDp_dV
---
src/test/gtest_processorIMU_jacobians.cpp | 12 ++++++++++++
1 file changed, 12 insertions(+)
diff --git a/src/test/gtest_processorIMU_jacobians.cpp b/src/test/gtest_processorIMU_jacobians.cpp
index ec973eac5..e7ef03c23 100644
--- a/src/test/gtest_processorIMU_jacobians.cpp
+++ b/src/test/gtest_processorIMU_jacobians.cpp
@@ -313,6 +313,18 @@ TEST_F(ProcessorIMU_jacobians, dDp_dP)
"\ndDp_dP_a - dDp_dP_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP << std::endl;
}
+TEST_F(ProcessorIMU_jacobians, dDp_dV)
+{
+ using namespace wolf;
+ Eigen::Matrix3s dDp_dV;
+
+ for(int i=0;i<3;i++)
+ dDp_dV.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i+6).tail(3)*dt - deltas_jac.Delta0_.tail(3)*dt)/Delta_noise(i+6);
+
+ EXPECT_TRUE((dDp_dV - deltas_jac.jacobian_delta_preint_.block(0,6,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dV : \n" << dDp_dV << "\n deltas_jac.jacobian_delta_preint_.block(0,6,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) <<
+ "\ndDp_dV_a - dDp_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV << std::endl;
+}
+
int main(int argc, char **argv)
{
using namespace wolf;
--
GitLab