passing bias jacobian tests on Imu3D

superjax · Jan 8, 2019 · 761e12e · 761e12e
1 parent 2d0f4b0
commit 761e12e
Show file tree

Hide file tree

Showing 7 changed files with 23 additions and 12 deletions.
diff --git a/.gitmodules b/.gitmodules
@@ -1,3 +1,6 @@
 [submodule "lib/geometry"]
 	path = lib/geometry
 	url = https://github.com/superjax/geometry
+[submodule "lib/multirotor_sim"]
+	path = lib/multirotor_sim
+	url = [email protected]:superjax/multirotor_sim
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -9,25 +9,33 @@ find_package(Eigen3 REQUIRED)
 find_package(GTest REQUIRED)
 
 add_subdirectory(lib/geometry)
+add_subdirectory(lib/multirotor_sim)
 
 include_directories(include
     lib/geometry/include
+    lib/multirotor_sim/include
+    lib/multirotor_sim/lib/nanoflann_eigen/include
+    lib/multirotor_sim/lib/lin_alg_tools/include
     lib/utils/include
     ${EIGEN3_INCLUDE_DIRS}
     ${GTEST_INCLUDE_DIRS})
 
 add_library(utils
-    lib/utils/src/robot1d.cpp)
+    lib/utils/src/robot1d.cpp
+    lib/utils/src/jac.cpp)
 
 add_executable(nano_mhe_test
     src/test_imu1d.cpp
+    src/test_imu3d.cpp
     src/test_mhe1d.cpp
+    src/test_mhe3d.cpp
     src/test_autodiff.cpp
     src/test_lm.cpp)
 target_link_libraries(nano_mhe_test
     utils
     gtest
     gtest_main
+    multirotor_sim
     ${GTEST_LIBRARIES}
     pthread)
 #add_custom_command(

diff --git a/lib/multirotor_sim b/lib/multirotor_sim
diff --git a/lib/utils/include/utils/jac.h b/lib/utils/include/utils/jac.h
@@ -4,4 +4,4 @@ Eigen::MatrixXd calc_jac(std::function<Eigen::MatrixXd(const Eigen::MatrixXd &)>
                          std::function<Eigen::MatrixXd(const Eigen::MatrixXd&, const Eigen::MatrixXd&)>f_boxminus=nullptr,
                          std::function<Eigen::MatrixXd(const Eigen::MatrixXd&, const Eigen::MatrixXd&)>f_boxplus=nullptr,
                          std::function<Eigen::MatrixXd(const Eigen::MatrixXd &, const Eigen::MatrixXd &)> f_boxminus2=nullptr,
-                         std::function<Eigen::MatrixXd(const Eigen::MatrixXd &, const Eigen::MatrixXd &)> f_boxplus2=nullptr);
+                         std::function<Eigen::MatrixXd(const Eigen::MatrixXd &, const Eigen::MatrixXd &)> f_boxplus2=nullptr, double step_size=1e-8);
diff --git a/lib/utils/src/jac.cpp b/lib/utils/src/jac.cpp
@@ -5,7 +5,8 @@ Eigen::MatrixXd calc_jac(std::function<Eigen::MatrixXd(const Eigen::MatrixXd&)>
                          std::function<Eigen::MatrixXd(const Eigen::MatrixXd&, const Eigen::MatrixXd&)>f_boxminus,
                          std::function<Eigen::MatrixXd(const Eigen::MatrixXd&, const Eigen::MatrixXd&)>f_boxplus,
                          std::function<Eigen::MatrixXd(const Eigen::MatrixXd&, const Eigen::MatrixXd&)>f_boxminus2,
-                         std::function<Eigen::MatrixXd(const Eigen::MatrixXd&, const Eigen::MatrixXd&)>f_boxplus2)
+                         std::function<Eigen::MatrixXd(const Eigen::MatrixXd&, const Eigen::MatrixXd&)>f_boxplus2,
+                         double step_size)
 {
   if (f_boxminus == nullptr)
   {
@@ -48,7 +49,7 @@ Eigen::MatrixXd calc_jac(std::function<Eigen::MatrixXd(const Eigen::MatrixXd&)>
   I.resize(cols, cols);
   I.setZero(cols, cols);
   for (int i = 0; i < cols; i++)
-    I(i,i) = 1e-8;
+    I(i,i) = step_size;
 
   Eigen::MatrixXd JFD;
   JFD.setZero(rows, cols);
@@ -61,7 +62,7 @@ Eigen::MatrixXd calc_jac(std::function<Eigen::MatrixXd(const Eigen::MatrixXd&)>
     Eigen::MatrixXd ym = fun(xm);
     Eigen::MatrixXd dy = f_boxminus2(yp,ym);
 
-    JFD.col(i) = dy/(2*1e-8);
+    JFD.col(i) = dy/(2*step_size);
   }
   return JFD;
 }
diff --git a/src/test_autodiff.cpp b/src/test_autodiff.cpp
@@ -7,8 +7,6 @@
 
 #include "test_common.h"
 
-using namespace quat;
-
 TEST (CostFunctorAutoDiff, CompileNormalVecs)
 {
     typedef Matrix<double, 2, 1> OT;
@@ -192,8 +190,8 @@ struct QuatPlus
         typedef typename Derived1::Scalar T;
         typedef Matrix<T, 4, 1> Vec4;
         typedef Matrix<T, 3, 1> Vec3;
-        Quat<T> qp(_qp);
-        Quat<T> q(_q);
+        quat::Quat<T> qp(_qp);
+        quat::Quat<T> q(_q);
         Vec4 qpdebug= _qp;
         Vec4 qdebug= _q;
         Vec3 ddebug = delta;
@@ -206,9 +204,9 @@ typedef CostFunctorAutoDiff<double, QuatPlus, 4, 4, 3> QuatParam;
 
 TEST (Autodiff, Quaternion)
 {
-    Quatd q1 = Quatd::from_euler(10.0*M_PI/180.0, -45.0*M_PI/180.0, 20.0*M_PI/180.0);
+    quat::Quatd q1 = quat::Quatd::from_euler(10.0*M_PI/180.0, -45.0*M_PI/180.0, 20.0*M_PI/180.0);
     Vector3d delta = (Vector3d() << 0.01, 0.02, -0.01).finished();
-    Quatd qp;
+    quat::Quatd qp;
     Matrix<double, 4, 4> dqp_dq1;
     Matrix<double, 4, 3> dqp_ddelta;
 

diff --git a/src/test_mhe1d.cpp b/src/test_mhe1d.cpp
@@ -7,7 +7,7 @@
 #include "utils/logger.h"
 
 
-TEST (nano_mhe_1d, init)
+TEST (nano_mhe_1d, compile)
 {
     MHE_1D<double, 5> mhe;
 }