From 5b075e08aee59ce37919e38480d0167c1c1ab748 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Wed, 7 Dec 2016 11:58:30 +0100
Subject: [PATCH] add test dDp_dP
---
src/test/gtest_processorIMU_jacobians.cpp | 11 +++++++++++
1 file changed, 11 insertions(+)
diff --git a/src/test/gtest_processorIMU_jacobians.cpp b/src/test/gtest_processorIMU_jacobians.cpp
index 3222cfdb0..e3e9aee1b 100644
--- a/src/test/gtest_processorIMU_jacobians.cpp
+++ b/src/test/gtest_processorIMU_jacobians.cpp
@@ -300,7 +300,18 @@ TEST_F(ProcessorIMU_jacobians, dDq_dwb)
dDo_doy = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,dthetay,0)) )/dthetay
dDo_doz = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,0,dthetaz)) )/dthetaz
*/
+
+TEST_F(ProcessorIMU_jacobians, dDp_dP)
+{
+ using namespace wolf;
+ Eigen::Matrix3s dDp_dP;
+ for(int i=0;i<3;i++)
+ dDp_dP.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i).head(3) - deltas_jac.Delta0_.head(3))/Delta_noise(i);
+
+ EXPECT_TRUE((dDp_dP - deltas_jac.jacobian_delta_preint_.block(0,0,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dP : \n" << dDp_dP << "\n deltas_jac.jacobian_delta_preint_.block(0,0,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) <<
+ "\ndDp_dP_a - dDp_dP_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP << std::endl;
+}
int main(int argc, char **argv)
{
--
GitLab