debugging. done after nom_prop. Next step imu trans mat

include/eskf_odom_alg_node.h

@@ -108,6 +108,9 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
     double t_ini_px4_;
     double t_ini_llstatus_;
 
+    // double time_fix_;
+    // double dt_;
+
     double max_freq_;
 
     // vector<bool> ll_flying_; // To store bool due to different Hz

src/eskf_odom_alg_node.cpp

@@ -44,9 +44,13 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
   
 #ifdef READ_FROM_FILE
 
-  this->imu_idx_ = 0;
-  this->px4_idx_ = 0;
-  this->llstatus_idx_ = 0;
+  // this->time_fix = 0.0;
+  // this->dt_=dt = 0.00999999;
+
+
+  this->imu_idx_ = 0.0;
+  this->px4_idx_ = 0.0;
+  this->llstatus_idx_ = 0.0;
 
   this->a_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/linear_acceleration.txt");
   this->w_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/angular_velocity.txt");
@@ -57,8 +61,9 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
   this->t_ini_px4_ = this->px4_[0][0];
   this->t_ini_llstatus_ = this->llstatus_[0][0];
 
-
   // The first IMU value is not used (integration purposes)
+
+
   double t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
 
   double ax = this->a_[this->imu_idx_][1];
@@ -79,7 +84,6 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
 
   ++this->imu_idx_;
  
-
   // Find message with max frequency
 
   double imu_freq = this->a_.size()/((this->a_[this->a_.size()-1][0]-this->t_ini_imu_)*1e-9);
@@ -244,8 +248,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
   // Set Quadrotor State
 
 #ifdef READ_FROM_FILE
-    // ROS_DEBUG("gdist: %f, vx: %f, vy: %f", this->px4_[this->imu_idx_][4],this->px4_[this->imu_idx_][7],this->px4_[this->imu_idx_][8]);
-    // ROS_DEBUG("flying: %f", this->llstatus_[this->imu_idx_][10]);
 
     ROS_DEBUG("IMU process");  
 
@@ -272,7 +274,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
 
     double next_t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
 
-
     // PX4 process ***************
     double t_px4 = (this->px4_[this->px4_idx_][0]-t_ini_px4_)*1e-9;