2 frequencies for lin and ang quantities

test/gtest_imu_mocap_fusion.cpp

@@ -31,7 +31,6 @@ using std::endl;
 
 const Vector3d zero3 = Vector3d::Zero();
 const double dt = 0.0001;
-const Vector3d freq = Vector3d::Ones(); 
 
 
 class ImuMocapFusion_Test : public testing::Test
@@ -57,6 +56,13 @@ class ImuMocapFusion_Test : public testing::Test
         // simulate trajectory
         //////////////////////
 
+        Vector3d freq_lin; freq_lin << 0.5, 1, 1.5; 
+        Vector3d freq_ang; freq_ang << 1.5, 1, 0.5; 
+
+        // P = sin(freq*t)
+        // V = freq*cos(freq*t)
+        // A = -freq^2*cos(freq*t)
+
         // biases and extrinsics
         ba_ = Vector3d::Zero();
         bw_ = Vector3d::Zero();
@@ -66,10 +72,10 @@ class ImuMocapFusion_Test : public testing::Test
         // initialize state
         Vector3d w_p_wb = Vector3d::Zero();
         Quaterniond w_q_b = Quaterniond::Identity();
-        Vector3d w_v_wb = freq.array(); // *(1,1,1)
+        Vector3d w_v_wb = freq_lin.array(); // *(1,1,1)
 
         // Problem and solver_
-        problem_ = Problem::create("PO", 3);
+        problem_ = Problem::create("POV", 3);
         solver_ = std::make_shared<SolverCeres>(problem_);
         solver_->getSolverOptions().max_num_iterations = 500;
 
@@ -106,22 +112,22 @@ class ImuMocapFusion_Test : public testing::Test
         sensor_imu_->fixIntrinsics();
 
         // Store necessary values in vectors
-        std::vector<Vector3d> w_p_wb_vec; 
-        std::vector<Vector3d> w_p_wb_sin_vec; 
-        std::vector<Quaterniond> w_q_b_vec; 
-        std::vector<Vector3d> w_v_wb_vec;
-
-        w_p_wb_vec.push_back(w_p_wb);
-        w_p_wb_sin_vec.push_back(w_p_wb);
-        w_q_b_vec.push_back(w_q_b);
-        w_v_wb_vec.push_back(w_v_wb);
+        // std::vector<Vector3d> w_p_wb_vec; 
+        // std::vector<Vector3d> w_p_wb_sin_vec; 
+        // std::vector<Quaterniond> w_q_b_vec; 
+        // std::vector<Vector3d> w_v_wb_vec;
+
+        // w_p_wb_vec.push_back(w_p_wb);
+        // w_p_wb_sin_vec.push_back(w_p_wb);
+        // w_q_b_vec.push_back(w_q_b);
+        // w_v_wb_vec.push_back(w_v_wb);
         
         int traj_size = 10001;
         for (int i=0; i < traj_size; i++){
             double t = i*dt;
-            Vector3d w_p_wb_sin =                    Eigen::sin((freq*t).array());
-            Vector3d w_omg_b = freq.array()*         Eigen::cos((freq*t).array());
-            Vector3d w_a_wb = -freq.array().square()*Eigen::sin((freq*t).array());
+            Vector3d w_p_wb_sin =                    Eigen::sin((freq_lin*t).array());  // just for comparison with the integration
+            Vector3d w_omg_b = freq_ang.array()*         Eigen::cos((freq_ang*t).array());
+            Vector3d w_a_wb = -freq_lin.array().square()*Eigen::sin((freq_lin*t).array());
 
             // integrate simulated traj
             w_p_wb = w_p_wb + w_v_wb*dt + 0.5*w_a_wb*dt*dt;
@@ -145,9 +151,9 @@ class ImuMocapFusion_Test : public testing::Test
             CP->process();
 
             if ((i%1000) == 0){
-                std::cout << "" << std::endl;
-                std::cout << w_p_wb.transpose() << std::endl;
-                std::cout << w_p_wb_sin.transpose() << std::endl;
+                std::cout << t << std::endl;
+                std::cout << "w_p_wb:     " << w_p_wb.transpose() << std::endl;
+                std::cout << "w_p_wb_sin: " << w_p_wb_sin.transpose() << std::endl;
             }
         }