TAG: Debugged with read_from_file! Working with computation using global and local frame method

include/eskf_odom_alg.h

@@ -172,8 +172,9 @@ class EskfOdomAlgorithm
     * Input:
     *   t: Time stamp.
     *   val: Sensor readings: val = [pz;vx;vy].
+    *   flow: Optical flow raw reading flow = [flow_x,flow_y].
     */
-    void set_px4_reading(const double& t, const Vector3d val);
+    void set_px4_reading(const double& t, const Vector3d& val, const Vector2d& flow);
 
     /**
     * \brief Nominal State Propagation

launch/params.yaml

@@ -12,7 +12,7 @@ imu_a_std: [0.2,0.2,0.2]
 imu_w_std: [0.005,0.005,0.005]
 imu_ab_std: [0.0,0.0,0.0]
 imu_wb_std: [0.0,0.0,0.0]
-imu_method: discrete #Sets the data time nature (continous or discrete).
+imu_method: discrete #Sets the data time nature (continous: simulated IMU or discrete: IMU sensor).
 
 # PX4 STD values
 px4_std_landed: [0.1,0.001,0.001]

src/eskf_odom_alg.cpp

@@ -44,9 +44,9 @@ void EskfOdomAlgorithm::set_imu_reading(const double& t, const Vector3d& a, cons
 	this->filter_.set_imu_reading(t,a,w);
 }
 
-void EskfOdomAlgorithm::set_px4_reading(const double& t, const Vector3d val)
+void EskfOdomAlgorithm::set_px4_reading(const double& t, const Vector3d& val, const Vector2d& flow)
 {
-  	this->filter_.set_px4_reading(t,val);
+  	this->filter_.set_px4_reading(t,val,flow);
 }
 
 void EskfOdomAlgorithm::prop_nominal(void)

src/eskf_odom_alg_node.cpp

@@ -246,6 +246,9 @@ void EskfOdomAlgNode::mainNodeThread(void)
   // [publish messages]
 
   // Set Quadrotor State
+  this->alg_.lock();  // protect algorithm
+  this->alg_.set_obs_phase(this->flying_,this->gnd_dist_); // Set observation phase (Landed, toff, Flying or Landing)
+  this->alg_.unlock(); 
 
 #ifdef READ_FROM_FILE
 
@@ -284,12 +287,17 @@ void EskfOdomAlgNode::mainNodeThread(void)
       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
+      double flowx = this->px4_[this->px4_idx_][5];
+      double flowy = -this->px4_[this->px4_idx_][6]; // axis change according to simulation
 
       Vector3d val;
       val << this->gnd_dist_,vx,vy;
 
+      Vector2d flow;
+      flow << flowx,flowy;
+
       this->alg_.lock(); 
-      this->alg_.set_px4_reading(t_px4,val); // Set values into filter object
+      this->alg_.set_px4_reading(t_px4,val,flow); // Set values into filter object
       this->alg_.obs_error_and_up_nominal(); // Error observation and Nominal-State Update
   
       this->alg_.unlock();
@@ -311,10 +319,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
 
 #endif
 
-  this->alg_.lock();  // protect algorithm
-  this->alg_.set_obs_phase(this->flying_,this->gnd_dist_); // Set observation phase (Landed, toff, Flying or Landing)
-  this->alg_.unlock(); 
-
 #ifndef READ_FROM_FILE 
 
   if (this->new_imu_)