debuged after prop_nom using read_from_file

include/eskf_odom_alg_node.h

@@ -100,12 +100,17 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
 
 #ifdef READ_FROM_FILE
 
-    int count_;
+    int imu_idx_;
+    int px4_idx_;
+    int llstatus_idx_;
+
     double t_ini_imu_;
     double t_ini_px4_;
     double t_ini_llstatus_;
 
-    vector<bool> ll_flying_; // To store bool due to different Hz
+    double max_freq_;
+
+    // vector<bool> ll_flying_; // To store bool due to different Hz
 
     vector <vector<double>> a_;
     vector <vector<double>> w_;

src/eskf_odom_alg_node.cpp

@@ -13,15 +13,15 @@ using namespace Eigen;
 EskfOdomAlgNode::EskfOdomAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>()
 {
-  //init class attributes if necessary
-  this->loop_rate_ = 100;//in [Hz]
+  this->flying_ = false;
+  this->gnd_dist_ = 0.0; // a little bit more than the minimum distance PX4 can detect (0.3m)
 
 #ifndef READ_FROM_FILE
 
+  this->loop_rate_ = 100;//in [Hz]
+
   // Initialize vars
-  this->flying_ = false;
-  this->gnd_dist_ = 0.0; // a little bit more than the minimum distance PX4 can detect (0.3m)
-  this->is_first_ = true;
+  this->is_first_ = true;  
   this->new_imu_ = false;
   this->new_px4_ = false;
 
@@ -44,7 +44,9 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
   
 #ifdef READ_FROM_FILE
 
-  this->count_ = 0;
+  this->imu_idx_ = 0;
+  this->px4_idx_ = 0;
+  this->llstatus_idx_ = 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");
@@ -56,42 +58,37 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
   this->t_ini_llstatus_ = this->llstatus_[0][0];
 
 
-  // Done to solve ll_status different frew r.t. px4 ____________TODO FIX this______
-  double t_fly;
-  double t_landed;
-  bool found = false;
-  for (int ii = 0; ii < this->llstatus_.size(); ++ii)
-  {
-    if (this->llstatus_[ii][10] == 1.0 && !found)
-    {
-      found = true;
-      t_fly = (this->llstatus_[ii][0]-this->t_ini_llstatus_)*1e-9;
-    }
-    if (this->llstatus_[ii][10] == 0.0 && found)
-    {
-      t_landed = (this->llstatus_[ii][0]-this->t_ini_llstatus_)*1e-9;
-    }
-  }
+  // The first IMU value is not used (integration purposes)
+  double t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
 
-  this->ll_flying_.resize(this->px4_.size());
+  double ax = this->a_[this->imu_idx_][1];
+  double ay = -this->a_[this->imu_idx_][2]; // axis change according to simulation
+  double az = -this->a_[this->imu_idx_][3]; // axis change according to simulation
+  double wx = this->w_[this->imu_idx_][1];
+  double wy = -this->w_[this->imu_idx_][2]; // axis change according to simulation
+  double wz = -this->w_[this->imu_idx_][3];// axis change according to simulation
 
-  for (int ii = 0; ii < this->px4_.size(); ++ii)
-  {
-    double tpx4 = (this->px4_[ii][0]-t_ini_px4_)*1e-9;
-    if((tpx4 < t_fly) || (tpx4 > t_landed))
-      this->ll_flying_[ii] = false;
-    else
-      this->ll_flying_[ii] = true;   
-  }
-  //________________________________
+  Vector3d a;
+  a << ax,ay,az;
+  Vector3d w;
+  w << wx,wy,wz;
+
+  this->alg_.lock();
+  this->alg_.set_imu_reading(t_imu,a,w); // Set values into filter object
+  this->alg_.unlock(); 
+
+  ++this->imu_idx_;
+ 
+
+  // Find message with max frequency
 
-  // cout << "t fly: " << t_fly << ", t land: " << t_landed << endl;
+  double imu_freq = this->a_.size()/((this->a_[this->a_.size()-1][0]-this->t_ini_imu_)*1e-9);
+  double px4_freq = this->px4_.size()/((this->px4_[this->px4_.size()-1][0]-this->t_ini_px4_)*1e-9);
+  double llstatus_freq = this->llstatus_.size()/((this->llstatus_[this->llstatus_.size()-1][0]-this->t_ini_llstatus_)*1e-9);
 
-  // double imu_freq = (this->a_[this->a_.size()-1][0]-this->t_ini_imu_)*1e-9/this->a_.size();
-  // double px4_freq = (this->px4_[this->px4_.size()-1][0]-this->t_ini_px4_)*1e-9/this->px4_.size();
-  // double llstatus_freq = (this->llstatus_[this->llstatus_.size()-1][0]-this->t_ini_llstatus_)*1e-9/this->llstatus_.size();
+  this->max_freq_ = max(max(imu_freq,px4_freq),llstatus_freq);
 
-  // cout << "imu: " << imu_freq << ", px4: " << px4_freq << ", llstatus: " << llstatus_freq << endl;
+  this->loop_rate_ = this->max_freq_;//in [Hz]
 
 #endif
 
@@ -247,20 +244,70 @@ void EskfOdomAlgNode::mainNodeThread(void)
   // Set Quadrotor State
 
 #ifdef READ_FROM_FILE
-    ROS_DEBUG("time: %f",this->a_[this->count_][0]*1e-9);
-    ROS_DEBUG("ax: %f, ay: %f, az: %f", this->a_[this->count_][1],this->a_[this->count_][2],this->a_[this->count_][3]);
-    ROS_DEBUG("wx: %f, wy: %f, wz: %f", this->w_[this->count_][1],this->w_[this->count_][2],this->w_[this->count_][3]);
-    ROS_DEBUG("gdist: %f, vx: %f, vy: %f", this->px4_[this->count_][4],this->px4_[this->count_][7],this->px4_[this->count_][8]);
-    ROS_DEBUG("flying: %f", this->llstatus_[this->count_][10]);
+    // 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");  
+
+    double t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
+
+    double ax = this->a_[this->imu_idx_][1];
+    double ay = -this->a_[this->imu_idx_][2]; // axis change according to simulation
+    double az = -this->a_[this->imu_idx_][3]; // axis change according to simulation
+    double wx = this->w_[this->imu_idx_][1];
+    double wy = -this->w_[this->imu_idx_][2]; // axis change according to simulation
+    double wz = -this->w_[this->imu_idx_][3];// axis change according to simulation
+
+    Vector3d a;
+    a << ax,ay,az;
+    Vector3d w;
+    w << wx,wy,wz;
+
+    this->alg_.lock();
+    this->alg_.set_imu_reading(t_imu,a,w); // Set values into filter object
+    this->alg_.prop_nominal(); // Propagate Nominal-State 
+    this->alg_.unlock(); 
+
+    ++this->imu_idx_;
+
+    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;
+
+    if (this->px4_idx_ < this->px4_.size() && t_px4 < next_t_imu)
+    {
+      ROS_DEBUG("process PX4");
+
+      this->gnd_dist_ = this->px4_[this->px4_idx_][4];
+      double vx = this->px4_[this->px4_idx_][7];
+      double vy = -this->px4_[this->px4_idx_][8]; // axis change according to simulation
+
+      Vector3d val;
+      val << this->gnd_dist_,vx,vy;
 
-    this->gnd_dist_ = this->px4_[this->count_][4];
+      this->alg_.lock(); 
+      this->alg_.set_px4_reading(t_px4,val); // Set values into filter object
+      this->alg_.obs_error_and_up_nominal(); // Error observation and Nominal-State Update
+  
+      this->alg_.unlock();
+
+      ++this->px4_idx_;
+    }
 
-    this->flying_ = this->ll_flying_[this->count_];
+    // LLStatus process ****************
+    double t_llstatus = (this->llstatus_[this->llstatus_idx_][0]-t_ini_llstatus_)*1e-9;
 
-    // double run_t = (this->a_[this->count_][0]-this->t_ini_imu_)*1e-9;
-    // cout << "Running time: " << run_t << ", g_dist: " << this->gnd_dist_ << ", flying: " << this->llstatus_[this->count_][10] << endl;
+    if (this->llstatus_idx_ < this->llstatus_.size() && t_llstatus < next_t_imu)
+    {
+      ROS_DEBUG("process ll_status");
+
+      this->flying_ = this->llstatus_[this->llstatus_idx_][10];
+
+      ++this->llstatus_idx_;
+    }
 
-    ++this->count_;
 #endif
 
   this->alg_.lock();  // protect algorithm
@@ -290,6 +337,9 @@ void EskfOdomAlgNode::mainNodeThread(void)
     this->alg_.unlock(); 
   }
 
+#endif
+
+  // Get state and publish TF **********************
   this->alg_.lock();  // protect algorithm
   VectorXd state = this->alg_.get_x_state(); // get state
   this->alg_.unlock(); 
@@ -301,11 +351,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
   tf::Quaternion q(state(7),state(8),state(9),state(6)); //TF:[qx,qy,qz,qw] Filter:[qw,qx,qy,qz]
   transform.setRotation(q);
   br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
-
-  printf("MAIN %2.10f \n",ros::Time::now().toSec());
-
-#endif
-
 }
 
 #ifndef READ_FROM_FILE